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 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 ground(self,random=False):
'''
This function finds the nearest beam to the robot that is connected
to the xy-plane (ground). It returns that beam and its direction from the
robot. If random = True, the function picks a random beam from those connected
to the xy-plane.
'''
# Get local boxes
boxes = self.structure.get_boxes(self.location)
# Initializations
distances = {}
vectors = {}
# Cycle through boxes
for box in boxes:
# Cycle through beams in each box
for name in box:
# So e1 is in the form (x,y,z)
e1, e2 = box[name].endpoints
# beam is lying on the ground (THIS IS NOT FUNCTIONAL)
if helpers.compare(e1[2],0) and helpers.compare(e2[0],0):
# pdb.set_trace()
vectors[name] = helpers.vector_to_line(e1,e2,self.location)
distances[name] = helpers.length(vectors[name])
# Only one point is on the ground
if helpers.compare(e1[2],0):
vectors[name] = helpers.make_vector(self.location, e1)
distances[name] = helpers.distance(e1, self.location)
elif helpers.compare(e2[2],0):
vectors[name] = helpers.make_vector(self.location, e2)
distances[name] = helpers.distance(e2, self.location)
# No points on the ground
else:
pass
# get name of beam at the minimum distance if one exists
if distances == {}:
return None
else:
# Random key
if random: name = choice(list(distances.keys()))
# This returns the key (ie, name) of the minimum value in distances
else: name = min(distances, key=distances.get)
# So far away that we can't "see it"
if distances[name] > ROBOT['local_radius']:
return None
else:
# All the same beans
beams = [box[name] for box in boxes if name in box]
return { 'beam' : beams[0],
'distance' : distances[name],
'direction' : vectors[name]}
def ground(self):
'''
This function finds the nearest beam to the robot that is connected
to the xy-plane (ground). It returns that beam and its direction from the
robot.
'''
# Get local boxes
boxes = self.structure.get_boxes(self.location)
# Initializations
distances = {}
vectors = {}
# Cycle through boxes
for box in boxes:
# Cycle through beams in each box
for name in box:
# So e1 is in the form (x,y,z)
e1, e2 = box[name].endpoints
# beam is lying on the ground (THIS IS NOT FUNCTIONAL)
if helpers.compare(e1[2], 0) and helpers.compare(e2[0], 0):
# pdb.set_trace()
vectors[name] = helpers.vector_to_line(
e1, e2, self.location)
distances[name] = helpers.length(vectors[name])
# Only one point is on the ground
elif helpers.compare(e1[2], 0):
vectors[name] = helpers.make_vector(self.location, e1)
distances[name] = helpers.distance(e1, self.location)
elif helpers.compare(e2[2], 0):
vectors[name] = helpers.make_vector(self.location, e2)
distances[name] = helpers.distances(e2, self.location)
# No points on the ground
else:
pass
# get name of beam at the minimum distance if one exists
if distances == {}:
return None
else:
# This returns the key (ie, name) of the minimum value in distances
name = min(distances, key=distances.get)
# So far away that we can't "see it"
if distances[name] > ROBOT['local_radius']:
return None
else:
# All the same beans
beams = [box[name] for box in boxes if name in box]
return {
'beam': beams[0],
'distance': distances[name],
'direction': vectors[name]
}
def go_to_construction_site(self):
vector_to_site = helpers.make_vector(self.Body.getLocation(), CONSTRUCTION['center'])
if helpers.length(vector_to_site) > 10:
direction_construction = helpers.make_vector(self.Body.getLocation(), CONSTRUCTION['center'])
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
self.Body.step, helpers.make_unit(direction_construction)))
self.Body.changeLocalLocation(new_location)
return True
else:
direction_construction = helpers.make_vector(self.Body.getLocation(), CONSTRUCTION['center'])
new_location = helpers.sum_vectors(self.Body.getLocation(), direction_construction)
self.Body.changeLocalLocation(new_location)
return True
def __init__(self,name,structure,location,program):
super(DumbMovable, self).__init__(name,program)
# Access to my Python structure
self.structure = structure
# Number of steps left in movement
self.step = ROBOT['step_length']
# The current location of the robot on the designed structure
self.location = location
# The beam on which the robot currently is
self.beam = None
# The weight of the robot
self.weight = ROBOT['load']
# The direction in which we should move
self.next_direction_info = None
# Contains Errors from SAP 2000
self.error_data = ''
# The robots all initially move towards the centertower
self.ground_direction = helpers.make_vector(location,
CONSTRUCTION['corner'])
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 __init__(self, name, structure, location, program):
super(DumbMovable, self).__init__(name, program)
# Access to my Python structure
self.structure = structure
# Number of steps left in movement
self.step = ROBOT['step_length']
# The current location of the robot on the designed structure
self.location = location
# The beam on which the robot currently is
self.beam = None
# The weight of the robot
self.weight = ROBOT['load']
# The direction in which we should move
self.next_direction_info = None
# Contains Errors from SAP 2000
self.error_data = ''
# The robots all initially move towards the centertower
self.ground_direction = helpers.make_vector(location,
CONSTRUCTION['corner'])
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 get_disturbance(self):
'''
Returns the disturbance level for adding a new beam at the tip. This is
modified so that the disturbance compensates for the angle at which the
current beam lies (using basic math)
'''
def compensate_change(coord, change=PROGRAM['epsilon']):
'''
Returns a random direction that is the right sign so that it compensates
for the sign of change
'''
if helpers.compare(coord, 0):
return random.uniform(-1 * change, change)
elif coord < 0:
return random.uniform(0, change)
else:
return random.uniform(-1 * change, 0)
# We are currently on a beam
if self.beam is not None:
i, j = self.beam.endpoints
v = helpers.make_vector(i, j)
const_change = lambda x: compensate_change(x, BEAM['random'])
delta_x, delta_y = const_change(v[0]), const_change(v[1])
return (delta_x, delta_y, 0)
else:
return super(DeflectionRepairer, self).get_disturbance()
def go_to_construction_site(self):
vector_to_site = helpers.make_vector(self.Body.getLocation(),
CONSTRUCTION['center'])
if helpers.length(vector_to_site) > 10:
direction_construction = helpers.make_vector(
self.Body.getLocation(), CONSTRUCTION['center'])
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
self.Body.step, helpers.make_unit(direction_construction)))
self.Body.changeLocalLocation(new_location)
return True
else:
direction_construction = helpers.make_vector(
self.Body.getLocation(), CONSTRUCTION['center'])
new_location = helpers.sum_vectors(self.Body.getLocation(),
direction_construction)
self.Body.changeLocalLocation(new_location)
return True
def pickup_beams(self, num=ROBOT["beam_capacity"]):
"""
Pickup beams by adding weight to the robot and by adding num to number
carried
"""
self.num_beams = self.num_beams + num
self.weight = self.weight + MATERIAL["beam_load"] * num
# Set the direction towards the structure
self.ground_direction = helpers.make_vector(self.location, CONSTRUCTION["center"])
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 getMomentMagnitudes(self, name, pivot=None):
'''
Returns the moment magnitudes (m11,m22,m33) for the local axes u1,u2,u3 at
the output station closest to the pivot. If there is no pivot, it returns
the values from the output station closest to the robot's location.
'''
# So we can modify the pivot whenever we call the fuction
pivot = self.location if pivot is None else pivot
# Format (ret[0], number_results[1], obj_names[2], i_end distances[3],
# elm_names[4], elm_dist[5], load_cases[6], step_types[7], step_nums[8],
# Ps[9], V2s[10], V3s[11], Ts[12], M2s[13], M3s[14]
results = self.model.Results.FrameForce(name, 0)
if results[0] != 0:
# pdb.set_trace()
helpers.check(results[0],
self,
"getting frame forces",
results=results,
state=self.currentState())
return 0
# Find index of closest data_point
close_index, i = 0, 0
shortest_distance = None
distances = results[3]
for i_distance in distances:
# Get beam endpoints to calculate global position of moment
i_end, j_end = self.structure.get_endpoints(name, self.location)
beam_direction = helpers.make_unit(
helpers.make_vector(i_end, j_end))
point = helpers.sum_vectors(
i_end, helpers.scale(i_distance, beam_direction))
distance = helpers.distance(pivot, point)
# If closer than the current closes point, update information
if shortest_distance is None or distance < shortest_distance:
close_index = i
shortest_distance = distance
i += 1
# Make sure index is indexable
assert close_index < results[1]
# Now that we have the closest moment, calculate sqrt(m2^2+m3^2)
m11 = results[12][close_index]
m22 = results[13][close_index]
m33 = results[14][close_index]
return m11, m22, m33
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 move(self, angle='random', step=10):
def random_NWSE():
rand = randint(0,3)
if rand == 0: return 90 #forward
if rand == 1: return 180 #left
if rand == 2: return 270 #backward
if rand == 3: return 0 #right
if angle == 'NWSE': angle = random_NWSE()
if angle == 'random': angle = random()*360
rad = radians(angle)
direction = helpers.make_vector((0,0,0),(cos(rad),sin(rad),0))
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
step, helpers.make_unit(direction)))
self.Body.changeLocalLocation(new_location)
return True
def get_preferred_direction(self, beam):
'''
Returns the preferred direction - this is the direction towards which the
robot wants to move when looking for an already set support tube.
The direction is a unit vector
'''
# Calculate direction of repair (check 0 dist, which means it is perfectly
# vertical!)
i, j = beam.endpoints.i, beam.endpoints.j
v1 = helpers.make_vector(self.location, j)
v2 = helpers.make_vector(i, self.location)
l1, l2 = helpers.length(v1), helpers.length(v2)
# v1 is non-zero and it is not vertical
if not (helpers.compare(l1, 0) or helpers.is_vertical(v1)):
return helpers.make_unit(v1)
# v2 is non-zero and it is not vertical
elif not (helpers.compare(l2, 0) or helpers.is_vertical(v2)):
return helpers.make_unit(v2)
# No preferred direction because the beam is perfectly vertical
else:
return None
def get_preferred_direction(self,beam):
'''
Returns the preferred direction - this is the direction towards which the
robot wants to move when looking for an already set support tube.
The direction is a unit vector
'''
# Calculate direction of repair (check 0 dist, which means it is perfectly
# vertical!)
i, j = beam.endpoints.i, beam.endpoints.j
v1 = helpers.make_vector(self.location,j)
v2 = helpers.make_vector(i,self.location)
l1,l2 = helpers.length(v1), helpers.length(v2)
# v1 is non-zero and it is not vertical
if not (helpers.compare(l1,0) or helpers.is_vertical(v1)):
return helpers.make_unit(v1)
# v2 is non-zero and it is not vertical
elif not (helpers.compare(l2,0) or helpers.is_vertical(v2)):
return helpers.make_unit(v2)
# No preferred direction because the beam is perfectly vertical
else:
return None
def getMomentMagnitudes(self,name,pivot=None):
'''
Returns the moment magnitudes (m11,m22,m33) for the local axes u1,u2,u3 at
the output station closest to the pivot. If there is no pivot, it returns
the values from the output station closest to the robot's location.
'''
# So we can modify the pivot whenever we call the fuction
pivot = self.location if pivot is None else pivot
# Format (ret[0], number_results[1], obj_names[2], i_end distances[3],
# elm_names[4], elm_dist[5], load_cases[6], step_types[7], step_nums[8],
# Ps[9], V2s[10], V3s[11], Ts[12], M2s[13], M3s[14]
results = self.model.Results.FrameForce(name,0)
if results[0] != 0:
# pdb.set_trace()
helpers.check(results[0],self,"getting frame forces",results=results,
state=self.currentState())
return 0
# Find index of closest data_point
close_index, i = 0, 0
shortest_distance = None
distances = results[3]
for i_distance in distances:
# Get beam endpoints to calculate global position of moment
i_end,j_end = self.structure.get_endpoints(name,self.location)
beam_direction = helpers.make_unit(helpers.make_vector(i_end,j_end))
point = helpers.sum_vectors(i_end,helpers.scale(i_distance,
beam_direction))
distance = helpers.distance(pivot,point)
# If closer than the current closes point, update information
if shortest_distance is None or distance < shortest_distance:
close_index = i
shortest_distance = distance
i += 1
# Make sure index is indexable
assert close_index < results[1]
# Now that we have the closest moment, calculate sqrt(m2^2+m3^2)
m11 = results[12][close_index]
m22 = results[13][close_index]
m33 = results[14][close_index]
return m11,m22,m33
def move(self, angle='random', step=10):
def random_NWSE():
rand = randint(0, 3)
if rand == 0: return 90 #forward
if rand == 1: return 180 #left
if rand == 2: return 270 #backward
if rand == 3: return 0 #right
if angle == 'NWSE': angle = random_NWSE()
if angle == 'random': angle = random() * 360
rad = radians(angle)
direction = helpers.make_vector((0, 0, 0), (cos(rad), sin(rad), 0))
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
step, helpers.make_unit(direction)))
self.Body.changeLocalLocation(new_location)
return True
def go_home_and_pick_up_beam(self, num_beams = ROBOT['beam_capacity']):
self.Body.beam = None
if not self.Body.atHome():
direction_home = helpers.make_vector(self.Body.getLocation(), HOME['center'])
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
self.Body.step, helpers.make_unit(direction_home)))
self.Body.changeLocalLocation(new_location)
else:
self.Body.pickupBeams(num_beams)
self.Body.addToMemory('wandering',-1)
self.Body.addToMemory('density_decisions', 0)
self.Body.addToMemory('climbing_back', 0)
self.Body.addToMemory('current_beam', None)
self.Body.addToMemory('prev_beam', None)
self.Body.addToMemory('same_loc_count', 0)
self.Body.addToMemory('stuck', False)
return True
def go_home_and_pick_up_beam(self, num_beams=ROBOT['beam_capacity']):
self.Body.beam = None
if not self.Body.atHome():
direction_home = helpers.make_vector(self.Body.getLocation(),
HOME['center'])
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
self.Body.step, helpers.make_unit(direction_home)))
self.Body.changeLocalLocation(new_location)
else:
self.Body.pickupBeams(num_beams)
self.Body.addToMemory('wandering', -1)
self.Body.addToMemory('density_decisions', 0)
self.Body.addToMemory('climbing_back', 0)
self.Body.addToMemory('current_beam', None)
self.Body.addToMemory('prev_beam', None)
self.Body.addToMemory('same_loc_count', 0)
self.Body.addToMemory('stuck', False)
return True
def add_beam(self,name,i,j):
'''
Visualization for the wiggling effect when adding a beam to the structure
'''
scale = 1
change = 1
unit_axis = helpers.make_unit(helpers.make_vector(i,j))
# Create the beam
self.beams[name] = visual.cylinder(pos=i,axis=unit_axis,
radius=MATERIAL['outside_diameter'],color=(0,1,0))
# Extrude the beam from the robot
while scale <= BEAM['length']:
axis = helpers.scale(scale,unit_axis)
self.beams[name].axis = axis
scale += change
time.sleep((change / 120) * self.inverse_speed)
def add_beam(self, name, i, j):
'''
Visualization for the wiggling effect when adding a beam to the structure
'''
scale = 1
change = 1
unit_axis = helpers.make_unit(helpers.make_vector(i, j))
# Create the beam
self.beams[name] = visual.cylinder(pos=i,
axis=unit_axis,
radius=MATERIAL['outside_diameter'],
color=(0, 1, 0))
# Extrude the beam from the robot
while scale <= BEAM['length']:
axis = helpers.scale(scale, unit_axis)
self.beams[name].axis = axis
scale += change
time.sleep((change / 120) * self.inverse_speed)
def climb_off(self, loc):
"""
Returns whether or not the robot should climb off the structure. Additionally,
sets some special variables
"""
# On the xy-plane with no beams OR repairing
if helpers.compare(loc[2], 0) and (self.num_beams == 0 or self.search_mode):
# Not repairing, so calculate direction
if not self.search_mode:
direction = helpers.make_vector(self.location, HOME["center"])
direction = (direction[0], direction[1], 0)
self.ground_direction = direction
return True
else:
# Resetting to None if not in search_mode
self.ground_direction = None if not self.search_mode else self.ground_direction
return False
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 add_beam(self, p1, p1_name, p2, p2_name, name):
'''
Function to add the name and endpoint combination of a beam
to all of the boxes that contain it. Returns the number of boxes (which
should be at least 1)
'''
def addbeam(beam, p):
'''
Function to add an arbitrary beam to its respective box. Returns number of
boxes changed. Also takes care of finding intersecting beams to the added
beam and adding the joints to both beams. Uses the point p (which should
be on the beam), to calculate the box it should be added to
'''
# Getting indeces
xi, yi, zi = self.__get_indeces(p)
# Getting the box and all of the other beams in the box
try:
box = self.model[xi][yi][zi]
except IndexError:
print("Addbeam is incorrect. Accessing box not defined.")
return False
# Finding intersection points with other beams
for key in box:
point = helpers.intersection(box[key].endpoints,
beam.endpoints)
# If they intersect, add the joint to both beams
if point != None:
assert key == box[key].name
if not beam.addjoint(point, box[key]):
sys.exit("Could not add joint to {} at {}".format(
beam.name, str(point)))
if not box[key].addjoint(point, beam):
sys.exit("Could not add joint to {} at {}".format(
box[key].name, str(point)))
# update the box
self.model[xi][yi][zi] = box
# Adding beam to boxes that contain it based on the point p.
try:
if beam.name in self.model[xi][yi][zi]:
return 0
else:
self.model[xi][yi][zi][beam.name] = beam
return 1
except IndexError:
raise OutofBox(
"The coordinate {}, is not in the structure. Something\
went wront in addpoint()".format(p))
# Create the beam
new_beam = Beam(name, (p1, p2), (p1_name, p2_name))
# Add to all boxes it is located in
total_boxes = 0
try:
for point in self.__path(p1, p2):
total_boxes += addbeam(new_beam, point)
except OutofBox as e:
print(e)
return False
# If something went wrong, kill the program
assert total_boxes > 0
# If showing the visualization, add the cylinder to the structure
if self.visualization:
temp = visual.cylinder(pos=p1,
axis=helpers.make_vector(p1, p2),
radius=MATERIAL['outside_diameter'])
temp.color = (0, 1, 1)
# Safe visualization data
self.visualization_data += "{}:{}-{}<>".format(
str(new_beam.name), str(helpers.round_tuple(p1, 3)),
str(helpers.round_tuple(p2, 3)))
# Add a beam to the structure count and increase height if necessary
self.tubes += 1
self.height = max(p1[2], p2[2], self.height)
return total_boxes
def get_build_vector(self, build_angle, direction):
current_beam_direction = self.Body.beam.global_default_axes()[0]
(x_dir, y_dir, z_dir) = current_beam_direction
(x, y, z) = (0, 0, 0)
# convert to complement of polar angle (=angle up from x-y)
build_angle = radians(90 - build_angle)
#direction = None
'''
#connect to nearby beam if possible.
best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
height = -1*sin(build_angle)
radius = cos(build_angle)
random_angle = radians(random()*360)
for theta in range(0,360,1):
rads = radians(theta)
x, y, z = radius*cos(random_angle+rads), radius*sin(random_angle+rads), height
x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
BEAM['length'], helpers.make_unit((x,y,z))))
if z_r <= 0: return (x,y,z)
if z_r < best_z: best_x, best_y, best_z = x, y, z
return (x,y,z)
'''
if direction == None:
random_angle = radians(random()*360)
height = sin(build_angle)
radius = cos(build_angle)
x, y, z = radius*cos(random_angle), radius*sin(random_angle), height
print(x,y,z)
print(current_beam_direction)
if direction == 'center':
height = sin(build_angle)
radius = cos(build_angle)
position_center = CONSTRUCTION['center']
position_center = (position_center[0], \
position_center[1], self.Body.getLocation()[2])
direction_construction = helpers.make_vector(self.Body.getLocation(), position_center)
endpoint = helpers.scale(radius, helpers.make_unit(direction_construction))
x, y, z = endpoint[0], endpoint[1], height
if direction == 'outward':
height = sin(build_angle)
radius = cos(build_angle)
position_center = CONSTRUCTION['center']
position_center = (position_center[0], \
position_center[1], self.Body.getLocation()[2])
direction_construction = helpers.make_vector(self.Body.getLocation(), position_center)
endpoint = helpers.scale(radius, helpers.make_unit(direction_construction))
x, y, z = -1*endpoint[0], -1*endpoint[1], height
if direction == 'upwards':
x, y, z = 0, 0, 1
if direction == 'ground':
best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
height = -1*sin(build_angle)
radius = cos(build_angle)
random_angle = radians(random()*360)
for theta in range(0,360,1):
rads = radians(theta)
x, y, z = radius*cos(random_angle+rads), radius*sin(random_angle+rads), height
x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
BEAM['length'], helpers.make_unit((x,y,z))))
if z_r <= 0: return (x,y,z)
if z_r < best_z: best_x, best_y, best_z = x, y, z
return (x,y,z)
x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
# prevent hanging beams that are just above the ground after rotation
if direction == 'outward' or direction == 'center' and z < 0:
z = -1*z
return (x, y, z)
def run(self,fullscreen = True, inverse_speed=.25):
if self.data == []:
print("No data has been loaded. Cannot run simulation.")
else:
# Store inverse speed
self.inverse_speed = inverse_speed
# Setup the scene
scene = setup_scene(fullscreen)
# Setup basic
setup_base()
# Cycle through timestep data
timestep = 1
for swarm_step, swarm_color,structure_step,struct_color in self.data:
for name, locations in swarm_step:
# Create the object
if name not in self.workers:
self.workers[name] = visual.sphere(pos=locations[0],
radius=VISUALIZATION['robot_size']/2,make_trail=False)
self.workers[name].color = (1,0,1)
# Change the objects position
else:
self.workers[name].pos = locations[0]
# Set the color
for name, colors in swarm_color:
self.workers[name].color = colors[0]
# Add beams if any
for name,coords in structure_step:
i,j = coords
# Add new beam if not in dictionary
if name not in self.beams:
self.add_beam(name,i,j)
# Otherwise, this means the beam has deflected, so change the position
else:
# Scale visualization
scale = VISUALIZATION['scaling']
# Old endpoints (we use this to scale at different values each time
# we run)
old_i = self.beams[name].pos
old_j = helpers.sum_vectors(self.beams[name].pos,self.beams[name].axis)
# Calculate changes from old location to new location
i_change = helpers.scale(scale,helpers.make_vector(old_i,i))
j_change = helpers.scale(scale,helpers.make_vector(old_j,j))
# Calculate the new location based on the scaled chage
new_i = helpers.sum_vectors(old_i,i_change)
new_j = helpers.sum_vectors(old_j,j_change)
new_axis = helpers.make_vector(new_i,new_j)
# Update the visualization
self.beams[name].pos = new_i
self.beams[name].axis = new_axis
# Update window dimensions
limit = max(j)
if limit > max(scene.range):
scene.range = (limit,limit,limit)
scene.center = helpers.scale(.5,helpers.sum_vectors(
CONSTRUCTION['corner'],scene.range))
# Change the color of the beams
for name,colors in struct_color:
try:
self.beams[name].color = colors[0]
except IndexError:
print("A nonexistant beam is beam is to be recolored!")
# Check key_presses
if scene.kb.keys:
s = scene.kb.getkey()
if len(s) == 1:
# Move faster
if s == 'f':
inverse_speed /= 2
# Move more slowly
elif s == 's':
inverse_speed *= 2
# Pause or continue
elif s == ' ':
self.pause(scene)
else:
pass
time.sleep(inverse_speed)
timestep += 1
def __path(self, coord1, coord2):
'''
Traverses the line formed between coord1 and coord2. Returns a list of
points on the line that lie in different boxes. This will NOT miss any
points that are in different boxes. The basic method is to find the
intersection of the line with one of the faces of the cube formed by the
box. There should not be multiple points, but this has not been proven. It
might return two points that are in the same box.
'''
def get_sign(n):
'''
Returns the sign of the number
'''
if n == 0:
return None
else:
return n > 0
# move from coord1 to coord2. Here, we determine the sign of the change
# (pos = True, neg = False, or None)
signs = (get_sign(coord2[0] - coord1[0]),
get_sign(coord2[1] - coord1[1]),
get_sign(coord2[2] - coord1[2]))
line = helpers.make_vector(coord1, coord2)
def crawl(point):
# get the current box boundaries (bottom left corner-(0,0,0) is starting)
# and coordinates
xi, yi, zi = self.__get_indeces(point)
bounds = xi * self.box_size[0], yi * self.box_size[
1], zi * self.box_size[2]
# This is defined here to have access to the above signs and bounds
def closest(p):
'''
Returns which coordinate in p is closest to the boundary of a box
(x = 0, y = 1, z = 2) if moving along the line, and the absolute change
in that coordinate.
'''
def distance(i):
'''
i is 0,1,2 for x,y,z
'''
if signs[i] == None:
# This will never be the minimum. Makes later code easier
return None
elif signs[i]:
return abs(p[i] - (bounds[i] + self.box_size[i]))
else:
return abs(p[i] - bounds[i])
# Find the shortest time distance (ie, distance/velocity)
index = None
for i in range(3):
dist, vel = distance(i), abs(line[i])
if dist is not None and vel != 0:
if index is None:
index = i
else:
min_time = distance(index) / abs(line[index])
index = i if dist / vel < min_time else index
return index, distance(index)
# In crawl, we obtain the coordinate closests to an edge (index), and its
# absolute distance from that edge
index, distance = closest(point)
# The change is the line scaled so that the right coordinate changes the
# amount necessary to cross into the next box. This means that we scale it
# and also add a teeny bit so as to push it into the right box. This is
# the scaled version, exactly the distance we need to move
move = helpers.scale(distance / abs(line[index]), line)
# Here we scale the line again by epsilon/2. This is our push
push = helpers.scale(PROGRAM['epsilon'] / 2, line)
# The total change is the addition of these two
change = helpers.sum_vectors(move, push)
# make sure we are changing the right amount
assert helpers.compare(abs(move[index]), distance)
# The new initial coordinate in the next box
new_point = helpers.sum_vectors(point, change)
return new_point
points, passed, temp = [coord2], False, coord1
while not passed:
points.append(temp)
temp = crawl(temp)
# Check the next coordinate to see if we have moved past the endpoint
for i in range(3):
if signs[i] != None:
# Movings positively, so set to True if our new_point has a larger
# positive coordinate
# Moving negatively, so set to True if our new_point has a smaller
# positive coordinate
passed = (temp[i] > coord2[i] + PROGRAM['epsilon'] / 2
if signs[i] else
temp[i] < coord2[i] - PROGRAM['epsilon'] / 2)
return points
def get_walkable_directions(self, box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location, joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist, 0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,
e1), helpers.make_vector(
self.location, e2)
# We don't want to include zero-vectors
bool_v1, bool_v2 = (
not helpers.compare(helpers.length(v1), 0),
not helpers.compare(helpers.length(v2), 0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([
helpers.make_vector(self.location, e1),
helpers.make_vector(self.location, e2)
])
elif bool_v1:
crawlable[beam.name] = [
helpers.make_vector(self.location, e1)
]
elif bool_v2:
crawlable[beam.name] = [
helpers.make_vector(self.location, e2)
]
else:
raise Exception(
"All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location, coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step
or helpers.compare(length, self.step))
and not helpers.compare(length, 0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception(
"Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print(
"The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name, self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(
helpers.make_vector(self.location, joint))
else:
crawlable[self.beam.name] = [
helpers.make_vector(self.location, joint)
]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location, self.beam.endpoints.i),
helpers.make_vector(self.location, self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1), 0)
b_v2 = not helpers.compare(helpers.length(v2), 0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1, v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,
v1) and helpers.dot(direct, v1) > 0:
bool_v1 = False
if helpers.parallel(direct,
v2) and helpers.dot(direct, v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
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 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_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 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 get_build_vector(self, build_angle, direction):
current_beam_direction = self.Body.beam.global_default_axes()[0]
(x_dir, y_dir, z_dir) = current_beam_direction
(x, y, z) = (0, 0, 0)
# convert to complement of polar angle (=angle up from x-y)
build_angle = radians(90 - build_angle)
#direction = None
'''
#connect to nearby beam if possible.
best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
height = -1*sin(build_angle)
radius = cos(build_angle)
random_angle = radians(random()*360)
for theta in range(0,360,1):
rads = radians(theta)
x, y, z = radius*cos(random_angle+rads), radius*sin(random_angle+rads), height
x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
BEAM['length'], helpers.make_unit((x,y,z))))
if z_r <= 0: return (x,y,z)
if z_r < best_z: best_x, best_y, best_z = x, y, z
return (x,y,z)
'''
if direction == None:
random_angle = radians(random() * 360)
height = sin(build_angle)
radius = cos(build_angle)
x, y, z = radius * cos(random_angle), radius * sin(
random_angle), height
print(x, y, z)
print(current_beam_direction)
if direction == 'center':
height = sin(build_angle)
radius = cos(build_angle)
position_center = CONSTRUCTION['center']
position_center = (position_center[0], \
position_center[1], self.Body.getLocation()[2])
direction_construction = helpers.make_vector(
self.Body.getLocation(), position_center)
endpoint = helpers.scale(radius,
helpers.make_unit(direction_construction))
x, y, z = endpoint[0], endpoint[1], height
if direction == 'outward':
height = sin(build_angle)
radius = cos(build_angle)
position_center = CONSTRUCTION['center']
position_center = (position_center[0], \
position_center[1], self.Body.getLocation()[2])
direction_construction = helpers.make_vector(
self.Body.getLocation(), position_center)
endpoint = helpers.scale(radius,
helpers.make_unit(direction_construction))
x, y, z = -1 * endpoint[0], -1 * endpoint[1], height
if direction == 'upwards':
x, y, z = 0, 0, 1
if direction == 'ground':
best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
height = -1 * sin(build_angle)
radius = cos(build_angle)
random_angle = radians(random() * 360)
for theta in range(0, 360, 1):
rads = radians(theta)
x, y, z = radius * cos(random_angle +
rads), radius * sin(random_angle +
rads), height
x, y, z = helpers.rotate_vector_3D((x, y, z),
current_beam_direction)
x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
BEAM['length'], helpers.make_unit((x,y,z))))
if z_r <= 0: return (x, y, z)
if z_r < best_z: best_x, best_y, best_z = x, y, z
return (x, y, z)
x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
# prevent hanging beams that are just above the ground after rotation
if direction == 'outward' or direction == 'center' and z < 0:
z = -1 * z
return (x, y, z)
def __path(self,coord1, coord2):
'''
Traverses the line formed between coord1 and coord2. Returns a list of
points on the line that lie in different boxes. This will NOT miss any
points that are in different boxes. The basic method is to find the
intersection of the line with one of the faces of the cube formed by the
box. There should not be multiple points, but this has not been proven. It
might return two points that are in the same box.
'''
def get_sign(n):
'''
Returns the sign of the number
'''
if n == 0:
return None
else:
return n > 0
# move from coord1 to coord2. Here, we determine the sign of the change
# (pos = True, neg = False, or None)
signs = (get_sign(coord2[0] - coord1[0]), get_sign(coord2[1] - coord1[1]),
get_sign(coord2[2] - coord1[2]))
line = helpers.make_vector(coord1,coord2)
def crawl(point):
# get the current box boundaries (bottom left corner-(0,0,0) is starting)
# and coordinates
xi, yi, zi = self.__get_indeces(point)
bounds = xi*self.box_size[0], yi*self.box_size[1], zi*self.box_size[2]
# This is defined here to have access to the above signs and bounds
def closest(p):
'''
Returns which coordinate in p is closest to the boundary of a box
(x = 0, y = 1, z = 2) if moving along the line, and the absolute change
in that coordinate.
'''
def distance(i):
'''
i is 0,1,2 for x,y,z
'''
if signs[i] == None:
# This will never be the minimum. Makes later code easier
return None
elif signs[i]:
return abs(p[i] - (bounds[i] + self.box_size[i]))
else:
return abs(p[i] - bounds[i])
# Find the shortest time distance (ie, distance/velocity)
index = None
for i in range(3):
dist, vel = distance(i), abs(line[i])
if dist is not None and vel != 0:
if index is None:
index = i
else:
min_time = distance(index) / abs(line[index])
index = i if dist / vel < min_time else index
return index, distance(index)
# In crawl, we obtain the coordinate closests to an edge (index), and its
# absolute distance from that edge
index, distance = closest(point)
# The change is the line scaled so that the right coordinate changes the
# amount necessary to cross into the next box. This means that we scale it
# and also add a teeny bit so as to push it into the right box. This is
# the scaled version, exactly the distance we need to move
move = helpers.scale(distance / abs(line[index]), line)
# Here we scale the line again by epsilon/2. This is our push
push = helpers.scale(PROGRAM['epsilon'] / 2, line)
# The total change is the addition of these two
change = helpers.sum_vectors(move,push)
# make sure we are changing the right amount
assert helpers.compare(abs(move[index]), distance)
# The new initial coordinate in the next box
new_point = helpers.sum_vectors(point,change)
return new_point
points, passed,temp = [coord2], False, coord1
while not passed:
points.append(temp)
temp = crawl(temp)
# Check the next coordinate to see if we have moved past the endpoint
for i in range(3):
if signs[i] != None:
# Movings positively, so set to True if our new_point has a larger
# positive coordinate
# Moving negatively, so set to True if our new_point has a smaller
# positive coordinate
passed = (temp[i] > coord2[i] + PROGRAM['epsilon'] / 2 if signs[i]
else temp[i] < coord2[i] - PROGRAM['epsilon'] / 2)
return points
def run(self, fullscreen=True, inverse_speed=.25):
if self.data == []:
print("No data has been loaded. Cannot run simulation.")
else:
# Store inverse speed
self.inverse_speed = inverse_speed
# Setup the scene
scene = setup_scene(fullscreen)
# Setup basic
setup_base()
# Cycle through timestep data
timestep = 1
for swarm_step, swarm_color, structure_step, struct_color in self.data:
for name, locations in swarm_step:
# Create the object
if name not in self.workers:
self.workers[name] = visual.sphere(
pos=locations[0],
radius=VISUALIZATION['robot_size'] / 2,
make_trail=False)
self.workers[name].color = (1, 0, 1)
# Change the objects position
else:
self.workers[name].pos = locations[0]
# Set the color
for name, colors in swarm_color:
self.workers[name].color = colors[0]
# Add beams if any
for name, coords in structure_step:
i, j = coords
# Add new beam if not in dictionary
if name not in self.beams:
self.add_beam(name, i, j)
# Otherwise, this means the beam has deflected, so change the position
else:
# Scale visualization
scale = VISUALIZATION['scaling']
# Old endpoints (we use this to scale at different values each time
# we run)
old_i = self.beams[name].pos
old_j = helpers.sum_vectors(self.beams[name].pos,
self.beams[name].axis)
# Calculate changes from old location to new location
i_change = helpers.scale(scale,
helpers.make_vector(old_i, i))
j_change = helpers.scale(scale,
helpers.make_vector(old_j, j))
# Calculate the new location based on the scaled chage
new_i = helpers.sum_vectors(old_i, i_change)
new_j = helpers.sum_vectors(old_j, j_change)
new_axis = helpers.make_vector(new_i, new_j)
# Update the visualization
self.beams[name].pos = new_i
self.beams[name].axis = new_axis
# Update window dimensions
limit = max(j)
if limit > max(scene.range):
scene.range = (limit, limit, limit)
scene.center = helpers.scale(
.5,
helpers.sum_vectors(CONSTRUCTION['corner'],
scene.range))
# Change the color of the beams
for name, colors in struct_color:
try:
self.beams[name].color = colors[0]
except IndexError:
print("A nonexistant beam is beam is to be recolored!")
# Check key_presses
if scene.kb.keys:
s = scene.kb.getkey()
if len(s) == 1:
# Move faster
if s == 'f':
inverse_speed /= 2
# Move more slowly
elif s == 's':
inverse_speed *= 2
# Pause or continue
elif s == ' ':
self.pause(scene)
else:
pass
time.sleep(inverse_speed)
timestep += 1
def add_beam(self,p1,p1_name,p2,p2_name,name):
'''
Function to add the name and endpoint combination of a beam
to all of the boxes that contain it. Returns the number of boxes (which
should be at least 1)
'''
def addbeam(beam,p):
'''
Function to add an arbitrary beam to its respective box. Returns number of
boxes changed. Also takes care of finding intersecting beams to the added
beam and adding the joints to both beams. Uses the point p (which should
be on the beam), to calculate the box it should be added to
'''
# Getting indeces
xi,yi,zi = self.__get_indeces(p)
# Getting the box and all of the other beams in the box
try:
box = self.model[xi][yi][zi]
except IndexError:
print ("Addbeam is incorrect. Accessing box not defined.")
return False
# Finding intersection points with other beams
for key in box:
point = helpers.intersection(box[key].endpoints, beam.endpoints)
# If they intersect, add the joint to both beams
if point != None:
assert key == box[key].name
if not beam.addjoint(point, box[key]):
sys.exit("Could not add joint to {} at {}".format(beam.name,
str(point)))
if not box[key].addjoint(point, beam):
sys.exit("Could not add joint to {} at {}".format(box[key].name,
str(point)))
# update the box
self.model[xi][yi][zi] = box
# Adding beam to boxes that contain it based on the point p.
try:
if beam.name in self.model[xi][yi][zi]:
return 0
else:
self.model[xi][yi][zi][beam.name] = beam
return 1
except IndexError:
raise OutofBox ("The coordinate {}, is not in the structure. Something\
went wront in addpoint()".format(p))
# Create the beam
new_beam = Beam(name,(p1,p2),(p1_name,p2_name))
# Add to all boxes it is located in
total_boxes = 0
try:
for point in self.__path(p1, p2):
total_boxes += addbeam(new_beam,point)
except OutofBox as e:
print (e)
return False
# If something went wrong, kill the program
assert total_boxes > 0
# If showing the visualization, add the cylinder to the structure
if self.visualization:
temp = visual.cylinder(pos=p1,axis=helpers.make_vector(p1,p2),
radius=MATERIAL['outside_diameter'])
temp.color = (0,1,1)
# Safe visualization data
self.visualization_data += "{}:{}-{}<>".format(str(new_beam.name),str(
helpers.round_tuple(p1,3)),str(helpers.round_tuple(p2,3)))
# Add a beam to the structure count and increase height if necessary
self.tubes += 1
self.height = max(p1[2],p2[2],self.height)
return total_boxes
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 get_walkable_directions(self,box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location,joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist,0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,e1), helpers.make_vector(
self.location,e2)
# We don't want to include zero-vectors
bool_v1,bool_v2 = (not helpers.compare(helpers.length(v1),0),
not helpers.compare(helpers.length(v2),0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([helpers.make_vector(self.location,e1),
helpers.make_vector(self.location,e2)])
elif bool_v1:
crawlable[beam.name] = [helpers.make_vector(self.location,e1)]
elif bool_v2:
crawlable[beam.name] = [helpers.make_vector(self.location,e2)]
else:
raise Exception("All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location,coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step or helpers.compare(length, self.step))
and not helpers.compare(length,0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception("Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print ("The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name,self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(helpers.make_vector(self.location,
joint))
else:
crawlable[self.beam.name] = [helpers.make_vector(self.location,joint)]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location,self.beam.endpoints.i),
helpers.make_vector(self.location,self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1),0)
b_v2 = not helpers.compare(helpers.length(v2),0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1,v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,v1) and helpers.dot(direct,v1) > 0:
bool_v1 = False
if helpers.parallel(direct,v2) and helpers.dot(direct,v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
def wander(self):
"""
When a robot is not on a structure, it wanders. The wandering in the working
class works as follows. The robot moves around randomly with the following
restrictions:
The robot moves towards the home location if it has no beams and
the home location is detected nearby.
Otherwise, if it has beams for construction, it moves toward the base
specified construction site. If it finds another beam nearby, it has a
tendency to climb that beam instead.
"""
# Check to see if robot is at home location and has no beams
if self.at_home() and self.num_beams == 0:
self.pickup_beams()
# If we have no beams, set the ground direction to home (TEMP CODE)
if self.num_beams == 0:
vector = helpers.make_vector(self.location, HOME["center"])
self.ground_direction = (
vector if not helpers.compare(helpers.length(vector), 0) else self.non_zero_xydirection()
)
# Find nearby beams to climb on
result = self.ground()
# Either there are no nearby beams, we are on repair_mode/search_mode, our beams are 0, or
# we are constructing a support - so don't mess with direction
if (
result == None
or self.repair_mode
or self.search_mode
or self.num_beams == 0
or self.memory["construct_support"]
):
direction = self.get_ground_direction()
new_location = helpers.sum_vectors(self.location, helpers.scale(self.step, helpers.make_unit(direction)))
self.change_location_local(new_location)
# Nearby beam, jump on it
else:
dist, close_beam, direction = (result["distance"], result["beam"], result["direction"])
# If the beam is within steping distance, just jump on it
if self.num_beams > 0 and dist <= self.step:
# Set the ground direction to None (so we walk randomly if we do get off
# the beam again)
self.ground_direction = None
# Then move on the beam
self.move(direction, close_beam)
# If we can "detect" a beam, change the ground direction to approach it
elif self.num_beams > 0 and dist <= ROBOT["local_radius"]:
self.ground_direction = direction
new_location = helpers.sum_vectors(
self.location, helpers.scale(self.step, helpers.make_unit(direction))
)
self.change_location_local(new_location)
# Local beams, but could not detect (this is redundant)
else:
direction = self.get_ground_direction()
new_location = helpers.sum_vectors(
self.location, helpers.scale(self.step, helpers.make_unit(direction))
)
self.change_location_local(new_location)
