def match(self, x, y, w, h, timeout=15):
def rect_in_rect(cap):
(xc1, yc1), im = cap
hc, wc = im.shape[:2]
xr1, yr1 = x, y
xr2, yr2 = x + w, y + h
xc2, yc2 = xc1 + wc, yc1 + hc
return (xc1 <= xr1 <= xr2 <= xc2 and yc1 <= yr1 <= yr2 <= yc2)
if self.pos is None:
self.capture(x, y)
matches = [cap for cap in self.caps if rect_in_rect(cap)]
# !!! alpha handicap !!! #
return matches
start = time.time()
while not len(matches) and (time.time() - start) < timeout:
# calculate suitable trajectory to target
ww, hh = self.dims
ori = Vector(*self.pos)
dst = Vector(x, y) - Vector(ww / 2, hh / 2)
vec = dst - ori
δ = vec.normalise() * (ww / 2)
xx, yy = ori + (vec if vec.r() < δ.r() else δ)
print(ori, dst, (x, y), δ)
new = self.capture(xx, yy)
if new is None:
break
elif rect_in_rect(new):
matches.append(new)
return matches
def draw(self, canvas):
end = Vector(self.direction.x * 1000, self.direction.y * 1000)
if self.hit is not None:
end = Vector(self.hit.x, self.hit.y)
line = Line(self.start, end)
line.set_color(self.color)
line.draw(canvas)
def polygon(self):
"""return a polygon object"""
a = self.origin
b = a + Vector.from_polar(self.width, self.angle)
c = b + Vector.from_polar(self.height, self.angle + 0.5*pi)
d = a + Vector.from_polar(self.height, self.angle + 0.5*pi)
return Polygon(a, b, c, d)
def polygon(self):
"""return a polygon object"""
a = self.origin
b = a + Vector.from_polar(self.width, self.angle)
c = b + Vector.from_polar(self.height, self.angle + 0.5 * pi)
d = a + Vector.from_polar(self.height, self.angle + 0.5 * pi)
return Polygon(a, b, c, d)
def cast_rays(self, polylines):
self.rays = []
angle = 2 * math.pi / self.num_rays
direction = Vector(0, -1)
for i in range(self.num_rays - 1):
x = direction.x * math.cos(angle) - direction.y * math.sin(angle)
y = direction.x * math.sin(angle) + direction.y * math.cos(angle)
direction = Vector(x, y)
ray = Ray(self.position, direction)
self.rays.append(ray)
ray.cast(polylines)
def inbounds(self, im):
boundc = cv2.perspectiveTransform(im['bounds'], im['h'])
bounds = HomographyTest.cv2vec(boundc)
xmin, xmax = sorted([x for x,_ in bounds])[1:3]
ymin, ymax = sorted([y for _,y in bounds])[1:3]
inbs = [Vector(xmin, ymin), Vector(xmax, ymin),
Vector(xmax, ymax), Vector(xmin, ymax)]
poly = Polygon(*bounds)
if not all(poly.contains(inb) for inb in inbs):
inbs = None
return inbs, (int(xmax-xmin), int(ymax-ymin))
def mutate(self):
childparams, childpoly = self.params, self.poly
if random.random() < 1 / 2:
childparams = [
round(random.gauss(1, 0.1) * p, 4) for p in self.params
]
else:
if len(self.poly.exterior.coords) < 5 or (
len(self.poly.exterior.coords) < 15
and random.random() < 1 / 2): #add a point
#find the edge of the polygon nearest the new point and add the point there
mindist = float('inf')
pt_new = Vector(
[round(random.uniform(-1 / 3, 1), 3) for i in range(2)])
for i in range(-1, len(self.poly.exterior.coords) - 2):
pt_left, pt_center, pt_right = [
Vector(self.poly.exterior.coords[1:][i + k])
for k in (-1, 0, 1)
]
v_left, v_right, v_new = [
pt - pt_center for pt in (pt_left, pt_right, pt_new)
]
if (v_left.unit() + v_right.unit()).norm() > 10**(
-4
): #if the vectors are pointed nearly precisely opposite each other, ignore them
if v_new.dot(v_right) > 0 and (pt_new - pt_right).dot(
pt_center - pt_right) > 0:
dist = v_new.reject(v_right).norm()
edge = i + 1
else:
dist = v_new.norm()
edge = i + bool(
v_new.reject(v_left.unit() +
v_right.unit()).dot(v_right) > 0)
if dist < mindist:
mindist = dist
nearedge = edge
childpoly = Polygon(self.poly.exterior.coords[:nearedge + 1] +
[pt_new.val] +
self.poly.exterior.coords[nearedge + 1:-1])
else: #remove a point
pt = random.randint(0, len(self.poly.exterior.coords) - 2)
childpoly = Polygon(self.poly.exterior.coords[:pt] +
self.poly.exterior.coords[pt + 1:-1])
while not (childpoly.is_valid and childpoly.is_simple):
pt = random.randint(0, len(self.poly.exterior.coords) - 2)
childpoly = Polygon(self.poly.exterior.coords[:pt] +
self.poly.exterior.coords[pt + 1:-1])
return self.__class__(childparams, childpoly, self.wls)
def generate_rects(self):
"""generate a list of rectangles of the cameras size covering
the given polygon; the rectangles will intersect by the
specified stitching amount
TODO: eliminate any rectangles which are not in the given
polygon"""
_, camera = self.backend
prec = camera.precision()
stitch = self.min_stitch * prec
width, height = camera.resolution()
width *= prec
height *= prec
wid = width - 2 * stitch
hei = height - 2 * stitch
bounding = self.polygon.rectangle(camera.orientation())
o = bounding.origin
cols = int(ceil(bounding.width / wid))
rows = int(ceil(bounding.height / hei))
return [
i for s in [[(
(i, j),
Rectangle(o + Vector(j * wid - stitch, i * hei -
stitch), 0, width, height), None)
for j in (reversed(range(cols)) if i %
2 else range(cols))]
for i in range(rows)] for i in s
]
def calsu(self, contact: vec.Vector, normal: vec.Vector):
#distance from center of mass to the collision contact
rP = contact - self.position
#vertex velocity before the collision
velocity = self.velocity + (vec.Vector(0, 0, self.angular).cross(rP))
e = 0.8 #maybe this could be parameter
impulse = -(e + 1) * (velocity.dot(normal) / ( 1/self.mass + (rP.cross(normal).magnitude()**2)/self.inertia ))
self.velocity += normal.scale(impulse/self.mass)
self.angular += rP.cross(normal).scale(impulse/self.inertia).z
def status(self):
"""return information on:
* calibration
* idle?
* position
* attached
"""
return Status(ready=False,
idle=False,
calibrated=False,
position=Vector(0, 0),
attached=False)
class Car:
def __init__(self, position):
self.position = Vector(position.x, position.y)
self.speed = 20
self.num_rays = 20
self.rays = []
def set_num_rays(self, amount):
self.num_rays = amount
def move(self, direction):
direction = direction.normalized()
self.position.x += direction.x * self.speed
self.position.y += direction.y * self.speed
def cast_rays(self, polylines):
self.rays = []
angle = 2 * math.pi / self.num_rays
direction = Vector(0, -1)
for i in range(self.num_rays - 1):
x = direction.x * math.cos(angle) - direction.y * math.sin(angle)
y = direction.x * math.sin(angle) + direction.y * math.cos(angle)
direction = Vector(x, y)
ray = Ray(self.position, direction)
self.rays.append(ray)
ray.cast(polylines)
def draw(self, canvas):
self.position.set_radius(5)
self.position.set_color("#FF0000")
self.position.draw(canvas)
for ray in self.rays:
ray.draw(canvas)
canvas.update_idletasks()
canvas.update()
def read_map(path):
polylines = []
with open(path) as json_file:
json_data = json.load(json_file)
for line in json_data['polylines'].split('\n'):
if line == '':
break
pl = Polyline([])
coords_str = line.split(sep=':')
coords_n = []
for c in coords_str:
xy = c.split(sep=',')
coords_n.append([int(xy[0]), int(xy[1])])
for i in range(len(coords_n) - 1):
pl.add_line(
Line(Vector(coords_n[i][0], coords_n[i][1]),
Vector(coords_n[i + 1][0], coords_n[i + 1][1])))
polylines.append(pl)
return Map(Car(Vector(json_data['start'][0], json_data['start'][1])),
polylines)
def status(self):
"""return information on:
* ready?
* calibration
* idle?
* position
* what head is attached
"""
return Status(ready=False,
idle=False,
calibrated=False,
position=Vector(0, 0),
attachment=None)
def cast(self, polylines):
min_dist = float('inf')
self.hit = None
for pl in polylines:
for line in pl.lines:
x1 = line.start.x
y1 = line.start.y
x2 = line.end.x
y2 = line.end.y
x3 = self.start.x
y3 = self.start.y
x4 = self.direction.x * 1000
y4 = self.direction.y * 1000
den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
if den == 0:
continue
t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / den
u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / den
if 0 < t < 1 and u > 0:
point = Vector(0, 0)
point.x = x1 + t * (x2 - x1)
point.y = y1 + t * (y2 - y1)
if self.hit is None:
self.hit = point
min_dist = self.start.dist(point)
else:
dist = self.start.dist(point)
if dist < min_dist:
min_dist = dist
self.hit = point
return self.hit
def rectangle(self, angle=0):
"""return the smallest bounding rectangle
angle - the orientation of the rectangle,
given in anticlockwise radians"""
points = [point.rotate(angle) for point in self.points]
x_max = max(x for x, _ in points)
x_min = min(x for x, _ in points)
y_max = max(y for _, y in points)
y_min = min(y for _, y in points)
origin = Vector(x_min, y_min).rotate(-angle)
width = x_max - x_min
height = y_max - y_min
return Rectangle(origin, -angle, width, height)
def render_dir_deri():
vector_input = request.args.get('vector')
vec_list = filter(None, re.split(r"[\s,]", vector_input))
to_real_vector = []
for vec in vec_list:
to_real_vector.append(float(vec))
dir_deri = None
try:
if len(vec_list) > 0 and len(vec_list) == len(f.getVariables()):
dir_deri = f.getDirectionalDerivative(
Vector(to_real_vector)).toString()
elif len(vec_list) > 0 and len(vec_list) != len(f.getVariables()):
raise InvalidVectorInput()
except InvalidVectorInput as e:
dir_deri = str(e)
return jsonify(dir_deri=dir_deri)
#!/usr/bin/env
from hw.shapeoko import Shapeoko, Axes
from lib.vector import Vector
from math import pi
import time
import glob
if __name__ == '__main__':
"""very simple circle drawing test (should instead use
G2/G3 to draw arcs); connects to the first serial
port it finds, calibrates it and then moves in a
full circle with 1deg resolution"""
com = glob.glob('/dev/ttyACM*')[0]
print(com)
stage = Shapeoko(com)
time.sleep(2)
stage.home([Axes.X, Axes.Y, Axes.Z])
origin = Vector(100, 100)
radius = 80
d2r = lambda deg: deg * pi / 180
move = lambda v: stage.move([v.x(), v.y(), None])
move(origin)
for deg in range(0, 360, 1):
point = origin + Vector.from_polar(radius, d2r(deg))
move(point)
stage.close()
def cv2vec(cv):
return [Vector(*b[0]) for b in cv]
from hw.shapeoko import Shapeoko, Axes
from lib.vector import Vector
from math import pi
import time
import glob
if __name__ == '__main__':
"""very simple circle drawing test (should instead use
G2/G3 to draw arcs); connects to the first serial
port it finds, calibrates it and then moves in a
full circle with 1deg resolution"""
com = glob.glob('/dev/ttyACM*')[0]
print(com)
stage = Shapeoko(com)
time.sleep(2)
stage.home([Axes.X, Axes.Y, Axes.Z])
origin = Vector(100,100)
radius = 80
d2r = lambda deg: deg * pi / 180
move = lambda v: stage.move([v.x(), v.y(), None])
move(origin)
for deg in range(0, 360, 1):
point = origin + Vector.from_polar(radius, d2r(deg))
move(point)
stage.close()
def bounds(self):
"""return bounding polygon, ideally a rectangle"""
from .canvas import Rectangle
return Rectangle(Vector(0, 0), Vector(1, 0), 1, 1)
coords_n = []
for c in coords_str:
xy = c.split(sep=',')
coords_n.append([int(xy[0]), int(xy[1])])
for i in range(len(coords_n) - 1):
pl.add_line(
Line(Vector(coords_n[i][0], coords_n[i][1]),
Vector(coords_n[i + 1][0], coords_n[i + 1][1])))
polylines.append(pl)
return Map(Car(Vector(json_data['start'][0], json_data['start'][1])),
polylines)
if __name__ == '__main__':
setup()
map = read_map('maps/map_name.json')
map.car.set_num_rays(50)
map.car.cast_rays(map.polylines)
map.draw(canvas)
while True:
canvas.delete('all')
map.car.move(
Vector(rd.random() - rd.random(),
rd.random() - rd.random()))
map.car.cast_rays(map.polylines)
map.draw(canvas)
time.sleep(.5)
continue
shapeoko = XY(dev, bounds)
break
print(shapeoko.status())
ε = shapeoko.register(XY.Empty())
ν = shapeoko.register(Needle())
shapeoko.head = ε
shapeoko.head = ν
ws = Workspace(shapeoko)
ws.optimise_queue(False)
up = 15
down = 0
done = threading.Event()
ws.enqueue(ν, [Vector(0, 0)], lambda _: done.set(), {
'down': up,
'up': up
}, {})
done.wait()
input("Ready? ")
while True:
ws.enqueue(ν, [Vector(0, 0)], lambda _: None, {
'down': down,
'up': up
}, {})
ws.enqueue(ν, [Vector(0.05, 0.05)], lambda _: None, {
'down': down,
'up': up
}, {})
def centroid(self):
"""return the centroid in polygon coordinates"""
origin = Vector(0, 0)
return sum(self.points, origin) / len(self.points)
def from_bench(self, point):
"""convert device coordinates to pixels"""
x, y = self.rectangle.from_bench(point)
return Vector(int(x * self.data.cols / self.rectangle.width),
int(y * self.data.rows / self.rectangle.height))
def to_bench(self, coord):
"""convert pixels to device coordinates"""
x, y = coord
return self.rectangle.to_bench(
Vector(x * self.rectangle.width / self.data.cols,
y * self.rectangle.height / self.data.rows))
def __init__(self, position):
self.position = Vector(position.x, position.y)
self.speed = 20
self.num_rays = 20
self.rays = []
dev = devs[int(choice)]
except:
continue
shapeoko = XY(dev, bounds)
break
print(shapeoko.status())
ε = shapeoko.register(XY.Empty())
shapeoko.head = ε
ws = Workspace(shapeoko)
# test queuing
ws.optimise_queue(True)
ws.pause()
imcb = lambda im:im.show()
ws.enqueue(ε, [Vector(0,0)], lambda _:print(0), {}, {})
ws.enqueue(ε, [Vector(0.05,0.05)], lambda _:print(1), {}, {})
ws.enqueue(ε, [Vector(0.06,0.05)], lambda _:print(2), {}, {})
ws.enqueue(ε, [Vector(0.05,0.04)], lambda _:print(3), {}, {})
ws.enqueue(ε, [Vector(0.08,0.08)], lambda _:print(4), {}, {})
ws.enqueue(ε, [Vector(0.05,0.05)], lambda _:print(5), {}, {})
ws.enqueue(ε, [Vector(0.02,0.05)], lambda _:print(6), {}, {})
ws.play()
time.sleep(2)
ws.wait()
num = input("How many random coordinates? ")
random.seed()
uni = random.uniform
rcoord = lambda:Vector(uni(0,0.1), uni(0,0.1))
def getNormal(self, edge: vec.Vector) -> vec.Vector:
normal = vec.Vector(-edge.y, edge.x, 0).normalize()
if edge.cross(normal).z > 0:
return normal.scale(-1.0)
return normal
continue
shapeoko = XY(dev, bounds)
break
print(shapeoko.status())
ε = shapeoko.register(XY.Empty())
ν = shapeoko.register(Needle())
shapeoko.head = ε
shapeoko.head = ν
ws = Workspace(shapeoko)
ws.optimise_queue(False)
down = 0
up = 15
done = threading.Event()
ws.enqueue(ν, [Vector(0, 0)], lambda _: done.set(), {
'down': up,
'up': up
}, {})
done.wait()
input("Ready? ")
while True:
ws.enqueue(ν, [
Vector(0, 0),
Vector(0, 0.05),
Vector(0.05, 0.05),
Vector(0.05, 0)
], lambda _: None, {
'down': down,
'up': down