def read(file_nm, no_strips):
"""Reads a file with on the first uncommented line a bbox
(4 numbers separated by a space) and subsequently 0 or more lines with
points (2 numbers separated by a space) into a Strip Structure.
If no valid box is found in the input file, it returns None.
Otherwise a StripStructure with 0 or more points is returned.
Returns - None or a StripStructure instance
"""
with open(file_nm, 'r') as fh:
lines_without_comment = [
c for c in fh.readlines() if not c.startswith('#')
]
bbox = (lines_without_comment[0].strip().split())
if len(bbox) == 4: # there are exactly 2 coordinates in bbox
if bbox[2] > bbox[0] and bbox[3] > bbox[
1]: # ll and ul are satisfied
structure = StripStructure(
Rectangle(Point(bbox[0], bbox[1]),
Point(bbox[2], bbox[3])), no_strips)
points_list = [i.split() for i in lines_without_comment[1:]]
for pt in points_list:
structure.append_point(Point(pt[0], pt[1]))
return structure
else:
return None
else:
return None
def test_poincare_disk_model_line_through_diameter():
model = PoincareDiskModel(Point(0, 0), radius=1)
p1 = Point(1 / 6, 1 / 5)
p2 = Point(2 / 6, 2 / 5)
actual_line = model.line_through(p1, p2)
expected_line = Line(Point(1 / 6, 1 / 5), slope=6 / 5)
assert_that(expected_line).is_equal_to(actual_line)
def test_poincare_disk_model_line_through_hyperbolic():
model = PoincareDiskModel(Point(0, 0), radius=1)
p1 = Point(1 / 2, 1 / 2)
p2 = Point(1 / 2, -1 / 2)
actual_line = model.line_through(p1, p2)
expected_line = PoincareDiskLine(Point(3 / 2, 0), (5 / 4)**0.5)
assert_that(expected_line).is_equal_to(actual_line)
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 __get_point_distances(self, mytank, obstacle): d1 = get_distance(Point(obstacle[0]), mytank) d2 = get_distance(Point(obstacle[1]), mytank) d3 = get_distance(Point(obstacle[2]), mytank) d4 = get_distance(Point(obstacle[3]), mytank) return [d1, d2, d3, d4]
def random_robot_arm_problem(methods):
print 'Random Robot Arm Problem'
robot = RobotArm(Point(10, 10), [ \
(Point(-.5, 0), create_link(4, 1)), \
(Point(3.5, 0), create_link(2, 1)), \
(Point(1.5, 0), create_link(4, 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 = []
problem = Problem(20, 20, robot, obstacles, start, goal, cspace)
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 path1(self, r, h, stepOver, passes, direction=CW):
r0 = stepOver # milling arc radius
r1 = r0 / 3 # return radius
a = degrees(pi / 2 + 2 * atan2(r0 - r1, r0))
x = -r
y = 0
c0 = Point(x, y)
path = []
if direction == CW:
arc0 = Arc(c0, r0, 270, 90, direction=CW)
for i in range(passes):
path.append(arc0)
c1 = Point(x, arc0.p1.y + r1)
arc1 = Arc(c1, r1, a, 270, direction=CW)
path.append(arc1)
x += r0
c0 = Point(x, y)
arc0 = Arc(c0, r0, 270, a, direction=CW)
l = Line(arc1.p1, arc0.p0)
path.append(l)
else:
a = 360 - a
arc0 = Arc(c0, r0, 270, 90, direction=CCW)
for i in range(passes):
path.append(arc0)
c1 = Point(x, arc0.p1.y - r1)
arc1 = Arc(c1, r1, 90, a, direction=CCW)
path.append(arc1)
x += r0
c0 = Point(x, y)
arc0 = Arc(c0, r0, a, 90, direction=CCW)
l = Line(arc1.p1, arc0.p0)
path.append(l)
return path
def test_concentric_true(self):
# given the two circles
circle1 = Circle(Point(0, 0), 5)
circle2 = Circle(Point(0, 0), 10)
# then the circles are cocentric
self.assertTrue(circle1.concentric(circle2))
def jaco():
I = Interface()
I.connect()
# ------------------------Jaco---------------------------#
ROBOT_URDF_PATH = '../models/jaco_pkg/jaco_description/urdf/test_jaco.urdf'
BLOCK_URDF = "../ss-pybullet-master/models/drake/objects/block_for_pick_and_place_mid_size.urdf"
END_EFFECTOR_LINK = 'jaco_eef_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
TRAY_URDF = '../models/tray/tray.urdf'
robot = I.load_model(ROBOT_URDF_PATH)
redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.22, -0.2, 0.265)), fixed_base = False)
redBlock2 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.35, 0.05, 0.265)), fixed_base = False)
greenBlock1 = I.load_model(CUBE_URDF % 'green', Pose(Point(0.38, -0.1, 0.2599)), fixed_base = False)
greenBlock2 = I.load_model(CUBE_URDF % 'green', Pose(Point(0.38, 0.2, 0.2599)), fixed_base = False)
table = I.load_model(TABLE, Pose(Point(0.35, 0.0, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base = False, scaling = 0.3)
# ------------------------Jaco---------------------------#
velocities = Csv('JACO_velocities.csv',
['joints_3', 'joints_5', 'joints_7', 'joints_9', 'joints_11', 'joints_13', 'joints_16',
'joints_19', 'joints_22'], mode = 'r').read()
for joint, values in velocities.items():
velocities[joint] = map(float, values)
velocities = np.vstack(velocities.values())
I.replay(robot, velocities.T)
time.sleep(2.0)
I.disconnect()
def test_line():
l1 = Line(1, 0, 3) # x = 3
l2 = Line(0, 2, 8) # y = 4
l3 = Line(-1, 2, 1)
p = Point(0, 3)
check_almost_eq(l1.dist_to_point(p), 3.)
check_almost_eq(l2.dist_to_point(p), 1.)
check_almost_eq(l3.dist_to_point(p), np.sqrt(5.))
check_eq_points(Line.cross(l1, l2), Point(3, 4))
check_eq_points(Line.cross(l1, l3), Point(3, 2))
check_eq_points(Line.cross(l2, l3), Point(7, 4))
abc1 = Line(3, 2, 13)
roth1 = Line.line_by_ro_theta_1(np.sqrt(13), np.arctan(2 / 3))
abc2 = Line(1, 0, 5)
roth2 = Line.line_by_ro_theta_1(5, 0)
abc3 = Line(0, 1, 5)
roth3 = Line.line_by_ro_theta_1(5, np.pi / 2)
abc4 = Line(0, 1, -7)
roth4 = Line.line_by_ro_theta_1(7, -np.pi / 2)
check_eq_lines(abc1, roth1)
check_eq_lines(abc2, roth2)
check_eq_lines(abc3, roth3)
check_eq_lines(abc4, roth4)
for _ in range(100000):
a, b,c = 0, 0, 0
while abs(a) + abs(b) < 0.000001:
a, b, c = [uniform(-100, 100) for _ in range(3)]
l1 = Line(a, b, c)
ro, theta = l1.ro(), l1.theta()
l2 = Line.line_by_ro_theta_1(ro, theta)
check_eq_lines(l1, l2)
def fill_crd_and_frame():
for i in range(len(to_change)):
s = to_change[i]
crd.append(Point(s))
if is_useful(s):
frame.append(Point(s))
index.append(i)
def test_list_protocol(self):
"""A PolyLine should implement some of the same
methods that the 'list' class implements, so that
we can use it as if it were just a list of Points.
"""
pl = PolyLine()
self.assertEqual(len(pl), 0)
pl.append(Point(0,0))
self.assertEqual(len(pl), 1)
self.assertEqual(pl[0], Point(0,0))
pl.append(Point(2,3))
self.assertEqual(len(pl), 2)
self.assertEqual(pl[0], Point(0,0))
self.assertEqual(pl[1], Point(2,3))
self.assertEqual(pl[1],pl[-1])
self.assertEqual(pl[0],pl[-2])
with self.assertRaises(IndexError):
p = pl[3]
with self.assertRaises(IndexError):
p = pl[-3]
count = 0
for pt in pl:
self.assertEqual(pt, pl[count])
count += 1
def compute_fundamental_triangle(tessellation_configuration):
"""Compute the vertices of the hyperbolic triangle with the following
properties:
- Vertex A lies at the origin.
- Vertex C lies on the x-axis.
- Vertex B is chosen so that angle CAB is pi / p, and (hyperbolic) angle
ABC is pi / q.
Derivation:
The desired point is B = (b_x, b_y). This point is on the line
L: y = tan(pi / p) x and on an unknown circle C perpendicular to the unit
circle with center G = (g_x, 0).
If A = (0, 0) and D = (d_x, 0) is the intersection of the line [AG] with C,
then we need (hyperbolic) angle ABD to be pi / q. This is the same as
requiring that the tangent line to C at B forms an angle of pi / q with the
line between A and (cos(pi / q), sin(pi / q)), which has slope
tan(pi / p + pi / q).
The slope of a tangent line to circle C at point B is given by
y'(B) = -(b_x - g_x) / b_y
Setting y'(B) = tan(pi / p + pi / q) and writing b_y in terms of b_x gives
tan(pi / p + pi / q) tan(pi / p) = (g_x - b_x) / b_x
Or, letting Z = tan(pi / p + pi / q) tan(pi / p),
b_x(Z + 1) = g_x.
Next, we use the fact that C and the unit circle are orthogonal to get a
relationship between their radii (pythagorean theorem):
1^2 + r^2 = g_x^2, where r^2 = (b_x - g_x)^2 + tan(pi / p)^2 b_x^2
substituting in g = b_x (Z + 1) and solving for b_x,
b_x = sqrt(1 / (1 + 2Z - (tan(pi / p))^2))
We can then solve for b_y, g, and d_x trivially.
"""
p = tessellation_configuration.num_polygon_sides
q = tessellation_configuration.num_polygons_per_vertex
tan_p = math.tan(math.pi / p)
Z = math.tan(math.pi / p + math.pi / q) * tan_p
b_x = math.sqrt(1 / (1 + 2 * Z - tan_p ** 2))
b_y = b_x * tan_p
g_x = b_x * (Z + 1)
d_x = g_x - math.sqrt(b_y ** 2 + (b_x - g_x) ** 2)
A = Point(0, 0)
B = Point(b_x, b_y)
D = Point(d_x, 0)
return [A, B, D]
def test_line_segment_generation():
LineSegment()
with pytest.raises(ValueError):
LineSegment(Point(0, 0, 0), Point(0, 0, 0))
LineSegment(Point(1, 1, 1), Point(2, 2, 2))
def load():
moons = []
with open("input") as file:
for line in file:
x,y,z = [int(g) for g in re.findall("-?\d+", line)]
moons.append(Moon(Point(x,y,z), Point(0,0,0)))
return moons
def __init__(self, screen_size):
self.screen_position = Point(0, 0)
self.zoom = 1
self.screen_size = screen_size
self.meteor_models = []
self.meteor_views = []
for (position, radius,
color) in [(Point(0, 0), 5, pygame.color.Color(100, 100, 100)),
(Point(10, 5), 3, pygame.color.Color(100, 100, 150)),
(Point(15, 16), 6, pygame.color.Color(150, 100, 100))]:
(meteor_model, meteor_view) = create_pair(MeteorModel, MeteorView,
position, radius, color)
self.meteor_models.append(meteor_model)
self.meteor_views.append(meteor_view)
self.player_models = []
self.player_views = []
(player_model, player_view) = create_pair(PlayerModel, PlayerView,
self.meteor_models[0],
Point(1, -6))
self.player = player_model
self.player_models.append(player_model)
self.player_views.append(player_view)
self.should_quit = False
def test_point_round():
pt = Point(1.22, 2.33445, 2.3343)
rounded = round(pt)
assert rounded == Point(1, 2, 2)
rounded = round(pt, 2)
assert rounded == Point(1.22, 2.33, 2.33)
def test_pdist(self):
p1 = Point(1, 2)
p2 = Point(4, 6)
assert pdist(p1, p2) == 5
p1 = Point(8, 3)
p2 = Point(7, 4)
assert pdist(p1, p2) == pytest.approx(math.sqrt(2))
def test_create_segment(ws, canvas, cleanup):
layer = Layer('M2', 'pin')
s = Segment.from_start_end(Point(0, 1), Point(10, 1), 2, layer)
canvas.append(s)
rod, = canvas.draw()
assert rod.valid
assert segment_equal(s, rod.db)
def _apply_te_thickness(self, in_point):
add_thick = 0.5 * self._te_thickness * in_point.x / self._chord
if in_point.surface == Surfaces.TOP:
out_point = in_point + Point(0, add_thick)
elif in_point.surface == Surfaces.BTM:
out_point = in_point + Point(0, -add_thick)
out_point.surface = in_point.surface
return out_point
def test_point_distance():
p0 = Point(1, 2, 3)
p1 = Point(1, 2, 3)
assert p0.distance_to(p1) == 0
p2 = Point(3, 4, 12)
assert p2.distance_to(Point.at_origin()) == 13
def generate_segment(plot_side):
min_segment = plot_side / 10
point_1 = Point(uniform(0, plot_side), uniform(0, plot_side))
while True:
point_2 = Point(uniform(0, plot_side), uniform(0, plot_side))
if Point.distance_between(point_1, point_2) > min_segment:
break
return Segment(point_1, point_2)
def test_point_operations(self):
p1 = Point(1.0, 2.0, 3.0)
v = Vec(4.0, 6.0, 8.0)
p2 = Point(4.0, 6.0, 8.0)
assert (p1 * 2).is_close(Point(2.0, 4.0, 6.0))
assert (p1 + v).is_close(Point(5.0, 8.0, 11.0))
assert (p2 - p1).is_close(Vec(3.0, 4.0, 5.0))
assert (p1 - v).is_close(Point(-3.0, -4.0, -5.0))
def test_parallel():
l1 = Line() # X-axis
l2 = Line(direction_vector=Vector([4, 0, 0]), point=Point(8, 0, 0))
l3 = Line.from_points(Point(1, 1, 1), Point(2, 2, 2))
l4 = Line.from_points(Point(0, 0, 0), Point(-1, -1, -1))
assert l1.is_parallel(l2)
assert not l1.is_parallel(l3)
assert l3.is_parallel(l4)
def run(self):
self._window_manager.create_window()
self._capture_manager.paused = False
frame_generator = self.frames()
frame = next(frame_generator)
rois = detect(frame)
self.save_rois(frame, rois)
previous_angle = None
while self._window_manager.window_created and frame is not None:
frame = next(frame_generator)
ok, rois = self._multi_tracker.update(frame)
if ok and len(rois) > 1:
upper, lower = Rectangle(*rois[0]), Rectangle(*rois[1])
if not self._frame_no % 100:
new_rois = detect(frame)
if len(new_rois) >= 2:
new_similarity = roi_similarity(frame, *new_rois[:2])
old_similarity = roi_similarity(frame, upper, lower)
current_overlap = rois_overlap(upper, lower)
if old_similarity < 0.4 and new_similarity - old_similarity > 0.4:
for pair in zip(new_rois[:2], [upper, lower]):
overlap = rois_overlap(*pair)
if current_overlap > 0.2 or overlap < 0.4:
self.save_rois(frame, new_rois[:2])
current_angle = self.to_angle(upper, lower)
if current_angle != previous_angle:
previous_angle = current_angle
self._angle_change.append(self.to_angle(upper, lower))
base_angle_line = (
Point(upper.x, upper.y + upper.h//2),
Point(lower.x, upper.y + upper.h//2))
current_angle_line = (
Point(upper.x, upper.y + upper.h//2),
self.count_point(current_angle, upper, lower))
self._capture_manager.add_lines([current_angle_line, base_angle_line])
self._capture_manager.add_rois([upper, lower])
if not (previous_angle and self._capture_manager.paused):
print(current_angle)
elif not self._capture_manager.paused:
print("Tracking failure")
if frame is not None and not self._frame_no % 15:
new_rois = detect(frame)
self.save_rois(frame, new_rois)
self._window_manager.process_events()
if frame is None:
self._window_manager.destroy_window()
def clip(self, p1, p2):
c1 = self.point_to_coordinates(p1)
c2 = self.point_to_coordinates(Point(p1.x, p2.y))
c3 = self.point_to_coordinates(p2)
c4 = self.point_to_coordinates(Point(p2.x, p1.y))
self.fp.write("path b;\n")
self.fp.write("b := {0}--{1}--{2}--{3}--cycle;\n".format(
c1, c2, c3, c4))
self.fp.write("clip currentpicture to b;\n")
def test_orthogonal():
l1 = Line() # X-axis
l2 = Line(direction_vector=Vector([0, 1, 0]), point=Point(0, 0,
0)) # Y-axis
l3 = Line(direction_vector=Vector([1, 1, 0]), point=Point(0, 0, 0))
l4 = Line(direction_vector=Vector([0, 0, 1]), point=Point(0, 0, 0))
assert l1.is_orthogonal(l2)
assert l3.is_orthogonal(l4)
assert not l1.is_orthogonal(l3)
def readLinesFromFile(self, filename):
"""Not in use currently"""
with open(filename, 'r') as f:
for line in f.readlines():
if line[0] != '#':
data = [x for x in line.split('\t')]
points = [int(x) for x in data[0].split(',')]
self.tree.data.append(LineSegment(Point(points[0], points[1]), Point(
points[2], points[3]), int(data[1]), data[2][0:len(data[2]) - 1]))
def recur(node, depth=0):
node_point = Point(node.value[0], node.value[1])
if depth < self.max_recursion:
for child in node.children:
child_point = Point(child.value[0], child.value[1])
Line(node_point, child_point).draw(window,
color=color,
width=1)
recur(child, depth + 1)
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))
