def __init__(self, face, initialAngle=0, initialPoint=Point(0,0)):
self.edges = []
self.annotations = []
initialEdge0 = face.edges[0]
initialEdge1 = face.edges[1]
initialEdge2 = face.edges[2]
# build initial 3 triangle points
side0len = initialEdge0.magnitude
side1len = initialEdge1.magnitude
angle01 = initialEdge0.vertB.angleThreePoints(initialEdge2.vertB, initialEdge1.vertB)
twoDPoint20 = Point(0, 0)
twoDPoint01 = twoDPoint20.plusPolar( side0len, initialAngle)
twoDPoint12 = twoDPoint01.plusPolar( side1len, 180 - angle01 + initialAngle)
# draw center annotation
midpoint = (twoDPoint01+twoDPoint12)/2
midpoint = (midpoint+twoDPoint20)/2
centerAnnotation = DrawnText(initialEdge0.faceId, constants.dimensions.large_text_size, midpoint)
self.addAnnotation(centerAnnotation)
self.buildEdge(initialEdge0, twoDPoint20, twoDPoint01, False)
self.buildEdge(initialEdge1, twoDPoint01, twoDPoint12, False)
self.buildEdge(initialEdge2, twoDPoint12, twoDPoint20, False)
self.offset = initialPoint
def test_creation(self):
self.assertRaises((Exception, ArgumentError), TrajectoryLineSegment, \
LineSegment.from_points([Point(0, 0), Point(1, 1)]), -1)
self.assertRaises((Exception, ArgumentError), TrajectoryLineSegment,
None, 1)
self.assertRaises((Exception, ArgumentError), TrajectoryLineSegment, \
LineSegment.from_points([Point(0, 0), Point(1, 1)]), None)
def create_trajectory_line_seg(self,
start,
end,
traj_id,
original_position=None):
return TrajectoryLineSegment(LineSegment.from_points([Point(start[0], start[1]), \
Point(end[0], end[1])]), traj_id, original_position)
def is_time_to_leave_wall(self, x, y, theta):
"""
This function is regularly called from the wallfollower state to check
the brain's belief about whether it is the right time (or place) to
leave the wall and move straight to the goal.
"""
self.wp_test_pose = Point(x, y)
self.new_is_left = self.ln_goal_vector.point_left(self.wp_test_pose)
#Line for comparing if robot is to left
self.ln_robot = Line.from_points(
[Point(x, y), Point(x + cos(theta), y + sin(theta))])
#Check for ditance to entry point
if abs(self.wp_test_pose.distance_to(
self.wp_entry_pose)) > self.POINT_TOLERANCE:
#Check for a change on side
if self.old_is_left != self.new_is_left:
self.old_is_left = self.new_is_left
#Save point
self.pose_list.append(self.wp_test_pose)
#Check goal is left to robot
if self.ln_robot.distance_to(self.wp_goal_point) < 0:
#Check if first time on point, then leave
if self.is_pose_repeated(self.wp_test_pose) == 1:
return True
return False
def test_two_line_segs(self):
points = [[Point(0, 1), Point(1, 1)], \
[Point(0, 0), Point(1, 0)]]
expected = [[Point(0, 0.5), (1, 0.5)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=100, min_neighbors=1, min_num_trajectories_in_cluster=2, min_vertical_lines=2, min_prev_dist=1.0)
self.verify_iterable_works_more_than_once(iterable=res,
list_ob=expected)
def test_four_line_sets_should_result(self):
lines = [LineSegment.from_points([Point(0, 0), Point(1, 0)]), \
LineSegment.from_points([Point(0.5, 1), Point(1.5, 1)])]
traj_lines = [TrajectoryLineSegment(lines[0], 0), \
TrajectoryLineSegment(lines[1], 1)]
res = get_representative_trajectory_average_inputs(trajectory_line_segments=traj_lines, \
min_prev_dist=0.5, min_lines=1)
expected = 4
self.assertEquals(len(res), expected)
def test_two_short_line_endpoints_get_picked(self):
lines = [LineSegment.from_points([Point(0, 0), Point(1, 0)]), \
LineSegment.from_points([Point(0, 1), Point(1, 1)])]
traj_lines = [TrajectoryLineSegment(lines[0], 0), \
TrajectoryLineSegment(lines[1], 1)]
res = get_representative_trajectory_average_inputs(trajectory_line_segments=traj_lines, \
min_prev_dist=1, min_lines=2)
expected = 2
self.assertEquals(len(res), expected)
def test_three_lines(self):
traj_line_segments = [self.create_trajectory_line_seg((0.0, 0.0), (2.0, 0.0), 0), \
self.create_trajectory_line_seg(start=(0.0, 1.0), end=(2.0, 1.0), traj_id=2), \
self.create_trajectory_line_seg(start=(0.0, 2.0), end=(2.0, 2.0), traj_id=1)]
res = get_representative_line_from_trajectory_line_segments(
traj_line_segments, 1, 0)
expected = [Point(0, 1), Point(2, 1)]
self.verify_iterable_works_more_than_once(iterable=res,
list_ob=expected)
def test_simple_lines(self):
traj_line_segments = [self.create_trajectory_line_seg(start=(0.0, 0.0), end=(1.0, 0.0), traj_id=0), \
self.create_trajectory_line_seg(start=(0.0, 1.0), end=(1.0, 1.0), traj_id=1)]
res = get_representative_line_from_trajectory_line_segments(
traj_line_segments, 1, 0)
expected = [Point(0, 0.5), Point(1, 0.5)]
for p in res:
self.assertEquals(p.__class__, Point)
self.verify_iterable_works_more_than_once(iterable=res,
list_ob=expected)
def test_single_line_seg(self):
points = [[Point(0, 0), Point(2, 0)]]
expected = [[Point(0, 0), Point(2, 0)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=1, \
min_neighbors=0, min_num_trajectories_in_cluster=1, \
min_vertical_lines=1, \
min_prev_dist=1)
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
def test_three_vertical_points_in_a_row_small_spacing(self):
points = [[Point(0, 0), Point(0, 1.1), Point(0, 2.2)]]
expected = [[Point(0, 0), Point(0, 2.2)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=1, \
min_neighbors=0, min_num_trajectories_in_cluster=1, \
min_vertical_lines=1, \
min_prev_dist=1)
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
def test_three_points_in_a_row_negative_diagonal(self):
points = [[Point(0, 20), Point(10, 10), Point(20, 0)]]
expected = [[Point(0, 20), Point(20, 0)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=0, \
min_neighbors=0, min_num_trajectories_in_cluster=1, \
min_vertical_lines=1, \
min_prev_dist=(2 * math.sqrt(2.0) - DECIMAL_MAX_DIFF_FOR_EQUALITY))
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
def test_doesnt_partition_diamond(self):
traj = [
Point(0, 10),
Point(10, 20),
Point(20, 10),
Point(10, 0),
Point(0, 10)
]
expected_par = [0, 1, 2, 3, 4]
self.verify_iterable_works_more_than_once(call_partition_trajectory(trajectory_point_list=traj), \
expected_par)
def test_longer_obvious_partition(self):
trajectory = [
Point(0, 0),
Point(10000, 1),
Point(20000, 0),
Point(30000, 2),
Point(40000, 1)
]
expected_par = [0, 4]
self.verify_iterable_works_more_than_once(expected_par, \
call_partition_trajectory(trajectory_point_list=trajectory))
def test_partition_happens_with_three_points_in_a_row_horizontally(self):
points = [[Point(0, 0), Point(2, 0), Point(4, 0)]]
expected = [[Point(0, 0), Point(4, 0)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=0, \
min_neighbors=0, min_num_trajectories_in_cluster=1, \
min_vertical_lines=1, \
min_prev_dist=4)
self.verify_iterable_works_more_than_once(iterable=res,
list_ob=expected)
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
def create_line(self,
horizontal_start,
horizontal_end,
line_set_ids,
trajectory_id,
orig_pos=None):
if orig_pos == None:
orig_pos = random.randint(0, 1000)
return {'trajectory_line_segment': \
TrajectoryLineSegment(LineSegment.from_points([Point(horizontal_start, random.randint(0, 1000)), \
Point(horizontal_end, random.randint(0, 1000))]), \
trajectory_id, orig_pos), \
'line_set_ids': line_set_ids}
def follow_wall(self, x, y, theta):
"""
This function is called when the state machine enters the wallfollower
state. """
self.x = x
self.y = y
self.theta = theta
self.wp_hit_point = Point(self.x, self.y) #creating a list for all the points it hits
self.hit_points_list.append((x,y))
self.ln_line_to_goal = Line.from_points([self.wp_hit_point, self.wp_goal_point]) # Draws a line from hit point to goal point
self.Vec_hit = Point(x,y)
self.angle_vec = self.wp_goal_point-self.Vec_hit
print self.angle_vec
self.last_hit_dist = self.Vec_hit.distance_to(self.wp_goal_point)
self.count = self.count + 1
class TestPoint(ClusterCandidate):
def __init__(self, x, y):
ClusterCandidate.__init__(self)
self.point = Point(x, y)
def distance_to_candidate(self, other_candidate):
return self.point.distance_to(other_candidate.point)
def read_test_file(file_name):
file = open(file_name)
num_dimensions = int(file.next())
if num_dimensions != 2:
raise Exception("didn't make this for this")
num_traj = int(file.next())
exp_id = 0
all_traj = []
for line in file:
tokens = line.split()
id = int(tokens[0])
num_points = int(tokens[1])
if id != exp_id:
raise Exception("something wrong")
traj = []
for i in filter(lambda x: x % 2 == 0, xrange(0, num_points)):
x = float(tokens[i + 2])
try:
y = float(tokens[i + 3])
except IndexError:
print "error on index " + str(i) + " with id " + str(id)
traj.append(Point(x, y))
all_traj.append(traj)
exp_id += 1
file.close()
return all_traj
def is_time_to_leave_wall(self, x, y, theta):
#calculating the distance between the current point to the line
distance = self.ln_one.distance_to(Point(x, y))
'''
check whether the distance between the current point and line and
return TRUE if the point lies on the line (or) return FALSE if the point away from the line
'''
if (abs(distance) < 0.02
and (abs(self.x1 - x) > 0.5 or abs(self.y1 - y) > 0.5)):
for a, b in zip(self.x_list, self.y_list):
if (abs(x - a) < 0.5 and abs(y - b) < 0.5):
if (abs(self.x_list[0] - x) < 0.5
and abs(self.y_list[0] - y) < 0.5):
self.goal_unreachable_count += 1
self.flag = False
break
else:
self.flag = True
#if the flag zero(point which is unvisited), the add the current point to the list
if (self.flag == True):
self.x_list.append(x)
self.y_list.append(y)
self.flag = False
return True
else:
return False
def __init__(self, goal_x, goal_y, side):
self.wall_side = side;
self.wp_destination = Point(goal_x, goal_y);
self.path_started = False;
#the tolerence == planar.EPSILON at default value does not work good
planar.set_epsilon(0.1);
self.distance_when_left = 9999;
def leave_wall(self, x, y, theta):
"""
This function is called when the state machine leaves the wallfollower
state.
"""
self.flag = True
self.wp_leave_wall_points = Point(x,y)
def __init__(self, goal_x, goal_y, side):
self.wp_goal_point = Point(goal_x, goal_y)
self.first_line = True
self.goal_unreachable = True
self.pose_list = []
self.old_is_left = True
self.new_is_left = True
pass
def test_parrallel_distance_joins_two_lines_segs(self):
points = [[Point(0, 0), Point(1, 0)], \
[Point(2, 0), Point(3, 0)]]
expected = [[Point(0, 0), Point(1, 0), Point(2, 0), Point(3, 0)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=1, \
min_neighbors=1, min_num_trajectories_in_cluster=2, \
min_vertical_lines=1, \
min_prev_dist=1)
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
def is_goal_unreachable(self, x, y, theta):
"""
This function is regularly called from the wallfollower state to check
the brain's belief about whether the goal is unreachable.
"""
if self.is_pose_repeated(Point(x, y)) > 1:
return True
return False
def is_contain(query_point, vertex_lst):
if len(vertex_lst) == 4:
vertex_box = copy.deepcopy(vertex_lst)
vertex_box = clockwise_sort(vertex_box)
polygon = Polygon(vertex_box)
else:
polygon = Polygon(vertex_lst)
point = Point(query_point[0], query_point[1])
if not polygon.is_convex:
return False
return polygon.contains_point(point)
def get_representative_line_from_rotated_line_segments(
trajectory_line_segments, min_vertical_lines, min_prev_dist):
inputs = get_representative_trajectory_average_inputs(trajectory_line_segments=trajectory_line_segments, \
min_prev_dist=min_prev_dist, min_lines=min_vertical_lines)
out = []
for line_seg_averaging_input in inputs:
vert_val = get_mean_vertical_coordinate_in_line_segments(
line_seg_averaging_input)
out.append(
Point(line_seg_averaging_input['horizontal_position'], vert_val))
return out
def test_reads_file(self):
expected = [[Point(1, 2), Point(3, 4)], \
[Point(2, 4), Point(6, 8)], \
[Point(1, 3), Point(5, 7)], \
[Point(9, 8)]]
res = read_test_file(os.path.dirname(__file__) + "\\dummy_input.txt")
exp_iter = iter(expected)
for traj in res:
exp_line = exp_iter.next()
self.assertListEqual(exp_line, traj)
def new_origin_x(cls, width: float, length: float) -> "DirectedRectangle":
"""Creates a new rect, centered at origin, headed in direction of the X axis."""
center = Point(0, 0)
top_right = center + Vec2(length / 2, width / 2)
shape = Polygon.from_points([
top_right,
top_right - Vec2(0, width),
top_right - Vec2(length, width),
top_right - Vec2(length, 0),
])
return cls(Ray(center, Vec2(1, 0)), shape)
def main_no_ui() -> None:
from time import sleep
neural_networks = []
for _ in range(100):
neural_networks.append(
NeuralNetwork(
[
LayerInfo(8, "tanh"),
LayerInfo(15, "sigmoid"),
LayerInfo(10, "sigmoid"),
],
2,
))
NeuralNetworkStore.store(neural_networks, "data")
neural_networks.clear()
neural_networks = NeuralNetworkStore.load("data")
print(len(neural_networks))
neural_network = neural_networks[0]
sensor = Sensor(Ray((0, 0), (0, 1))) # TODO: move construction elsewhere
car = Car((1, 2), [sensor], neural_network)
track = Track.from_points([
(Point(-10.0, -10.0), Point(-10.0, 10.0)),
(Point(10.0, -10.0), Point(10.0, 10.0)),
(Point(20.0, -10.0), Point(20.0, 10.0)),
])
sleep_time = 0.1
while True:
sleep(sleep_time)
car.tick(track, sleep_time)
def test_normal_turning_line(self):
points = [[
Point(0, 0),
Point(20, 20),
Point(40, 0),
Point(60, 20),
Point(80, 0)
]]
expected = [[
Point(0, 0),
Point(20, 20),
Point(40, 0),
Point(60, 20),
Point(80, 0)
]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=100, \
min_neighbors=3, min_num_trajectories_in_cluster=1, \
min_vertical_lines=1, \
min_prev_dist=1)
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
def test_two_vertical_line_segments(self):
points = [[Point(0, 0), Point(0, 1)], \
[Point(1, 0), Point(1, 1)]]
expected = [[Point(0.5, 0.0), Point(0.5, 1)]]
res = the_whole_enchilada(point_iterable_list=points, \
epsilon=100, min_neighbors=0, min_num_trajectories_in_cluster=1, min_vertical_lines=1, min_prev_dist=0)
self.verify_point_iterable_almost_equals_list(iterable=res,
expected_list=expected)
class BugBrain:
# declearing constants
def LEFT_WALLFOLLOWING(self):
return 0;
def RIGHT_WALLFOLLOWING(self):
return 1;
def LEFT_SIDE(self):
return -1;
def RIGHT_SIDE(self):
return 1;
def TOLERANCE(self):
return planar.EPSILON;
def __init__(self, goal_x, goal_y, side):
self.wall_side = side;
self.wp_destination = Point(goal_x, goal_y);
self.path_started = False;
#the tolerence == planar.EPSILON at default value does not work good
planar.set_epsilon(0.1);
self.distance_when_left = 9999;
# method to determin if the destenation is on opposit side of wall being followed.
# @param: distance
# signed distance from the robot to goal.
# can be obtained by path_line.distance_to(ROBOT CURRENT POSITION).
def is_destination_opposite_to_wall(self,distance):
direction = math.copysign(1,distance);
if(self.wall_side == self.LEFT_WALLFOLLOWING()):
if(direction == self.RIGHT_SIDE()):
return True;
else:
return False;
else:
if(direction == self.LEFT_SIDE()):
return True;
else:
return False;
def follow_wall(self, x, y, theta):
"""
This function is called when the state machine enters the wallfollower
state.
"""
# compute and store necessary variables
theta = degrees(theta);
position = Point(x,y);
self.ln_path = Line.from_points([position,self.wp_destination]);
# saving where it started wall following
self.wp_wf_start = position;
def leave_wall(self, x, y, theta):
"""
This function is called when the state machine leaves the wallfollower
state.
"""
# compute and store necessary variables
self.path_started = False;
self.distance_when_left = self.wp_destination.distance_to(Point(x,y));
self.wp_left_wall_at = Point(x,y);
# self.wp_when_left =
pass
def is_goal_unreachable(self, x, y, theta):
"""
This function is regularly called from the wallfollower state to check
the brain's belief about whether the goal is unreachable.
"""
# if the robot goes around an obstacle and
# reaches the starting point and the destenation is still not reached then
# the goal is unreachable.
distance_to_path= self.ln_path.distance_to(Point(x,y));
if(abs(distance_to_path) < self.TOLERANCE()
and Vec2(x,y).almost_equals(self.wp_wf_start)
and self.path_started):
# self.path_started = False;
rospy.logwarn("UNREACHABLE POINT!");
return True
return False
def is_time_to_leave_wall(self, x, y, theta):
"""
This function is regularly called from the wallfollower state to check
the brain's belief about whether it is the right time (or place) to
leave the wall and move straight to the goal.
"""
theta = degrees(theta);
self.current_theta =theta;
self.wp_current_position = Point(x,y);
self.current_direction = Vec2.polar(angle = theta,length = 1);
#Robot Orientation Line.
self.ln_current_orentation = Line(Vec2(x,y),self.current_direction);
# the prependicular line to the path
self.ln_distance = self.ln_path.perpendicular(self.wp_current_position);
distance_to_path= self.ln_path.distance_to(Point(x,y));
self.distance_to_path = distance_to_path;
distance_to_destination = self.ln_current_orentation.distance_to(self.wp_destination);
if(abs(distance_to_path) > 1):
self.path_started =True;
self.distance_to_goal = self.wp_destination.distance_to(Point(x,y));
"""
checking if distance to the straight path is approx. 0 and
if destenation on the opposit side of wall then leave the path
NOTE and TODO: works only for the circles not for complex path.
"""
if(abs(distance_to_path) < self.TOLERANCE() and
self.distance_to_goal < self.distance_when_left and
self.is_destination_opposite_to_wall(distance_to_destination) and
self.path_started): # is robot started following wall!
self.wp_wf_stop = Point(x,y);
return True;
return False