def acceptable_support(angle,coord):
# Find beam endpoints
beam_endpoint = helpers.beam_endpoint(pivot,coord)
# Calculate angle from vertical of beam we wish to construct based on the
# information we've gathered
from_vertical = (angle_from_vertical + angle if beam_endpoint[2] <=
endpoint[2] else angle_from_vertical - angle)
simple = not (from_vertical < min_constraining_angle or from_vertical >
max_constraining_angle)
# On the ground
if self.beam is None:
return simple
# On a beam, so check our support_angle_difference
else:
beam_vector = helpers.make_vector(self.beam.endpoints.i,
self.beam.endpoints.j)
support_vector = helpers.make_vector(self.location,coord)
angle = helpers.smallest_angle(beam_vector,support_vector)
real_angle = abs(90-angle) if angle > 90 else angle
return simple and real_angle > BConstants.beam['support_angle_difference']
def support_vertical_change(self):
# Get vertical vector
change_vector = super(Repairer,self).support_vertical_change()
# If we're on the ground, just return (no rotation necessary)
if self.beam is None:
return change_vector
# Otherwise, rotate it based on our current beam
else:
# Debugging
# pdb.set_trace()
if self.memory['previous_direction'] is None:
#pdb.set_trace()
pass
# Get the correct vector for the current beam
i,j = self.beam.endpoints
current_vector = helpers.make_vector(i,j)
# Find rotation from vertical
angle = helpers.smallest_angle((0,0,1),current_vector)
rotation_angle = 180 - angle if angle > 90 else angle
vertical_angle = abs(BConstants.beam['support_angle'] - rotation_angle)
return super(Repairer,self).support_vertical_change(angle=vertical_angle)
def acceptable_support(angle, coord):
# Find beam endpoints
beam_endpoint = helpers.beam_endpoint(pivot, coord)
# Calculate angle from vertical of beam we wish to construct based on the
# information we've gathered
from_vertical = (angle_from_vertical +
angle if beam_endpoint[2] <= endpoint[2] else
angle_from_vertical - angle)
simple = not (from_vertical < min_constraining_angle
or from_vertical > max_constraining_angle)
# On the ground
if self.beam is None:
return simple
# On a beam, so check our support_angle_difference
else:
beam_vector = helpers.make_vector(self.beam.endpoints.i,
self.beam.endpoints.j)
support_vector = helpers.make_vector(self.location, coord)
angle = helpers.smallest_angle(beam_vector, support_vector)
real_angle = abs(90 - angle) if angle > 90 else angle
return simple and real_angle > BConstants.beam[
'support_angle_difference']
def support_vertical_change(self):
# Get vertical vector
change_vector = super(Repairer, self).support_vertical_change()
# If we're on the ground, just return (no rotation necessary)
if self.beam is None:
return change_vector
# Otherwise, rotate it based on our current beam
else:
# Debugging
# pdb.set_trace()
if self.memory['previous_direction'] is None:
#pdb.set_trace()
pass
# Get the correct vector for the current beam
i, j = self.beam.endpoints
current_vector = helpers.make_vector(i, j)
# Find rotation from vertical
angle = helpers.smallest_angle((0, 0, 1), current_vector)
rotation_angle = 180 - angle if angle > 90 else angle
vertical_angle = abs(BConstants.beam['support_angle'] -
rotation_angle)
return super(Repairer,
self).support_vertical_change(angle=vertical_angle)
def pick_support(vs):
'''
Returns index, sorting_angle of vs.
'''
angle_list = [abs(helpers.smallest_angle((1,0,0),v) -
BConstants.beam['support_angle']) for v in vs]
min_val = min(angle_list)
index = angle_list.index(min_val)
return index, min_val
def preferred(self, vector):
"""
Returns True if vector is preferred, False if it is not
"""
xy = self.memory["preferred_direction"]
xy = (xy[0], xy[1], 0)
if helpers.compare_tuple(xy, (0, 0, 0)) or helpers.compare_tuple((vector[0], vector[1], 0), (0, 0, 0)):
return True
return helpers.smallest_angle((vector[0], vector[1], 0), xy) <= BConstants.beam["direction_tolerance_angle"]
def start_repair(self, beam):
'''
Initializes the repair of the specified beam. Figures out which direction to
travel in and stores it within the robot's memory, then tells it to climb
down in a specific direction if necessary. Also sets the number of steps to
climb down looking for a support beam.
'''
def set_dir(string, coord):
'''
Figures out what pos_var should be in order to travel in that direction
NO LONGER USED
'''
if helpers.compare(coord, 0):
self.memory[string] = None
if coord > 0:
self.memory[string] = True
else:
self.memory[string] = False
angle_with_vertical = helpers.smallest_angle(
helpers.make_vector(beam.endpoints.i, beam.endpoints.j), (0, 0, 1))
# Get direction of travel
self.memory['preferred_direction'] = self.get_preferred_direction(beam)
# Get direction of travel if on the ground based on preferred direction on
# the structure
self.ground_direction = self.get_preferred_ground_direction(
self.memory['preferred_direction'])
# Travel down !
self.memory['pos_z'] = False
# Store name of repair beam
self.memory['broken_beam_name'] = beam.name
# Number of steps to search once we find a new beam that is close to
# parallel to the beam we are repairing (going down, ie NOT support beam)
length = BEAM['length'] * math.cos(
math.radians(BConstants.beam['support_angle']))
self.memory['new_beam_steps'] = math.floor(
length / ROBOT['step_length']) + 1
self.memory['new_beam_ground_steps'] = (
self.memory['new_beam_steps'] if self.ground_direction is None else
self.memory['new_beam_steps'] - 1 + math.floor(
math.sin(math.radians(angle_with_vertical)) *
self.memory['new_beam_steps']))
# So the entire robot knows that we are in repair mode
self.repair_mode = True
self.search_mode = True
def start_repair(self,beam):
'''
Initializes the repair of the specified beam. Figures out which direction to
travel in and stores it within the robot's memory, then tells it to climb
down in a specific direction if necessary. Also sets the number of steps to
climb down looking for a support beam.
'''
def set_dir(string,coord):
'''
Figures out what pos_var should be in order to travel in that direction
NO LONGER USED
'''
if helpers.compare(coord,0):
self.memory[string] = None
if coord > 0:
self.memory[string] = True
else:
self.memory[string] = False
angle_with_vertical = helpers.smallest_angle(helpers.make_vector(
beam.endpoints.i,beam.endpoints.j),(0,0,1))
# Get direction of travel
self.memory['preferred_direction'] = self.get_preferred_direction(beam)
# Get direction of travel if on the ground based on preferred direction on
# the structure
self.ground_direction = self.get_preferred_ground_direction(
self.memory['preferred_direction'])
# Travel down !
self.memory['pos_z'] = False
# Store name of repair beam
self.memory['broken_beam_name'] = beam.name
# Number of steps to search once we find a new beam that is close to
# parallel to the beam we are repairing (going down, ie NOT support beam)
length = BEAM['length'] * math.cos(
math.radians(BConstants.beam['support_angle']))
self.memory['new_beam_steps'] = math.floor(length/ROBOT['step_length'])+1
self.memory['new_beam_ground_steps'] = (self.memory['new_beam_steps'] if
self.ground_direction is None else self.memory['new_beam_steps'] - 1 + math.floor(
math.sin(math.radians(angle_with_vertical)) * self.memory['new_beam_steps']))
# So the entire robot knows that we are in repair mode
self.repair_mode = True
self.search_mode = True
def global_default_axes(self):
'''
Returns the default local axes. Later on we might incorporate the ability to return
rotated axes.
'''
axis_1 = helpers.make_unit(
helpers.make_vector(self.endpoints.i, self.endpoints.j))
vertical = (math.sin(
math.radians(helpers.smallest_angle(axis_1, (0, 0, 1)))) <= 0.001)
# Break up axis_1 into unit component vectors on 1-2 plane, along with
# their maginitudes
u1, u2 = (axis_1[0], axis_1[1], 0), (0, 0, axis_1[2])
l1, l2 = helpers.length(u1), helpers.length(u2)
u1 = helpers.make_unit(u1) if not helpers.compare(l1, 0) else (1, 1, 0)
u2 = helpers.make_unit(u2) if not helpers.compare(l2, 0) else (0, 0, 1)
# Calculate axis_2 by negating and flipping componenet vectors of axis_1
axis_2 = (1, 0, 0) if vertical else helpers.make_unit(
helpers.sum_vectors(helpers.scale(-1 *
l2, u1), helpers.scale(l1, u2)))
# Make it have a positive z-component
axis_2 = axis_2 if axis_2[2] > 0 else helpers.scale(-1, axis_2)
# Calculate axis_3 by crossing axis 1 with axis 2 (according to right hand
# rule)
axis_3 = helpers.cross(axis_1, axis_2)
axis_3 = helpers.make_unit(
(axis_3[0], axis_3[1], 0)) if vertical else axis_3
# Sanity checks
# Unit length
assert helpers.compare(helpers.length(axis_3), 1)
# On the x-y plane
assert helpers.compare(axis_3[2], 0)
return axis_1, axis_2, axis_3
def global_default_axes(self):
'''
Returns the default local axes. Later on we might incorporate the ability to return
rotated axes.
'''
axis_1 = helpers.make_unit(helpers.make_vector(self.endpoints.i,
self.endpoints.j))
vertical = (math.sin(math.radians(helpers.smallest_angle(axis_1,(0,0,1))))
<= 0.001)
# Break up axis_1 into unit component vectors on 1-2 plane, along with
# their maginitudes
u1, u2 = (axis_1[0],axis_1[1],0),(0,0,axis_1[2])
l1,l2 = helpers.length(u1), helpers.length(u2)
u1 = helpers.make_unit(u1) if not helpers.compare(l1,0) else (1,1,0)
u2 = helpers.make_unit(u2) if not helpers.compare(l2,0) else (0,0,1)
# Calculate axis_2 by negating and flipping componenet vectors of axis_1
axis_2 = (1,0,0) if vertical else helpers.make_unit(helpers.sum_vectors(
helpers.scale(-1 * l2,u1),helpers.scale(l1,u2)))
# Make it have a positive z-component
axis_2 = axis_2 if axis_2[2] > 0 else helpers.scale(-1,axis_2)
# Calculate axis_3 by crossing axis 1 with axis 2 (according to right hand
# rule)
axis_3 = helpers.cross(axis_1,axis_2)
axis_3 = helpers.make_unit((axis_3[0],axis_3[1],0)) if vertical else axis_3
# Sanity checks
# Unit length
assert helpers.compare(helpers.length(axis_3),1)
# On the x-y plane
assert helpers.compare(axis_3[2],0)
return axis_1,axis_2,axis_3
def support_beam_endpoint(self):
'''
Returns the endpoint for a support beam
'''
#pdb.set_trace()
# Get broken beam
e1,e2 = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
# Direction
v = helpers.make_unit(helpers.make_vector(e1,e2))
# Get pivot and repair beam midpoint
pivot = self.location
midpoint1 = helpers.midpoint(e1,e2)
# Upper midpoint to encourate upward building
midpoint = helpers.midpoint(e2,midpoint1)
# Add an offset to mimick inability to determine location exactly
offset = helpers.scale(random.uniform(-1*BEAM['random'],BEAM['random']),v)
midpoint = (helpers.sum_vectors(midpoint,offset) if random.randint(0,4) == 1
else midpoint)
# Calculate starting beam_endpoint
endpoint = helpers.beam_endpoint(pivot,midpoint)
# Calculate angle from vertical
angle_from_vertical = helpers.smallest_angle(helpers.make_vector(pivot,
endpoint),(0,0,1))
# Get angles
sorted_angles = self.local_angles(pivot,endpoint)
min_support_angle,max_support_angle = self.get_angles()
min_constraining_angle,max_constraining_angle = self.get_angles(
support=False)
# Defining here to have access to min,max, etc.
def acceptable_support(angle,coord):
# Find beam endpoints
beam_endpoint = helpers.beam_endpoint(pivot,coord)
# Calculate angle from vertical of beam we wish to construct based on the
# information we've gathered
from_vertical = (angle_from_vertical + angle if beam_endpoint[2] <=
endpoint[2] else angle_from_vertical - angle)
simple = not (from_vertical < min_constraining_angle or from_vertical >
max_constraining_angle)
# On the ground
if self.beam is None:
return simple
# On a beam, so check our support_angle_difference
else:
beam_vector = helpers.make_vector(self.beam.endpoints.i,
self.beam.endpoints.j)
support_vector = helpers.make_vector(self.location,coord)
angle = helpers.smallest_angle(beam_vector,support_vector)
real_angle = abs(90-angle) if angle > 90 else angle
return simple and real_angle > BConstants.beam['support_angle_difference']
return_coord = None
for coord,angle in sorted_angles:
if acceptable_support(angle,coord) and helpers.on_line(e1,e2,coord):
self.memory['broken_beam_name'] = ''
return coord
elif acceptable_support(angle,coord):
return_coord = coord
if return_coord is not None:
return return_coord
else:
# Otherwise, do default behaviour
return super(Repairer,self).support_beam_endpoint()
def support_xy_direction(self):
'''
Improves the construction direction so that we take into account the angle
at which our current beam is located, and the verticality of the beam we
are attempting to reach. This returns a unit direction (always should!)
'''
# If we're on the ground, then continue doing as before
if self.beam is None:
return super(Repairer,self).support_xy_direction()
else:
# Get repair beam vector
b_i,b_j = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
repair_vector = helpers.make_vector(b_i,b_j)
# Get the correct vector for the current beam
# Remember - we travel in the opposite direction as normal when building
# the support beam, so that's why this seems opposite of normal
c_i,c_j = self.beam.endpoints
current_vector = (helpers.make_vector(c_j,c_i) if
self.memory['previous_direction'][1][2] > 0 else helpers.make_vector(
c_i,c_j))
#
angle = helpers.smallest_angle(repair_vector,current_vector)
# If below the specified angle, then place the beam directly upwards (no
# change in xy)
if angle < BConstants.beam['direct_repair_limit']:
return None
else:
vertical = (0,0,1)
v1,v2 = helpers.make_vector(b_i,c_i), helpers.make_vector(b_i,c_j)
# We can't get a direction based on beam locations
if helpers.parallel(vertical,v1) and helpers.parallel(vertical,v2):
return super(Repairer,self).support_xy_direction()
# We can use the current beam to decide the direction
elif not helpers.parallel(vertical,current_vector):
# pdb.set_trace()
# Project onto the xy-plane and negate
if current_vector[2] > 0:
projection = helpers.make_unit(helpers.scale(-1,(current_vector[0],
current_vector[1],0)))
else:
projection = helpers.make_unit((current_vector[0],current_vector[1],0))
# Add some small disturbance
disturbance = helpers.scale(random.uniform(-1,1),(-projection[1],
projection[0],projection[2]))
result = helpers.sum_vectors(projection,disturbance)
# TODO
return result
elif not helpers.parallel(vertical,repair_vector):
return super(Repairer,self).support_xy_direction()
else:
return super(Repairer,self).support_xy_direction()
def add_angles(box, dictionary):
for name, beam in box.items():
# Ignore the beam you're on.
if self.beam == None or self.beam.name != name:
# Base vector (from which angles are measured)
base_vector = helpers.make_vector(pivot, endpoint)
# Get the closest points between the beam we want to construct and the
# current beam
points = helpers.closest_points(beam.endpoints, (pivot, endpoint))
if points != None:
# Endpoints (e1 is on a vertical beam, e2 is on the tilted one)
e1, e2 = points
# If we can actually reach the second point from vertical
if (
not helpers.compare(helpers.distance(pivot, e2), 0)
and helpers.distance(pivot, e2) <= BEAM["length"]
):
# Distance between the two endpoints
dist = helpers.distance(e1, e2)
# Vector of beam we want to construct and angle from base_vector
construction_vector = helpers.make_vector(pivot, e2)
angle = helpers.smallest_angle(base_vector, construction_vector)
# Add to dictionary
if e2 in dictionary:
assert helpers.compare(dictionary[e2], angle)
else:
dictionary[e2] = angle
# Get the points at which the beam intersects the sphere created by
# the vertical beam
sphere_points = helpers.sphere_intersection(beam.endpoints, pivot, BEAM["length"])
if sphere_points != None:
# Cycle through intersection points (really, should be two, though
# it is possible for it to be one, in
# which case, we would have already taken care of this). Either way,
# we just cycle
for point in sphere_points:
# Vector to the beam we want to construct
construction_vector = helpers.make_vector(pivot, point)
angle = helpers.smallest_angle(base_vector, construction_vector)
# Add to dictionary
if point in dictionary:
assert helpers.compare(dictionary[point], angle)
else:
dictionary[point] = angle
# Endpoints are also included
for e in beam.endpoints:
v = helpers.make_vector(pivot, e)
l = helpers.length(v)
if (
e not in dictionary
and not helpers.compare(l, 0)
and (helpers.compare(l, BEAM["length"]) or l < BEAM["length"])
):
angle = helpers.smallest_angle(base_vector, v)
dictionary[e] = angle
return dictionary
def add_angles(box, dictionary):
for name, beam in box.items():
# Ignore the beam you're on.
if self.beam == None or self.beam.name != name:
# Base vector (from which angles are measured)
base_vector = helpers.make_vector(pivot, endpoint)
# Get the closest points between the beam we want to construct and the
# current beam
points = helpers.closest_points(beam.endpoints,
(pivot, endpoint))
if points != None:
# Endpoints (e1 is on a vertical beam, e2 is on the tilted one)
e1, e2 = points
# If we can actually reach the second point from vertical
if (not helpers.compare(helpers.distance(pivot, e2), 0)
and
helpers.distance(pivot, e2) <= BEAM['length']):
# Distance between the two endpoints
dist = helpers.distance(e1, e2)
# Vector of beam we want to construct and angle from base_vector
construction_vector = helpers.make_vector(
pivot, e2)
angle = helpers.smallest_angle(
base_vector, construction_vector)
# Add to dictionary
if e2 in dictionary:
assert helpers.compare(dictionary[e2], angle)
else:
dictionary[e2] = angle
# Get the points at which the beam intersects the sphere created by
# the vertical beam
sphere_points = helpers.sphere_intersection(
beam.endpoints, pivot, BEAM['length'])
if sphere_points != None:
# Cycle through intersection points (really, should be two, though
# it is possible for it to be one, in
# which case, we would have already taken care of this). Either way,
# we just cycle
for point in sphere_points:
# Vector to the beam we want to construct
construction_vector = helpers.make_vector(
pivot, point)
angle = helpers.smallest_angle(
base_vector, construction_vector)
# Add to dictionary
if point in dictionary:
assert helpers.compare(dictionary[point],
angle)
else:
dictionary[point] = angle
# Endpoints are also included
for e in beam.endpoints:
v = helpers.make_vector(pivot, e)
l = helpers.length(v)
if (e not in dictionary and not helpers.compare(l, 0)
and (helpers.compare(l, BEAM['length'])
or l < BEAM['length'])):
angle = helpers.smallest_angle(base_vector, v)
dictionary[e] = angle
return dictionary
def support_beam_endpoint(self):
'''
Returns the endpoint for a support beam
'''
#pdb.set_trace()
# Get broken beam
e1, e2 = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
# Direction
v = helpers.make_unit(helpers.make_vector(e1, e2))
# Get pivot and repair beam midpoint
pivot = self.location
midpoint1 = helpers.midpoint(e1, e2)
# Upper midpoint to encourate upward building
midpoint = helpers.midpoint(e2, midpoint1)
# Add an offset to mimick inability to determine location exactly
offset = helpers.scale(
random.uniform(-1 * BEAM['random'], BEAM['random']), v)
midpoint = (helpers.sum_vectors(midpoint, offset) if random.randint(
0, 4) == 1 else midpoint)
# Calculate starting beam_endpoint
endpoint = helpers.beam_endpoint(pivot, midpoint)
# Calculate angle from vertical
angle_from_vertical = helpers.smallest_angle(
helpers.make_vector(pivot, endpoint), (0, 0, 1))
# Get angles
sorted_angles = self.local_angles(pivot, endpoint)
min_support_angle, max_support_angle = self.get_angles()
min_constraining_angle, max_constraining_angle = self.get_angles(
support=False)
# Defining here to have access to min,max, etc.
def acceptable_support(angle, coord):
# Find beam endpoints
beam_endpoint = helpers.beam_endpoint(pivot, coord)
# Calculate angle from vertical of beam we wish to construct based on the
# information we've gathered
from_vertical = (angle_from_vertical +
angle if beam_endpoint[2] <= endpoint[2] else
angle_from_vertical - angle)
simple = not (from_vertical < min_constraining_angle
or from_vertical > max_constraining_angle)
# On the ground
if self.beam is None:
return simple
# On a beam, so check our support_angle_difference
else:
beam_vector = helpers.make_vector(self.beam.endpoints.i,
self.beam.endpoints.j)
support_vector = helpers.make_vector(self.location, coord)
angle = helpers.smallest_angle(beam_vector, support_vector)
real_angle = abs(90 - angle) if angle > 90 else angle
return simple and real_angle > BConstants.beam[
'support_angle_difference']
return_coord = None
for coord, angle in sorted_angles:
if acceptable_support(angle, coord) and helpers.on_line(
e1, e2, coord):
self.memory['broken_beam_name'] = ''
return coord
elif acceptable_support(angle, coord):
return_coord = coord
if return_coord is not None:
return return_coord
else:
# Otherwise, do default behaviour
return super(Repairer, self).support_beam_endpoint()
def support_xy_direction(self):
'''
Improves the construction direction so that we take into account the angle
at which our current beam is located, and the verticality of the beam we
are attempting to reach. This returns a unit direction (always should!)
'''
# If we're on the ground, then continue doing as before
if self.beam is None:
return super(Repairer, self).support_xy_direction()
else:
# Get repair beam vector
b_i, b_j = self.structure.get_endpoints(
self.memory['broken_beam_name'], self.location)
repair_vector = helpers.make_vector(b_i, b_j)
# Get the correct vector for the current beam
# Remember - we travel in the opposite direction as normal when building
# the support beam, so that's why this seems opposite of normal
c_i, c_j = self.beam.endpoints
current_vector = (helpers.make_vector(c_j, c_i)
if self.memory['previous_direction'][1][2] > 0
else helpers.make_vector(c_i, c_j))
#
angle = helpers.smallest_angle(repair_vector, current_vector)
# If below the specified angle, then place the beam directly upwards (no
# change in xy)
if angle < BConstants.beam['direct_repair_limit']:
return None
else:
vertical = (0, 0, 1)
v1, v2 = helpers.make_vector(b_i, c_i), helpers.make_vector(
b_i, c_j)
# We can't get a direction based on beam locations
if helpers.parallel(vertical, v1) and helpers.parallel(
vertical, v2):
return super(Repairer, self).support_xy_direction()
# We can use the current beam to decide the direction
elif not helpers.parallel(vertical, current_vector):
# pdb.set_trace()
# Project onto the xy-plane and negate
if current_vector[2] > 0:
projection = helpers.make_unit(
helpers.scale(
-1, (current_vector[0], current_vector[1], 0)))
else:
projection = helpers.make_unit(
(current_vector[0], current_vector[1], 0))
# Add some small disturbance
disturbance = helpers.scale(random.uniform(
-1, 1), (-projection[1], projection[0], projection[2]))
result = helpers.sum_vectors(projection, disturbance)
# TODO
return result
elif not helpers.parallel(vertical, repair_vector):
return super(Repairer, self).support_xy_direction()
else:
return super(Repairer, self).support_xy_direction()
