def ground(self):
'''
This function finds the nearest beam to the robot that is connected
to the xy-plane (ground). It returns that beam and its direction from the
robot.
'''
# Get local boxes
boxes = self.structure.get_boxes(self.location)
# Initializations
distances = {}
vectors = {}
# Cycle through boxes
for box in boxes:
# Cycle through beams in each box
for name in box:
# So e1 is in the form (x,y,z)
e1, e2 = box[name].endpoints
# beam is lying on the ground (THIS IS NOT FUNCTIONAL)
if helpers.compare(e1[2],0) and helpers.compare(e2[0],0):
# pdb.set_trace()
vectors[name] = helpers.vector_to_line(e1,e2,self.location)
distances[name] = helpers.length(vectors[name])
# Only one point is on the ground
elif helpers.compare(e1[2],0):
vectors[name] = helpers.make_vector(self.location, e1)
distances[name] = helpers.distance(e1, self.location)
elif helpers.compare(e2[2],0):
vectors[name] = helpers.make_vector(self.location, e2)
distances[name] = helpers.distances(e2, self.location)
# No points on the ground
else:
pass
# get name of beam at the minimum distance if one exists
if distances == {}:
return None
else:
# This returns the key (ie, name) of the minimum value in distances
name = min(distances, key=distances.get)
# So far away that we can't "see it"
if distances[name] > variables.local_radius:
return None
else:
# All the same beans
beams = [box[name] for box in boxes if name in box]
return { 'beam' : beams[0],
'distance' : distances[name],
'direction' : vectors[name]}
def 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,construction.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(construction.beam['verticality_angle']):
return travel
else:
return helpers.make_unit(normal)
else:
scalar = 1 / helpers.ratio(construction.beam['moment_angle_max'])
scaled_travel = helpers.scale(scalar,travel)
return helpesr.make_unit(helpers.sum_vectors(normal,scaled_travel))
def decide(self):
'''
Overwritting to allow for repair work to take place
'''
if self.search_mode and self.repair_mode:
self.pre_decision()
# We have moved off the structure entirely, so wander
if self.beam is None:
self.ground_support()
# We've moved off the beam, so run the search support routine
elif (self.memory['broken_beam_name'] != self.beam.name and
self.search_mode and self.memory['broken_beam_name'] != ''):
# Remember the beam we moved onto right after the broken one
if self.memory['previous_beam'] is None:
self.memory['previous_beam'] = self.beam.name
# We have found a support beam, so return to construct mode (the support beam is vertical)
if (self.memory['previous_beam'] != self.beam.name and (
self.memory['previous_direction'] is None or
self.memory['previous_direction'][1][2] > 0 or helpers.compare(
self.memory['previous_direction'][1][2],0))):
self.construction_mode()
# Decide again since we're out of repair mode
self.decide()
else:
self.find_support()
# Move (don't check construction)
self.movable_decide()
# Simply move
else:
self.movable_decide()
# We found a support beam and are on it, planning on construction. If
# we reach the endpoint of the beam (the support beam), then construct.
elif self.repair_mode:
if (helpers.compare(helpers.distance(self.location,
self.beam.endpoints.j),self.step / 2)):
self.start_contruction = True
self.memory['broken'] = []
# Build Mode
else:
super(DumbRepairer,self).decide()
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(variables.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 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,construction.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 <= variables.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 at_joint(self):
'''
Updating to return true when we are on a beam AND on the ground. We want to
consider this a joint so that the robot treats it as one and checks that
limit instead of another.
'''
return ((self.beam is not None and helpers.compare(self.location[2],0)) or
super(NormalRepairer,self).at_joint())
def removeload(location):
'''
Removes the load assigned to a specific location based on where the robot
existed (assumes the robot is on a beams
'''
# Sanity check
assert not self.model.GetModelIsLocked()
# obtain current values.
# values are encapsulated in a list as follows: ret, number_items,
# frame_names, loadpat_names, types, coordinates, directions, rel_dists,
# dists, loads
data = self.model.FrameObj.GetLoadPoint(self.beam.name)
# Sanity check with data
assert data[0] == 0
if data[1] == 0:
helpers.check(1,self,"getting loads",beam=self.beam.name,
return_data=data,state=self.current_state())
return;
# Find location of load
i, j = self.beam.endpoints
curr_dist = helpers.distance(i,self.location)
# Loop through distances to find our load (the one in self.location) and
# remove all reference to it. Additionally remove loads not related to our
# load pattern
indeces = []
index = 0
for ab_dist in data[8]: # this provides acces to the absolute_distance
if ((helpers.compare(ab_dist,curr_dist) and variables.robot_load_case ==
data[3][index]) or data[3][index] != variables.robot_load_case):
indeces.append(index)
index += 1
# Delete the loads off the beam
ret = self.model.FrameObj.DeleteLoadPoint(self.beam.name,
variables.robot_load_case)
helpers.check(ret,self,"deleting loads",return_val=ret,
beam=self.beam.name,distance=curr_dist,previous_loads=data,
state=self.current_state())
# add the loads we want back to the frame (automatically deletes all
# previous loads)
for i in range(data[1]):
if i not in indeces:
ret = self.model.FrameObj.SetLoadPoint(self.beam.name,
data[3][i], data[4][i], data[6][i], data[7][i], data[9][i],"Global",
True,False)
helpers.check(ret,self,"adding back loads",return_val=ret,
beam=self.beam.name,load_pat=data[3][i],type=data[4][i],
direction=data[6][i],rel_dist=data[7][i],load_val=data[9][i],
state=self.current_state())
def beam_check(self,name):
'''
Checks a beam to see whether it is in a state that requires repair
'''
moment = self.get_moment(name)
e1,e2 = self.structure.get_endpoints(name,self.location)
xy_dist = helpers.distance((e1[0],e1[1],0),(e2[0],e2[1],0))
limit = construction.beam['beam_limit'] + (
xy_dist / construction.beam['length']) * construction.beam['horizontal_beam_limit']
return (moment < limit or helpers.compare(moment,limit))
def compensate_change(coord,change = variables.epsilon):
'''
Returns a random direction that is the right sign so that it compensates
for the sign of change
'''
if helpers.compare(coord,0):
return random.uniform(-1 * change,change)
elif coord < 0:
return random.uniform(0,change)
else:
return random.uniform(-1 * change, 0)
def at_joint(self):
'''
Returns whether or not the robot is at a joint
'''
if self.on_structure():
for joint in self.beam.joints:
# If we're at a joint to another beam
if helpers.compare(helpers.distance(self.location,joint),0):
return True
return False
def set_dir(string,coord):
'''
Figures out what pos_var should be in order to travel in that direction
NO LONGER USED
'''
if helpers.compare(coord,0):
self.memory[string] = None
if coord > 0:
self.memory[string] = True
else:
self.memory[string] = False
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 == variables.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 local_rules(self):
'''
Overriding so we can build support beam
'''
# If we ran our course with the support, construct it
if (((self.beam is not None and self.memory['new_beam_steps'] == 0) or (
helpers.compare(self.location[2],0) and
self.memory['new_beam_ground_steps'] == 0)) and
self.memory['construct_support']):
return True
# Local rules as before
else:
return super(DumbRepairer,self).local_rules()
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_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 addpoint(p):
'''
Adds a point object to our model. The object is retrained in all
directions if on the ground (including rotational and translational
motion. Returns the name of the added point.
'''
# Add to SAP Program
name = self.program.point_objects.addcartesian(p)
# Check Coordinates
if helpers.compare(p[2], 0):
DOF = (True,True,True,True,True,True)
if self.program.point_objects.restraint(name,DOF):
return name
else:
print("Something went wrong adding ground point {}.".format(str(p)))
else:
return name
def decide(self):
# Tell each robot to make the decion
for repairer in self.repairers:
self.repairers[repairer].decide()
# Add location data for visualization of simulation
loc = self.repairers[repairer].get_true_location()
location = (loc[0], loc[1], 0) if helpers.compare(loc[2],0) else loc
self.visualization_data += "{}:{}<>".format(repairer,str(
helpers.round_tuple(location,3)))
# Get color data based on what the robot is doing
color = (1,0,1) if not self.repairers[repairer].repair_mode else (0,1,0)
self.color_data += "{}:{}<>".format(repairer,str(color))
self.visualization_data += "\n"
self.color_data += "\n"
def change_location_local(self,new_location, first_beam = None):
'''
Moves the robot about locally (ie, without worrying about the structure,
except for when it moves onto the first beam)
'''
# When we move onto our first beam, add the load
if first_beam != None:
self.__addload(first_beam,new_location,self.weight)
self.location = new_location
# Check that we are on the first octant
assert self.location[0] >= 0
assert self.location[1] >= 0
# Verify that we are still on the xy plane
if helpers.compare(self.location[2], 0):
loc = list(self.location)
loc[2] = 0
self.location = tuple(loc)
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 remove_specific(self,dirs):
'''
We don't want to move UP along our own beam when we are repairing and at a
joint.
'''
new_dirs = {}
if self.at_joint() and self.repair_mode:
# Access items
for beam, vectors in dirs.items():
# If the directions is about our beam, remove references to up.
# Also do this for our broken beam (we don't want to get stuck climbing
# onto it again and again in a cycle)
if beam == self.beam.name or beam == self.memory['broken_beam_name']:
vectors = [v for v in vectors if v[2] < 0 or helpers.compare(v[2],0)]
if vectors != []:
new_dirs[beam] = vectors
else:
new_dirs[beam] = vectors
return super(Repairer,self).remove_specific(new_dirs)
return super(Repairer,self).remove_specific(dirs)
def climb_off(self,loc):
'''
Returns whether or not the robot should climb off the structure. Additionally,
sets some special variables
'''
# On the xy-plane with no beams OR repairing
if helpers.compare(loc[2],0) and (self.num_beams == 0 or self.search_mode):
# Not repairing, so calculate direction
if not self.search_mode:
direction = helpers.make_vector(self.location,construction.home)
direction = (direction[0],direction[1],0)
self.ground_direction = direction
return True
else:
# Resetting to None if not in search_mode
self.ground_direction = (None if not self.search_mode else
self.ground_direction)
return False
def filter_feasable(self,dirs):
'''
Filters the set of dirs passed in to check that the beam can support a robot
+ beam load if the robot were to walk in the specified direction to the
very tip of the beam.
This function is only ever called if an analysis model exists.
Additionally, this function stores information on the beams that need to be
repaired. This is stored in self.memory['broken'], which is originally set
to none.
'''
# Sanity check
assert self.model.GetModelIsLocked()
results = {}
# If at a joint, cycle through possible directions and check that the beams
# meet the joint_limit. If they do, keep them. If not, discard them.
if self.at_joint():
# Cycle through directions
for name, directions in dirs.items():
# If the name is our beam and we can read moment from beams,
# do a structural check instead of a joint check
if (variables.read_beam and
((self.beam.name == name and self.beam_check(name)) or
(self.beam.name != name and self.joint_check(name)))):
results[name] = directions
# Otherwise, do a joint_check for all beams
elif self.joint_check(name):
results[name] = directions
# It joint check failed, only keep down directions
else:
# Keep only the directions that take us down
new_directions = ([direction for direction in directions if
helpers.compare(direction[2],0) or direction[2] < 0])
if len(new_directions) > 0:
results[name] = new_directions
# Add beam to broken
beam = self.structure.get_beam(name,self.location)
if not any(beam in broken for broken in self.memory['broken']):
moment = self.get_moment(name)
self.memory['broken'].append((beam,moment))
# Not at joint, and can read beam moments
elif variables.read_beam:
# Sanity check (there should only be one beam in the set of directions if
# We are not at a joint)
assert len(dirs) == 1
# Check beam
if self.beam_check(self.beam.name):
results = dirs
# Add the beam to the broken
else:
# Keep only the directions that take us down
for name,directions in dirs.items():
new_directions = ([direction for direction in directions if
helpers.compare(direction[2],0) or direction[2] < 0])
if len(new_directions) > 0:
results[name] = new_directions
# Beam is not already in broken
if not any(self.beam in broken for broken in self.memory['broken']):
moment = self.get_moment(name)
self.memory['broken'].append((self.beam,moment))
# We aren't reading beams, so we keep all the directions if not at a joint
else:
results = dirs
return results
def add_angles(box,dictionary):
for name, beam in box.items():
# Ignore the beam you're on.
if self.beam == None or self.beam.name != name:
# Base vector (from which angles are measured)
base_vector = helpers.make_vector(pivot,endpoint)
# Get the closest points between the beam we want to construct and the
# current beam
points = helpers.closest_points(beam.endpoints,(pivot,endpoint))
if points != None:
# Endpoints (e1 is on a vertical beam, e2 is on the tilted one)
e1,e2 = points
# If we can actually reach the second point from vertical
if (not helpers.compare(helpers.distance(pivot,e2),0) and
helpers.distance(pivot,e2) <= variables.beam_length):
# Distance between the two endpoints
dist = helpers.distance(e1,e2)
# Vector of beam we want to construct and angle from base_vector
construction_vector = helpers.make_vector(pivot,e2)
angle = helpers.smallest_angle(base_vector,construction_vector)
# Add to dictionary
if e2 in dictionary:
assert helpers.compare(dictionary[e2],angle)
else:
dictionary[e2] = angle
# Get the points at which the beam intersects the sphere created by
# the vertical beam
sphere_points = helpers.sphere_intersection(beam.endpoints,pivot,
variables.beam_length)
if sphere_points != None:
# Cycle through intersection points (really, should be two, though
# it is possible for it to be one, in
# which case, we would have already taken care of this). Either way,
# we just cycle
for point in sphere_points:
# Vector to the beam we want to construct
construction_vector = helpers.make_vector(pivot,point)
angle = helpers.smallest_angle(base_vector,construction_vector)
# Add to dictionary
if point in dictionary:
assert helpers.compare(dictionary[point],angle)
else:
dictionary[point] = angle
# Endpoints are also included
for e in beam.endpoints:
v = helpers.make_vector(pivot,e)
l = helpers.length(v)
if (e not in dictionary and not helpers.compare(l,0) and (
helpers.compare(l,variables.beam_length) or l < variables.beam_length)):
angle = helpers.smallest_angle(base_vector,v)
dictionary[e] = angle
return dictionary
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 = variables.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 <= construction.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(
variables.beam_length,
helpers.make_unit(construction.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),construction.beam['length'])
return self.addbeam(i,j)
def climb_off(self, loc): ''' Decides whether or not the robot should climb off the structure ''' return helpers.compare(loc[2],0) and random.int(0,1) == 1
def get_walkable_directions(self,box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location,joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist,0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,e1), helpers.make_vector(
self.location,e2)
# We don't want to include zero-vectors
bool_v1,bool_v2 = (not helpers.compare(helpers.length(v1),0),
not helpers.compare(helpers.length(v2),0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([helpers.make_vector(self.location,e1),
helpers.make_vector(self.location,e2)])
elif bool_v1:
crawlable[beam.name] = [helpers.make_vector(self.location,e1)]
elif bool_v2:
crawlable[beam.name] = [helpers.make_vector(self.location,e2)]
else:
raise Exception("All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location,coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step or helpers.compare(length, self.step))
and not helpers.compare(length,0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception("Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print ("The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name,self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(helpers.make_vector(self.location,
joint))
else:
crawlable[self.beam.name] = [helpers.make_vector(self.location,joint)]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location,self.beam.endpoints.i),
helpers.make_vector(self.location,self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1),0)
b_v2 = not helpers.compare(helpers.length(v2),0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1,v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,v1) and helpers.dot(direct,v1) > 0:
bool_v1 = False
if helpers.parallel(direct,v2) and helpers.dot(direct,v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable