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 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_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 get_disturbance(self):
"""
Returns the disturbance level for adding a new beam at the tip (in this
class, the disturbance is random at a level set in BEAM['random'])
"""
change = BEAM["random"]
return helpers.make_unit((random.uniform(-change, change), random.uniform(-change, change), 0))
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 min_max_dir(vs,get_min=True):
unit_list = [helpers.make_unit(v) for v in vs]
if get_min:
val = min(unit_list,key=lambda t : t[2])
else:
val = max(unit_list,key=lambda t : t[2])
index = unit_list.index(val)
return index,val
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 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 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 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 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 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 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 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 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_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_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 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 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 support_xy_direction(self):
"""
Returns the direction in which the support beam should be constructed
"""
# Check to see if direction is vertical
default_direction = self.get_repair_beam_direction()
# The beam was vertical
if default_direction is None:
xy_dir = self.non_zero_xydirection()
# Use the default direction
else:
xy_dir = default_direction
return helpers.make_unit(xy_dir)
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 non_zero_xydirection(self):
"""
Returns a non_zero list of random floats with zero z component.
The direction returned is a unit vector.
"""
# Random list
tuple_list = [random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)]
# All are non-zero
if all(tuple_list):
tuple_list[2] = 0
return helpers.make_unit(tuple(tuple_list))
# All are zero - try again
else:
return self.non_zero_xydirection()
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 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_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 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 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 support_beam_endpoint(self):
'''
Returns the endpoint for a support beam
'''
#pdb.set_trace()
# Get broken beam
e1, e2 = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
# Direction
v = helpers.make_unit(helpers.make_vector(e1, e2))
# Get pivot and repair beam midpoint
pivot = self.location
midpoint1 = helpers.midpoint(e1, e2)
# Upper midpoint to encourate upward building
midpoint = helpers.midpoint(e2, midpoint1)
# Add an offset to mimick inability to determine location exactly
offset = helpers.scale(
random.uniform(-1 * BEAM['random'], BEAM['random']), v)
midpoint = (helpers.sum_vectors(midpoint, offset) if random.randint(
0, 4) == 1 else midpoint)
# Calculate starting beam_endpoint
endpoint = helpers.beam_endpoint(pivot, midpoint)
# Calculate angle from vertical
angle_from_vertical = helpers.smallest_angle(
helpers.make_vector(pivot, endpoint), (0, 0, 1))
# Get angles
sorted_angles = self.local_angles(pivot, endpoint)
min_support_angle, max_support_angle = self.get_angles()
min_constraining_angle, max_constraining_angle = self.get_angles(
support=False)
# Defining here to have access to min,max, etc.
def acceptable_support(angle, coord):
# Find beam endpoints
beam_endpoint = helpers.beam_endpoint(pivot, coord)
# Calculate angle from vertical of beam we wish to construct based on the
# information we've gathered
from_vertical = (angle_from_vertical +
angle if beam_endpoint[2] <= endpoint[2] else
angle_from_vertical - angle)
simple = not (from_vertical < min_constraining_angle
or from_vertical > max_constraining_angle)
# On the ground
if self.beam is None:
return simple
# On a beam, so check our support_angle_difference
else:
beam_vector = helpers.make_vector(self.beam.endpoints.i,
self.beam.endpoints.j)
support_vector = helpers.make_vector(self.location, coord)
angle = helpers.smallest_angle(beam_vector, support_vector)
real_angle = abs(90 - angle) if angle > 90 else angle
return simple and real_angle > BConstants.beam[
'support_angle_difference']
return_coord = None
for coord, angle in sorted_angles:
if acceptable_support(angle, coord) and helpers.on_line(
e1, e2, coord):
self.memory['broken_beam_name'] = ''
return coord
elif acceptable_support(angle, coord):
return_coord = coord
if return_coord is not None:
return return_coord
else:
# Otherwise, do default behaviour
return super(Repairer, self).support_beam_endpoint()
def support_xy_direction(self):
'''
Improves the construction direction so that we take into account the angle
at which our current beam is located, and the verticality of the beam we
are attempting to reach. This returns a unit direction (always should!)
'''
# If we're on the ground, then continue doing as before
if self.beam is None:
return super(Repairer, self).support_xy_direction()
else:
# Get repair beam vector
b_i, b_j = self.structure.get_endpoints(
self.memory['broken_beam_name'], self.location)
repair_vector = helpers.make_vector(b_i, b_j)
# Get the correct vector for the current beam
# Remember - we travel in the opposite direction as normal when building
# the support beam, so that's why this seems opposite of normal
c_i, c_j = self.beam.endpoints
current_vector = (helpers.make_vector(c_j, c_i)
if self.memory['previous_direction'][1][2] > 0
else helpers.make_vector(c_i, c_j))
#
angle = helpers.smallest_angle(repair_vector, current_vector)
# If below the specified angle, then place the beam directly upwards (no
# change in xy)
if angle < BConstants.beam['direct_repair_limit']:
return None
else:
vertical = (0, 0, 1)
v1, v2 = helpers.make_vector(b_i, c_i), helpers.make_vector(
b_i, c_j)
# We can't get a direction based on beam locations
if helpers.parallel(vertical, v1) and helpers.parallel(
vertical, v2):
return super(Repairer, self).support_xy_direction()
# We can use the current beam to decide the direction
elif not helpers.parallel(vertical, current_vector):
# pdb.set_trace()
# Project onto the xy-plane and negate
if current_vector[2] > 0:
projection = helpers.make_unit(
helpers.scale(
-1, (current_vector[0], current_vector[1], 0)))
else:
projection = helpers.make_unit(
(current_vector[0], current_vector[1], 0))
# Add some small disturbance
disturbance = helpers.scale(random.uniform(
-1, 1), (-projection[1], projection[0], projection[2]))
result = helpers.sum_vectors(projection, disturbance)
# TODO
return result
elif not helpers.parallel(vertical, repair_vector):
return super(Repairer, self).support_xy_direction()
else:
return super(Repairer, self).support_xy_direction()
def 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 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_beam_endpoint(self):
'''
Returns the endpoint for a support beam
'''
#pdb.set_trace()
# Get broken beam
e1,e2 = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
# Direction
v = helpers.make_unit(helpers.make_vector(e1,e2))
# Get pivot and repair beam midpoint
pivot = self.location
midpoint1 = helpers.midpoint(e1,e2)
# Upper midpoint to encourate upward building
midpoint = helpers.midpoint(e2,midpoint1)
# Add an offset to mimick inability to determine location exactly
offset = helpers.scale(random.uniform(-1*BEAM['random'],BEAM['random']),v)
midpoint = (helpers.sum_vectors(midpoint,offset) if random.randint(0,4) == 1
else midpoint)
# Calculate starting beam_endpoint
endpoint = helpers.beam_endpoint(pivot,midpoint)
# Calculate angle from vertical
angle_from_vertical = helpers.smallest_angle(helpers.make_vector(pivot,
endpoint),(0,0,1))
# Get angles
sorted_angles = self.local_angles(pivot,endpoint)
min_support_angle,max_support_angle = self.get_angles()
min_constraining_angle,max_constraining_angle = self.get_angles(
support=False)
# Defining here to have access to min,max, etc.
def acceptable_support(angle,coord):
# Find beam endpoints
beam_endpoint = helpers.beam_endpoint(pivot,coord)
# Calculate angle from vertical of beam we wish to construct based on the
# information we've gathered
from_vertical = (angle_from_vertical + angle if beam_endpoint[2] <=
endpoint[2] else angle_from_vertical - angle)
simple = not (from_vertical < min_constraining_angle or from_vertical >
max_constraining_angle)
# On the ground
if self.beam is None:
return simple
# On a beam, so check our support_angle_difference
else:
beam_vector = helpers.make_vector(self.beam.endpoints.i,
self.beam.endpoints.j)
support_vector = helpers.make_vector(self.location,coord)
angle = helpers.smallest_angle(beam_vector,support_vector)
real_angle = abs(90-angle) if angle > 90 else angle
return simple and real_angle > BConstants.beam['support_angle_difference']
return_coord = None
for coord,angle in sorted_angles:
if acceptable_support(angle,coord) and helpers.on_line(e1,e2,coord):
self.memory['broken_beam_name'] = ''
return coord
elif acceptable_support(angle,coord):
return_coord = coord
if return_coord is not None:
return return_coord
else:
# Otherwise, do default behaviour
return super(Repairer,self).support_beam_endpoint()
def support_xy_direction(self):
'''
Improves the construction direction so that we take into account the angle
at which our current beam is located, and the verticality of the beam we
are attempting to reach. This returns a unit direction (always should!)
'''
# If we're on the ground, then continue doing as before
if self.beam is None:
return super(Repairer,self).support_xy_direction()
else:
# Get repair beam vector
b_i,b_j = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
repair_vector = helpers.make_vector(b_i,b_j)
# Get the correct vector for the current beam
# Remember - we travel in the opposite direction as normal when building
# the support beam, so that's why this seems opposite of normal
c_i,c_j = self.beam.endpoints
current_vector = (helpers.make_vector(c_j,c_i) if
self.memory['previous_direction'][1][2] > 0 else helpers.make_vector(
c_i,c_j))
#
angle = helpers.smallest_angle(repair_vector,current_vector)
# If below the specified angle, then place the beam directly upwards (no
# change in xy)
if angle < BConstants.beam['direct_repair_limit']:
return None
else:
vertical = (0,0,1)
v1,v2 = helpers.make_vector(b_i,c_i), helpers.make_vector(b_i,c_j)
# We can't get a direction based on beam locations
if helpers.parallel(vertical,v1) and helpers.parallel(vertical,v2):
return super(Repairer,self).support_xy_direction()
# We can use the current beam to decide the direction
elif not helpers.parallel(vertical,current_vector):
# pdb.set_trace()
# Project onto the xy-plane and negate
if current_vector[2] > 0:
projection = helpers.make_unit(helpers.scale(-1,(current_vector[0],
current_vector[1],0)))
else:
projection = helpers.make_unit((current_vector[0],current_vector[1],0))
# Add some small disturbance
disturbance = helpers.scale(random.uniform(-1,1),(-projection[1],
projection[0],projection[2]))
result = helpers.sum_vectors(projection,disturbance)
# TODO
return result
elif not helpers.parallel(vertical,repair_vector):
return super(Repairer,self).support_xy_direction()
else:
return super(Repairer,self).support_xy_direction()
def 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 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)