def get_true_location(self):
'''
Returns the location of the robot on the current beam, accounting for the
deflections that the beam has undergone. Uses the design location and the
deflection data from SAP to calculate this location.
'''
# Not on the structure, no deflection, or not recording deflection
if not self.on_structure(
) or self.beam.deflection is None or not VISUALIZATION['deflection']:
return super(Movable, self).get_true_location()
else:
# Get deflections
i_def, j_def = self.beam.deflection.i, self.beam.deflection.j
# Obtain weight of each scale based on location on beam
i_weight = 1 - (helpers.distance(
self.location, self.beam.endpoints.i) / BEAM['length'])
j_weight = 1 - i_weight
# Sum the two vectors to obtain general deflection
# This sort of addition works under the assumption that the beam ITSELF
# does not deflect significantly
deflection = helpers.sum_vectors(helpers.scale(i_weight, i_def),
helpers.scale(j_weight, j_def))
# Return true location
return helpers.sum_vectors(self.location, deflection)
def get_true_location(self):
'''
Returns the location of the robot on the current beam, accounting for the
deflections that the beam has undergone. Uses the design location and the
deflection data from SAP to calculate this location.
'''
# Not on the structure, no deflection, or not recording deflection
if not self.on_structure() or self.beam.deflection is None or not VISUALIZATION['deflection']:
return super(Movable,self).get_true_location()
else:
# Get deflections
i_def, j_def = self.beam.deflection.i, self.beam.deflection.j
# Obtain weight of each scale based on location on beam
i_weight = 1 - (helpers.distance(self.location,self.beam.endpoints.i) /
BEAM['length'])
j_weight = 1 - i_weight
# Sum the two vectors to obtain general deflection
# This sort of addition works under the assumption that the beam ITSELF
# does not deflect significantly
deflection = helpers.sum_vectors(helpers.scale(i_weight,i_def),helpers.scale(
j_weight,j_def))
# Return true location
return helpers.sum_vectors(self.location,deflection)
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
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
def place_beam(self, direction=None):
# don't make triple joints
if self.Body.atJoint(): return False
pivot = self.Body.getLocation()
# don't place beams with a 2 ft. radius from each other
nearby_beams = self.get_structure_density(
pivot, BConstants.beam['joint_distance'])
if nearby_beams > 1:
print('TOO CLOSE: ' + str(nearby_beams))
self.climb_back(1)
return False
build_angle = BConstants.beam['beam_angle']
end_coordinates = self.get_build_vector(build_angle, direction)
# try to connect to already present beam
endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'],\
helpers.make_unit(end_coordinates)))
density = self.get_structure_density(endpoint)
# location at end of beam you are about to place is too dense,
# so do not place it.
if density > BConstants.beam['max_beam_density']:
print('TOO DENSE: ' + str(density))
density_decisions = self.Body.readFromMemory('density_decisions')
#print('Density Decisions: ', density_decisions)
if density_decisions >= 10:
self.climb_back(2)
return False
#
#elif random() <= (BConstants.prob['build_out']):#**density_decisions):
# end_coordinates = self.get_build_vector(build_angle, 'upwards')
# endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'], \
# helpers.make_unit(end_coordinates)))
#
else:
end_coordinates = self.get_build_vector(build_angle, 'outward')
endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'],\
helpers.make_unit(end_coordinates)))
self.Body.addToMemory('density_decisions', density_decisions + 1)
# prevent beams going into the ground (-z)
if endpoint[2] <= 0:
dx, dy, dz = helpers.make_unit(end_coordinates)
factor = endpoint[2] / dz
dx, dy, dz = dx * factor, dy * factor, dz * factor
pivot = (pivot[0] - dx, pivot[1] - dy, pivot[2] - dz)
endpoint = (endpoint[0] - dx, endpoint[1] - dy, endpoint[2] - dz)
# don't want beams "inside" the beam you are on.
#if helpers.between_points(pivot,endpoint,self.Body.beam.endpoints.i, False) \
#or helpers.between_points(pivot,endpoint,self.Body.beam.endpoints.j, False):
# return False
self.Body.addBeam(pivot, endpoint)
return True
def get_preferred_direction(self, beam):
# Obtain the moment vector
u1, u2, u3 = beam.global_default_axes()
m11, m22, m33 = self.get_moment_magnitudes(beam.name)
# Quick debugging - making sure the torsion doesn't get too high
if not helpers.compare(m11, 0, 4):
#pdb.set_trace()
pass
# Sum m22 and m33 (m11 is torsion, which doesn't play a role in the direction)
moment_vector = helpers.sum_vectors(helpers.scale(m22, u2),
helpers.scale(m33, u3))
'''
Cross axis-1 with the moment vector to obtain the positive clockwise direction
This keeps the overall magnitude of moment_vector since u1 is a unit vector
We are always attempting to repair further up the broken beam (closer to
the j-end). The cross will always give us the vector in the right direction
if we do it this way.
'''
twist_direction = helpers.cross(moment_vector, u1)
# Project the twist direction onto the xy-plane and normalize to have a
# maximum of 45 degree approach (or, as specified in the variables.py)
# depending on the ratio of moment magnitude to max_magnitude
xy_change = (twist_direction[0], twist_direction[1], 0)
# The rotation is parallel to the z-axis, so we don't add disturbance
if helpers.compare(helpers.length(xy_change), 0):
# Return the normal direction - which way is the beam leaning???
return super(MomentAwareBuilder,
self).get_preferred_direction(beam)
# We know the direction in which the beam is turning
else:
# Get direction of travel (this is a unit vector)
travel = super(MomentAwareBuilder,
self).get_preferred_direction(beam)
# Normalize twist to the maximum moment of force -- structure_check
normal = helpers.normalize(xy_change,
BConstants.beam['structure_check'])
# The beam is vertical - check to see how large the normalized moment is
if travel is None:
# The change is relatively small, so ignore it
if helpers.length(normal) <= helpers.ratio(
BConstants.beam['verticality_angle']):
return travel
else:
return helpers.make_unit(normal)
else:
scalar = 1 / helpers.ratio(BConstants.beam['moment_angle_max'])
scaled_travel = helpers.scale(scalar, travel)
return helpesr.make_unit(
helpers.sum_vectors(normal, scaled_travel))
def place_beam(self, direction=None):
# don't make triple joints
if self.Body.atJoint(): return False
pivot = self.Body.getLocation()
# don't place beams with a 2 ft. radius from each other
nearby_beams = self.get_structure_density(pivot, BConstants.beam['joint_distance'])
if nearby_beams > 1:
print('TOO CLOSE: ' + str(nearby_beams))
self.climb_back(1)
return False
build_angle = BConstants.beam['beam_angle']
end_coordinates = self.get_build_vector(build_angle, direction)
# try to connect to already present beam
endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'],\
helpers.make_unit(end_coordinates)))
density = self.get_structure_density(endpoint)
# location at end of beam you are about to place is too dense,
# so do not place it.
if density > BConstants.beam['max_beam_density']:
print('TOO DENSE: ' + str(density))
density_decisions = self.Body.readFromMemory('density_decisions')
#print('Density Decisions: ', density_decisions)
if density_decisions >= 10:
self.climb_back(2)
return False
#
#elif random() <= (BConstants.prob['build_out']):#**density_decisions):
# end_coordinates = self.get_build_vector(build_angle, 'upwards')
# endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'], \
# helpers.make_unit(end_coordinates)))
#
else:
end_coordinates = self.get_build_vector(build_angle, 'outward')
endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'],\
helpers.make_unit(end_coordinates)))
self.Body.addToMemory('density_decisions', density_decisions+1)
# prevent beams going into the ground (-z)
if endpoint[2] <= 0:
dx, dy, dz = helpers.make_unit(end_coordinates)
factor = endpoint[2]/dz
dx, dy, dz = dx*factor, dy*factor, dz*factor
pivot = (pivot[0]-dx, pivot[1]-dy,pivot[2]-dz)
endpoint = (endpoint[0]-dx, endpoint[1]-dy, endpoint[2]-dz)
# don't want beams "inside" the beam you are on.
#if helpers.between_points(pivot,endpoint,self.Body.beam.endpoints.i, False) \
#or helpers.between_points(pivot,endpoint,self.Body.beam.endpoints.j, False):
# return False
self.Body.addBeam(pivot,endpoint)
return True
def get_true_endpoints(self):
'''
Takes into account the deflection and returns the true physical endpoints of
the structure
'''
if self.deflection is None:
return self.endpoints
else:
return EndPoints(i=helpers.sum_vectors(self.endpoints.i,self.deflection.i),
j=helpers.sum_vectors(self.endpoints.j,self.deflection.j))
def get_true_endpoints(self):
'''
Takes into account the deflection and returns the true physical endpoints of
the structure
'''
if self.deflection is None:
return self.endpoints
else:
return EndPoints(i=helpers.sum_vectors(self.endpoints.i,
self.deflection.i),
j=helpers.sum_vectors(self.endpoints.j,
self.deflection.j))
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 move(self, direction, beam):
'''
Moves the robot in direction passed in and onto the beam specified
'''
length = helpers.length(direction)
# The direction is smaller than the determined step, so move exactly by
# direction
if length < self.step:
new_location = helpers.sum_vectors(self.location, direction)
self.change_location(new_location, beam)
# call do_action again since we still have some distance left, and update
# step to reflect how much distance is left to cover
self.step = self.step - length
# Reset step in preparation for next timestep
if helpers.compare(self.step, 0):
self.step == ROBOT['step_length']
# We still have steps to go, so run an analysis if necessary
elif self.beam is not None:
# Run analysis before deciding to get the next direction
if not self.model.GetModelIsLocked() and self.need_data():
errors = helpers.run_analysis(self.model)
if errors != '':
# pdb.set_trace()
pass
# Get next direction
self.next_direction_info = self.get_direction()
# Unlock the results so that we can actually move
if self.model.GetModelIsLocked():
self.model.SetModelIsLocked(False)
# Move
self.do_action()
# We climbed off
else:
assert not self.model.GetModelIsLocked()
self.do_action()
# The direction is larger than the usual step, so move only the step in the
# specified direction
else:
movement = helpers.scale(self.step, helpers.make_unit(direction))
new_location = helpers.sum_vectors(self.location, movement)
self.change_location(new_location, beam)
def move(self, direction, beam):
'''
Moves the robot in direction passed in and onto the beam specified
'''
length = helpers.length(direction)
# The direction is smaller than the determined step, so move exactly by
# direction
if length < self.step:
new_location = helpers.sum_vectors(self.location, direction)
self.change_location(new_location, beam)
# call do_action again since we still have some distance left, and update
# step to reflect how much distance is left to cover
self.step = self.step - length
# Reset step in preparation for next timestep
if helpers.compare(self.step,0):
self.step == ROBOT['step_length']
# We still have steps to go, so run an analysis if necessary
elif self.beam is not None:
# Run analysis before deciding to get the next direction
if not self.model.GetModelIsLocked() and self.need_data():
errors = helpers.run_analysis(self.model)
if errors != '':
# pdb.set_trace()
pass
# Get next direction
self.next_direction_info = self.get_direction()
# Unlock the results so that we can actually move
if self.model.GetModelIsLocked():
self.model.SetModelIsLocked(False)
# Move
self.do_action()
# We climbed off
else:
assert not self.model.GetModelIsLocked()
self.do_action()
# The direction is larger than the usual step, so move only the step in the
# specified direction
else:
movement = helpers.scale(self.step, helpers.make_unit(direction))
new_location = helpers.sum_vectors(self.location, movement)
self.change_location(new_location, beam)
def climb(self, location, beam):
length = helpers.length(location)
if length <= self.Body.step:
new_location = helpers.sum_vectors(self.Body.getLocation(), location)
if helpers.compare(new_location[2], 0):
beam = None
else: print('climbing beam', beam.name)
else:
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
self.Body.step, helpers.make_unit(location)))
print('climbing beam', beam.name)
self.Body.model.SetModelIsLocked(False)
self.Body.changeLocationOnStructure(new_location, beam)
return True
def get_deflection(joint_name):
'''
Returns the correct displacement in absolute coordinates for the named
joint
'''
# Get displacements
results = program.model.Results.JointDisplAbs(joint_name,0)
if results[0] != 0:
#pdb.set_trace()
return (0,0,0)
u1,u2,u3 = results[7][0], results[8][0], results[9][0]
# Return the total deflection based on the local axes
return helpers.sum_vectors(helpers.scale(u1,axis_1),helpers.sum_vectors(
helpers.scale(u2,axis_2),helpers.scale(u3,axis_3)))
def go_to_beam(self): beam_info = self.Body.ground() beam, distance, direction = beam_info['beam'], beam_info['distance'], beam_info['direction'] #self.move(direction, beam) new_location = helpers.sum_vectors(self.Body.getLocation(), direction) self.Body.changeLocationOnStructure(new_location, beam) return True
def get_repair_beam_direction(self):
"""
Returns the xy direction at which the support beam should be set (if none is
found). Currently, we just add a bit of disturbace while remaining within
the range that the robot was set to search.
"""
direction = self.memory["preferred_direction"]
# No preferred direction, so beam was vertically above use
if direction is None:
return None
# Add a bit of disturbace
else:
# Project onto xy_plane and make_unit
xy = helpers.make_unit((direction[0], direction[1], 0))
xy_perp = (-1 * xy[1], xy[0], 0)
# Obtain disturbance based on "search_angle"
limit = helpers.ratio(BConstants.beam["direction_tolerance_angle"])
scale = random.uniform(-1 * limit, limit)
disturbance = helpers.scale(scale, xy_perp)
return helpers.sum_vectors(disturbance, xy)
def check(i, j):
"""
Checks the endpoints and returns two that don't already exist in the
structure. If they do already exist, then it returns two endpoints that
don't. It does this by changing the j-endpoint. This function also takes
into account making sure that the returned value is still within the
robot's tendency to build up. (ie, it does not return a beam which would
build below the limit angle_constraint)
"""
# There is already a beam here, so let's move our current beam slightly to
# some side
if not self.structure.available(i, j):
# Create a small disturbace
lim = BEAM["random"]
f = random.uniform
disturbance = (f(-1 * lim, lim), f(-1 * lim, lim), f(-1 * lim, lim))
# find the new j-point for the beam
new_j = helpers.beam_endpoint(i, helpers.sum_vectors(j, disturbance))
return check(i, new_j)
else:
# Calculate the actual endpoint of the beam (now that we now direction
# vector)
return (i, helpers.beam_endpoint(i, j))
def climb(self, location, beam):
length = helpers.length(location)
if length <= self.Body.step:
new_location = helpers.sum_vectors(self.Body.getLocation(),
location)
if helpers.compare(new_location[2], 0):
beam = None
else:
print('climbing beam', beam.name)
else:
new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
self.Body.step, helpers.make_unit(location)))
print('climbing beam', beam.name)
self.Body.model.SetModelIsLocked(False)
self.Body.changeLocationOnStructure(new_location, beam)
return True
def __init__(self,size, structure, program):
# The number of robots in the swarm
self.size = size
# Keeps track of how many robots we have created (in order to keep the
# names different)
self.num_created = size
self.original_size = size
# The location of the swarm.
self.home = HOME['corner']
# Access to the structure, so we can create repairers
self.structure = structure
# Access to the program
self.model = program
# create repairers
self.repairers = {}
for i in range(size):
name = "SwarmRobot_" + str(i)
# repairers start at home
location = helpers.sum_vectors(self.home,(120*i,0,0))
self.repairers[name] = self.create(name,structure,location,program)
# Keeps track of visualization data
self.visualization_data = ''
# Keeps track of the color each robot should be at each timestep
self.color_data = ''
def setup_scene(fullscreen):
'''
Sets up the scene for the display output.
'''
# Set title and background color (white)
scene = visual.display(title="Robot Simulation",background=(1,1,1))
# Automatically center
scene.autocenter = True
# Removing autoscale
scene.autoscale = 0
# Set whether the windows will take up entire screen or not
scene.fullscreen = fullscreen
# Size of the windows seen is the size of one beam (to begin)
scene.range = (BEAM['length'],BEAM['length'],BEAM['length'])
# The center of the windows exists at the construction location
scene.center = helpers.scale(.5,helpers.sum_vectors(
CONSTRUCTION['corner'],scene.range))
# Vector from which the camera start
scene.forward = (1,0,0)
# Define up (used for lighting)
scene.up = (0,0,1)
# Defines whether or not we exit the program when we exit the screen
# visualization
scene.exit = False
return scene
def __init__(self,size, structure, program):
# The number of robots in the swarm
self.size = size
# The location of the swarm.
self.home = HOME['corner']
# Access to the structure, so we can create repairers
self.structure = structure
# Access to the program
self.model = program
# create repairers
self.repairers = {}
for i in range(size):
name = "smartrepairer_" + str(i)
# repairers start at home
location = helpers.sum_vectors(self.home,(i,0,0))
self.repairers[name] = self.create(name,structure,location,program)
# Keeps track of visualization data
self.visualization_data = ''
# Keeps track of the color each robot should be at each timestep
self.color_data = ''
def setup_scene(fullscreen):
'''
Sets up the scene for the display output.
'''
# Set title and background color (white)
scene = visual.display(title="Robot Simulation", background=(1, 1, 1))
# Automatically center
scene.autocenter = True
# Removing autoscale
scene.autoscale = 0
# Set whether the windows will take up entire screen or not
scene.fullscreen = fullscreen
# Size of the windows seen is the size of one beam (to begin)
scene.range = (BEAM['length'], BEAM['length'], BEAM['length'])
# The center of the windows exists at the construction location
scene.center = helpers.scale(
.5, helpers.sum_vectors(CONSTRUCTION['corner'], scene.range))
# Vector from which the camera start
scene.forward = (1, 0, 0)
# Define up (used for lighting)
scene.up = (0, 0, 1)
# Defines whether or not we exit the program when we exit the screen
# visualization
scene.exit = False
return scene
def get_deflection(joint_name):
'''
Returns the correct displacement in absolute coordinates for the named
joint
'''
# Get displacements
results = program.model.Results.JointDisplAbs(joint_name, 0)
if results[0] != 0:
#pdb.set_trace()
return (0, 0, 0)
u1, u2, u3 = results[7][0], results[8][0], results[9][0]
# Return the total deflection based on the local axes
return helpers.sum_vectors(
helpers.scale(u1, axis_1),
helpers.sum_vectors(helpers.scale(u2, axis_2),
helpers.scale(u3, axis_3)))
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 go_to_beam(self):
beam_info = self.Body.ground()
beam, distance, direction = beam_info['beam'], beam_info[
'distance'], beam_info['direction']
#self.move(direction, beam)
new_location = helpers.sum_vectors(self.Body.getLocation(), direction)
self.Body.changeLocationOnStructure(new_location, beam)
return True
def reset(self):
'''
Create a spanking new army of repairers the size of the original army!
'''
self.repairers = {}
for i in range(self.original_size):
name = "SwarmRobot" + str(i)
location = helpers.sum_vectors(self.home,(i,0,0))
self.repairers[name] = self.create(name,self.structure,location,self.model)
def wander(self):
'''
When a robot is not on a structure, it wanders around randomly. The
wandering is restricted to the 1st octant in global coordinates. If the
robot is near enough a beam to be on it in the next time step, it jumps on
the beam. The robots have a tendency to scale the structure, per se, but are
restricted to their immediate surroundings.
'''
# Check to see if robot is on a beam. If so, pick between moving on it or
# off it.
result = self.ground()
# Nothign nearby
if result is None:
# Get direction
direction = self.get_ground_direction()
new_location = helpers.sum_vectors(
self.location,
helpers.scale(self.step, helpers.make_unit(direction)))
# Move
self.change_location_local(new_location)
# A beam is nearby
else:
dist, close_beam, direction = (result['distance'], result['beam'],
result['direction'])
# If close enough, just jump on it
if dist < self.step:
self.move(direction, close_beam)
# Otherwise, walk towards it
else:
# Scale direction to be step_size
direction = helpers.scale(self.step,
helpers.make_unit(direction))
new_location = helpers.sum_vectors(
self.location,
helpers.scale(self.step, helpers.make_unit(direction)))
# Move
self.change_location_local(new_location)
def support_beam_endpoint(self):
"""
Returns the endpoint for construction of a support beam
"""
# Add beam_directions plus vertical change based on angle ratio (tan)
ratio = helpers.ratio(self.get_angle("support_angle"))
vertical = self.support_vertical_change()
xy_dir = self.support_xy_direction()
if xy_dir is None or vertical is None:
direction = (0, 0, 1)
else:
xy_dir = helpers.make_unit(xy_dir)
direction = helpers.make_unit(helpers.sum_vectors(xy_dir, vertical))
# Calculate endpoints
endpoint = helpers.sum_vectors(self.location, helpers.scale(BConstants.beam["length"], direction))
return endpoint
def new_robots(self, num = 1):
'''
Creates new robots at the home location. They line up along the positive
x-axis. The names are a continuation of the size of the swarm.
'''
for i in range(self.num_created, self.num_created + num):
name = "SwarmRobot_" + str(i)
location = helpers.sum_vectors(self.home,(i - num, 0, 0))
self.repairers[name] = self.create(name,self.structure,location,self.model)
self.size += num
self.num_created += num
def wander(self):
'''
When a robot is not on a structure, it wanders around randomly. The
wandering is restricted to the 1st octant in global coordinates. If the
robot is near enough a beam to be on it in the next time step, it jumps on
the beam. The robots have a tendency to scale the structure, per se, but are
restricted to their immediate surroundings.
'''
# Check to see if robot is on a beam. If so, pick between moving on it or
# off it.
result = self.ground()
# Nothign nearby
if result is None:
# Get direction
direction = self.get_ground_direction()
new_location = helpers.sum_vectors(self.location,helpers.scale(self.step,
helpers.make_unit(direction)))
# Move
self.change_location_local(new_location)
# A beam is nearby
else:
dist, close_beam, direction = (result['distance'], result['beam'],
result['direction'])
# If close enough, just jump on it
if dist < self.step:
self.move(direction,close_beam)
# Otherwise, walk towards it
else:
# Scale direction to be step_size
direction = helpers.scale(self.step,helpers.make_unit(direction))
new_location = helpers.sum_vectors(self.location,helpers.scale(
self.step, helpers.make_unit(direction)))
# Move
self.change_location_local(new_location)
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 get_default(self, ratio_coord, vertical_coord):
'''
Returns the coordinate onto which the j-point of the beam to construct
should lie
'''
# No vertical coordinate this time, since we will use a leaning one
coord = super(LeanRepairer, self).get_default(ratio_coord, None)
if coord is not None:
return coord
# We need to return one that leans
else:
xy_dir = self.non_zero_xydirection()
scale = 1 / helpers.ratio(BConstants.beam['construction_angle'])
vertical = helpers.scale(scale,
BConstants.beam['vertical_dir_set'])
direction = helpers.make_unit(helpers.sum_vectors(
xy_dir, vertical))
endpoint = helpers.sum_vectors(
self.location,
helpers.scale(BConstants.beam['length'], direction))
return endpoint
def build_base(self):
pivot = self.Body.getLocation()
ground_angle = radians(BConstants.beam['ground_angle'])
random_angle = radians(random()*360)
height = sin(ground_angle)
radius = cos(ground_angle)
x, y, z = radius*cos(random_angle), radius*sin(random_angle), height
end_coordinates = (x,y,z) #directional unit vector
endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'],\
helpers.make_unit(end_coordinates)))
#try to connect to already present beam
self.Body.addBeam(pivot,endpoint)
return True
def build_base(self):
pivot = self.Body.getLocation()
ground_angle = radians(BConstants.beam['ground_angle'])
random_angle = radians(random() * 360)
height = sin(ground_angle)
radius = cos(ground_angle)
x, y, z = radius * cos(random_angle), radius * sin(
random_angle), height
end_coordinates = (x, y, z) #directional unit vector
endpoint = helpers.sum_vectors(pivot,helpers.scale(BEAM['length'],\
helpers.make_unit(end_coordinates)))
#try to connect to already present beam
self.Body.addBeam(pivot, endpoint)
return True
def get_ground_direction(self):
'''
In future classes, this function can be altered to return a preferred
direction, but currently it only returns a random feasable direction if no
direction is assigned for the robot (self.ground_direction)
'''
def random_direction():
'''
Returns a random, new location (direction)
'''
# obtain a random direction
direction = (random.uniform(-1 * self.step, self.step),
random.uniform(-1 * self.step, self.step), 0)
# The they can't all be zero!
if helpers.compare(helpers.length(direction), 0):
return random_direction()
else:
step = helpers.scale(self.step, helpers.make_unit(direction))
predicted_location = helpers.sum_vectors(step, self.location)
# Check the location
if helpers.check_location(predicted_location):
return direction
else:
return random_direction()
# If we have a currently set direction, check to see if we will go out of
# bounds.
if self.ground_direction != None:
step = helpers.scale(self.step,
helpers.make_unit(self.ground_direction))
predicted_location = helpers.sum_vectors(step, self.location)
# We are going out of bounds, so set the direction to none and call
# yourself again (to find a new location)
if not helpers.check_location(predicted_location):
self.ground_direction = None
return self.get_ground_direction()
# Here, we return the right direction
else:
assert self.ground_direction != None
return self.ground_direction
# We don't have a direction, so pick a random one (it is checked when we
# pick it)
else:
self.ground_direction = random_direction()
return self.ground_direction
def get_ground_direction(self):
'''
In future classes, this function can be altered to return a preferred
direction, but currently it only returns a random feasable direction if no
direction is assigned for the robot (self.ground_direction)
'''
def random_direction():
'''
Returns a random, new location (direction)
'''
# obtain a random direction
direction = (random.uniform(-1 * self.step, self.step), random.uniform(
-1 * self.step, self.step), 0)
# The they can't all be zero!
if helpers.compare(helpers.length(direction),0):
return random_direction()
else:
step = helpers.scale(self.step,helpers.make_unit(direction))
predicted_location = helpers.sum_vectors(step, self.location)
# Check the location
if helpers.check_location(predicted_location):
return direction
else:
return random_direction()
# If we have a currently set direction, check to see if we will go out of
# bounds.
if self.ground_direction != None:
step = helpers.scale(self.step,helpers.make_unit(self.ground_direction))
predicted_location = helpers.sum_vectors(step, self.location)
# We are going out of bounds, so set the direction to none and call
# yourself again (to find a new location)
if not helpers.check_location(predicted_location):
self.ground_direction = None
return self.get_ground_direction()
# Here, we return the right direction
else:
assert self.ground_direction != None
return self.ground_direction
# We don't have a direction, so pick a random one (it is checked when we
# pick it)
else:
self.ground_direction = random_direction()
return self.ground_direction
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 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 random_direction():
'''
Returns a random, new location (direction)
'''
# obtain a random direction
direction = (random.uniform(-1 * self.step, self.step),
random.uniform(-1 * self.step, self.step), 0)
# The they can't all be zero!
if helpers.compare(helpers.length(direction), 0):
return random_direction()
else:
step = helpers.scale(self.step, helpers.make_unit(direction))
predicted_location = helpers.sum_vectors(step, self.location)
# Check the location
if helpers.check_location(predicted_location):
return direction
else:
return random_direction()
def random_direction():
'''
Returns a random, new location (direction)
'''
# obtain a random direction
direction = (random.uniform(-1 * self.step, self.step), random.uniform(
-1 * self.step, self.step), 0)
# The they can't all be zero!
if helpers.compare(helpers.length(direction),0):
return random_direction()
else:
step = helpers.scale(self.step,helpers.make_unit(direction))
predicted_location = helpers.sum_vectors(step, self.location)
# Check the location
if helpers.check_location(predicted_location):
return direction
else:
return random_direction()
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 get_default(self, angle_coord, vertical_coord):
"""
Returns the coordinate onto which the j-point of the beam to construct
should lie
"""
# pdb.set_trace()
if self.memory["construct_support"]:
return self.support_beam_endpoint()
elif angle_coord is not None and self.struck_coordinate():
return angle_coord
# Retunr the vertical coordinate
else:
# Create disturbance
disturbance = self.get_disturbance()
# We add a bit of disturbance every once in a while
new_coord = (
vertical_coord if self.default_probability() else (helpers.sum_vectors(vertical_coord, disturbance))
)
return new_coord
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 build(self):
"""
This functions sets down a beam. This means it "wiggles" it around in the
air until it finds a connection (programatically, it just finds the
connection which makes the smallest angle). Returns false if something went
wrong, true otherwise.
"""
def check(i, j):
"""
Checks the endpoints and returns two that don't already exist in the
structure. If they do already exist, then it returns two endpoints that
don't. It does this by changing the j-endpoint. This function also takes
into account making sure that the returned value is still within the
robot's tendency to build up. (ie, it does not return a beam which would
build below the limit angle_constraint)
"""
# There is already a beam here, so let's move our current beam slightly to
# some side
if not self.structure.available(i, j):
# Create a small disturbace
lim = BEAM["random"]
f = random.uniform
disturbance = (f(-1 * lim, lim), f(-1 * lim, lim), f(-1 * lim, lim))
# find the new j-point for the beam
new_j = helpers.beam_endpoint(i, helpers.sum_vectors(j, disturbance))
return check(i, new_j)
else:
# Calculate the actual endpoint of the beam (now that we now direction
# vector)
return (i, helpers.beam_endpoint(i, j))
# Sanitiy check
assert self.num_beams > 0
# Default pivot is our location
pivot = self.location
if self.beam is not None:
# Obtain any nearby joints, and insert the i/j-end if needed
all_joints = [coord for coord in self.beam.joints if not helpers.compare(coord[2], 0)]
if self.beam.endpoints.j not in all_joints and not helpers.compare(self.beam.endpoints.j[2], 0):
all_joints.append(self.beam.endpoints.j)
if self.beam.endpoints.i not in all_joints and not helpers.compare(self.beam.endpoints.i[2], 0):
all_joints.append(self.beam.endpoints.i)
# Find the nearest one
joint_coord, dist = min(
[(coord, helpers.distance(self.location, coord)) for coord in all_joints], key=lambda t: t[1]
)
# If the nearest joint is within our error, then use it as the pivot
if dist <= BConstants.beam["joint_error"]:
pivot = joint_coord
# Default vertical endpoint (the ratios are measured from the line created
# by pivot -> vertical_endpoint)
vertical_endpoint = helpers.sum_vectors(
pivot, helpers.scale(BEAM["length"], helpers.make_unit(BConstants.beam["vertical_dir_set"]))
)
# Get the ratios
sorted_angles = self.local_angles(pivot, vertical_endpoint)
# Find the most vertical position
final_coord = self.find_nearby_beam_coord(sorted_angles, pivot)
# Obtain the default endpoints
default_endpoint = self.get_default(final_coord, vertical_endpoint)
i, j = check(pivot, default_endpoint)
# Sanity check
assert helpers.compare(helpers.distance(i, j), BConstants.beam["length"])
return self.addbeam(i, j)
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 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 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 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 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 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)
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()