def ground(self,random=False):
'''
This function finds the nearest beam to the robot that is connected
to the xy-plane (ground). It returns that beam and its direction from the
robot. If random = True, the function picks a random beam from those connected
to the xy-plane.
'''
# Get local boxes
boxes = self.structure.get_boxes(self.location)
# Initializations
distances = {}
vectors = {}
# Cycle through boxes
for box in boxes:
# Cycle through beams in each box
for name in box:
# So e1 is in the form (x,y,z)
e1, e2 = box[name].endpoints
# beam is lying on the ground (THIS IS NOT FUNCTIONAL)
if helpers.compare(e1[2],0) and helpers.compare(e2[0],0):
# pdb.set_trace()
vectors[name] = helpers.vector_to_line(e1,e2,self.location)
distances[name] = helpers.length(vectors[name])
# Only one point is on the ground
if helpers.compare(e1[2],0):
vectors[name] = helpers.make_vector(self.location, e1)
distances[name] = helpers.distance(e1, self.location)
elif helpers.compare(e2[2],0):
vectors[name] = helpers.make_vector(self.location, e2)
distances[name] = helpers.distance(e2, self.location)
# No points on the ground
else:
pass
# get name of beam at the minimum distance if one exists
if distances == {}:
return None
else:
# Random key
if random: name = choice(list(distances.keys()))
# This returns the key (ie, name) of the minimum value in distances
else: name = min(distances, key=distances.get)
# So far away that we can't "see it"
if distances[name] > ROBOT['local_radius']:
return None
else:
# All the same beans
beams = [box[name] for box in boxes if name in box]
return { 'beam' : beams[0],
'distance' : distances[name],
'direction' : vectors[name]}
def get_structure_density(self, location, radius=BConstants.beam['density_radius']):
boxes = self.Body.structure.get_boxes(location, radius)
nearby_beams = []
for box in boxes:
for beam_name in box.keys():
beam_info = box[beam_name].current_state()
endpoint_1, endpoint_2 = beam_info['endpoints']
distance_1, distance_2 = helpers.distance(location, endpoint_1), helpers.distance(location, endpoint_2)
if distance_1 <= radius or distance_2 <= radius:
if [beam_name, distance_1, distance_2] not in nearby_beams:
nearby_beams.append([beam_name, distance_1, distance_2])
#print(nearby_beams)
num_beams = len(nearby_beams)
return num_beams
def update_deflection(self, i_val, j_val):
'''
Updates the deflection using a named tupled
'''
# Update deflection
self.deflection = EndPoints(i=i_val, j=j_val)
# Update endpoints
self.deflected_endpoints = self.get_true_endpoints()
return (self.previous_write_endpoints is None or helpers.distance(
self.deflected_endpoints.i, self.previous_write_endpoints.i) >=
VISUALIZATION['step'] or helpers.distance(
self.deflected_endpoints.j,
self.previous_write_endpoints.j) >= VISUALIZATION['step'])
def update_deflection(self,i_val,j_val):
'''
Updates the deflection using a named tupled
'''
# Update deflection
self.deflection = EndPoints(i=i_val,j=j_val)
# Update endpoints
self.deflected_endpoints = self.get_true_endpoints()
return (self.previous_write_endpoints is None or helpers.distance(
self.deflected_endpoints.i,self.previous_write_endpoints.i)
>= VISUALIZATION['step'] or helpers.distance(
self.deflected_endpoints.j,self.previous_write_endpoints.j) >=
VISUALIZATION['step'])
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 update_radius(self):
center = CONSTRUCTION['center']
max_radius = self.Body.readFromMemory('base_radius')
boxes = self.Body.structure.get_boxes(center, max_radius + 120)
for box in boxes:
for beam_name in box.keys():
beam_info = box[beam_name].current_state()
endpoint_1, endpoint_2 = beam_info['endpoints']
distance = 0
if endpoint_1[2] == 0:
distance = helpers.distance(center, endpoint_1)
if endpoint_2[2] == 0:
distance = helpers.distance(center, endpoint_2)
if distance > max_radius:
max_radius = distance
return max_radius
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 update_radius(self):
center = CONSTRUCTION['center']
max_radius = self.Body.readFromMemory('base_radius')
boxes = self.Body.structure.get_boxes(center, max_radius + 120)
for box in boxes:
for beam_name in box.keys():
beam_info = box[beam_name].current_state()
endpoint_1, endpoint_2 = beam_info['endpoints']
distance = 0
if endpoint_1[2] == 0:
distance = helpers.distance(center, endpoint_1)
if endpoint_2[2] == 0:
distance = helpers.distance(center, endpoint_2)
if distance > max_radius:
max_radius = distance
return max_radius
def ground(self):
'''
This function finds the nearest beam to the robot that is connected
to the xy-plane (ground). It returns that beam and its direction from the
robot.
'''
# Get local boxes
boxes = self.structure.get_boxes(self.location)
# Initializations
distances = {}
vectors = {}
# Cycle through boxes
for box in boxes:
# Cycle through beams in each box
for name in box:
# So e1 is in the form (x,y,z)
e1, e2 = box[name].endpoints
# beam is lying on the ground (THIS IS NOT FUNCTIONAL)
if helpers.compare(e1[2], 0) and helpers.compare(e2[0], 0):
# pdb.set_trace()
vectors[name] = helpers.vector_to_line(
e1, e2, self.location)
distances[name] = helpers.length(vectors[name])
# Only one point is on the ground
elif helpers.compare(e1[2], 0):
vectors[name] = helpers.make_vector(self.location, e1)
distances[name] = helpers.distance(e1, self.location)
elif helpers.compare(e2[2], 0):
vectors[name] = helpers.make_vector(self.location, e2)
distances[name] = helpers.distances(e2, self.location)
# No points on the ground
else:
pass
# get name of beam at the minimum distance if one exists
if distances == {}:
return None
else:
# This returns the key (ie, name) of the minimum value in distances
name = min(distances, key=distances.get)
# So far away that we can't "see it"
if distances[name] > ROBOT['local_radius']:
return None
else:
# All the same beans
beams = [box[name] for box in boxes if name in box]
return {
'beam': beams[0],
'distance': distances[name],
'direction': vectors[name]
}
def get_structure_density(self,
location,
radius=BConstants.beam['density_radius']):
boxes = self.Body.structure.get_boxes(location, radius)
nearby_beams = []
for box in boxes:
for beam_name in box.keys():
beam_info = box[beam_name].current_state()
endpoint_1, endpoint_2 = beam_info['endpoints']
distance_1, distance_2 = helpers.distance(
location,
endpoint_1), helpers.distance(location, endpoint_2)
if distance_1 <= radius or distance_2 <= radius:
if [beam_name, distance_1, distance_2] not in nearby_beams:
nearby_beams.append(
[beam_name, distance_1, distance_2])
#print(nearby_beams)
num_beams = len(nearby_beams)
return num_beams
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 PROGRAM['robot_load_case'] ==
data[3][index]) or data[3][index] != PROGRAM['robot_load_case']):
indeces.append(index)
index += 1
# Delete the loads off the beam
ret = self.model.FrameObj.DeleteLoadPoint(self.beam.name,
PROGRAM['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 = BConstants.beam['beam_limit'] + (
xy_dist / BConstants.beam['length']
) * BConstants.beam['horizontal_beam_limit']
return (moment < limit or helpers.compare(moment, limit))
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 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 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 atTrueTop(self):
'''
Returns if we really are at the top
'''
def below(beams):
'''
Returns whether all of the beams are below us
'''
for beam in beams:
for endpoint in beam.endpoints:
# If the beam is not close to us and it is greater than our location
if (not helpers.compare(
helpers.distance(self.location, endpoint), 0)
and endpoint[2] > self.location[2]):
return False
return True
if self.beam is not None:
if (helpers.compare(
helpers.distance(self.location, self.beam.endpoints.i), 0)
and self.beam.endpoints.i[2] > self.beam.endpoints.j[2]):
close = self.beam.endpoints.i
elif (helpers.compare(
helpers.distance(self.location, self.beam.endpoints.j), 0)
and self.beam.endpoints.j[2] > self.beam.endpoints.i[2]):
close = self.beam.endpoints.j
else:
close = None
if close is not None:
try:
beams = self.beam.joints[self.beam.endpoints.i]
return below(beams)
except KeyError:
return True
return False
def below(beams):
'''
Returns whether all of the beams are below us
'''
for beam in beams:
for endpoint in beam.endpoints:
# If the beam is not close to us and it is greater than our location
if (not helpers.compare(helpers.distance(self.location,endpoint),0)
and endpoint[2] > self.location[2]):
return False
return True
def addLoad(self,beam,location,value):
'''
Adds a load of the specified value to the named beam at the specific
location
'''
# Sanity check
assert not self.model.GetModelIsLocked()
# Find distance and add load
distance = helpers.distance(beam.endpoints.i,location)
ret = self.model.FrameObj.SetLoadPoint(beam.name,PROGRAM['robot_load_case'],
1,10,distance,value,"Global", False, True,0)
helpers.check(ret,self,"adding new load",beam=beam.name,distance=distance,
value=value,state=self.currentState())
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 at_top(self):
'''
Returns if we really are at the top
'''
def below(beams):
'''
Returns whether all of the beams are below us
'''
for beam in beams:
for endpoint in beam.endpoints:
# If the beam is not close to us and it is greater than our location
if (not helpers.compare(helpers.distance(self.location,endpoint),0)
and endpoint[2] > self.location[2]):
return False
return True
if self.beam is not None:
if (helpers.compare(helpers.distance(self.location,self.beam.endpoints.i),0)
and self.beam.endpoints.i[2] > self.beam.endpoints.j[2]):
close = self.beam.endpoints.i
elif (helpers.compare(helpers.distance(self.location,self.beam.endpoints.j),0)
and self.beam.endpoints.j[2] > self.beam.endpoints.i[2]):
close = self.beam.endpoints.j
else:
close = None
if close is not None:
try:
beams = self.beam.joints[self.beam.endpoints.i]
return below(beams)
except KeyError:
return True
return False
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 atJoint(self):
'''
Returns whether or not the robot is at a joint
'''
if self.onStructure() and helpers.compare(self.location[2],0):
return True
# super section
elif self.onStructure():
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 helpers.compare(self.location[2],0)
else:
return False
def atJoint(self):
'''
Returns whether or not the robot is at a joint
'''
if self.onStructure() and helpers.compare(self.location[2], 0):
return True
# super section
elif self.onStructure():
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 helpers.compare(self.location[2], 0)
else:
return False
def special_repair(self):
'''
Returns whether or not we should start repairing due to any special rules
'''
if self.at_top():
if self.beam.joints == {}:
return True
elif self.beam.joints != {}:
# Find the closest joint on our beam
# There should always be a joint on a beam (the i-end :))
distance_to_joint = min(([
helpers.distance(self.location, coord)
for coord in self.beam.joints
]))
# Add the current beam to broken because it needs support
if distance_to_joint > BConstants.beam['joint_distance']:
return True
return False
def addLoad(self, beam, location, value):
'''
Adds a load of the specified value to the named beam at the specific
location
'''
# Sanity check
assert not self.model.GetModelIsLocked()
# Find distance and add load
distance = helpers.distance(beam.endpoints.i, location)
ret = self.model.FrameObj.SetLoadPoint(beam.name,
PROGRAM['robot_load_case'], 1,
10, distance, value, "Global",
False, True, 0)
helpers.check(ret,
self,
"adding new load",
beam=beam.name,
distance=distance,
value=value,
state=self.currentState())
def add_angles(box, dictionary):
for name, beam in box.items():
# Ignore the beam you're on.
if self.beam == None or self.beam.name != name:
# Base vector (from which angles are measured)
base_vector = helpers.make_vector(pivot, endpoint)
# Get the closest points between the beam we want to construct and the
# current beam
points = helpers.closest_points(beam.endpoints, (pivot, endpoint))
if points != None:
# Endpoints (e1 is on a vertical beam, e2 is on the tilted one)
e1, e2 = points
# If we can actually reach the second point from vertical
if (
not helpers.compare(helpers.distance(pivot, e2), 0)
and helpers.distance(pivot, e2) <= BEAM["length"]
):
# Distance between the two endpoints
dist = helpers.distance(e1, e2)
# Vector of beam we want to construct and angle from base_vector
construction_vector = helpers.make_vector(pivot, e2)
angle = helpers.smallest_angle(base_vector, construction_vector)
# Add to dictionary
if e2 in dictionary:
assert helpers.compare(dictionary[e2], angle)
else:
dictionary[e2] = angle
# Get the points at which the beam intersects the sphere created by
# the vertical beam
sphere_points = helpers.sphere_intersection(beam.endpoints, pivot, BEAM["length"])
if sphere_points != None:
# Cycle through intersection points (really, should be two, though
# it is possible for it to be one, in
# which case, we would have already taken care of this). Either way,
# we just cycle
for point in sphere_points:
# Vector to the beam we want to construct
construction_vector = helpers.make_vector(pivot, point)
angle = helpers.smallest_angle(base_vector, construction_vector)
# Add to dictionary
if point in dictionary:
assert helpers.compare(dictionary[point], angle)
else:
dictionary[point] = angle
# Endpoints are also included
for e in beam.endpoints:
v = helpers.make_vector(pivot, e)
l = helpers.length(v)
if (
e not in dictionary
and not helpers.compare(l, 0)
and (helpers.compare(l, BEAM["length"]) or l < BEAM["length"])
):
angle = helpers.smallest_angle(base_vector, v)
dictionary[e] = angle
return dictionary
def at_construction_site(self): return helpers.compare(helpers.distance(self.location, CONSTRUCTION['center']), 0)
def get_walkable_directions(self,box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location,joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist,0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,e1), helpers.make_vector(
self.location,e2)
# We don't want to include zero-vectors
bool_v1,bool_v2 = (not helpers.compare(helpers.length(v1),0),
not helpers.compare(helpers.length(v2),0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([helpers.make_vector(self.location,e1),
helpers.make_vector(self.location,e2)])
elif bool_v1:
crawlable[beam.name] = [helpers.make_vector(self.location,e1)]
elif bool_v2:
crawlable[beam.name] = [helpers.make_vector(self.location,e2)]
else:
raise Exception("All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location,coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step or helpers.compare(length, self.step))
and not helpers.compare(length,0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception("Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print ("The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name,self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(helpers.make_vector(self.location,
joint))
else:
crawlable[self.beam.name] = [helpers.make_vector(self.location,joint)]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location,self.beam.endpoints.i),
helpers.make_vector(self.location,self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1),0)
b_v2 = not helpers.compare(helpers.length(v2),0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1,v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,v1) and helpers.dot(direct,v1) > 0:
bool_v1 = False
if helpers.parallel(direct,v2) and helpers.dot(direct,v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
def at_construction_site(self):
return helpers.compare(
helpers.distance(self.location, CONSTRUCTION['center']), 0)
def add_angles(box, dictionary):
for name, beam in box.items():
# Ignore the beam you're on.
if self.beam == None or self.beam.name != name:
# Base vector (from which angles are measured)
base_vector = helpers.make_vector(pivot, endpoint)
# Get the closest points between the beam we want to construct and the
# current beam
points = helpers.closest_points(beam.endpoints,
(pivot, endpoint))
if points != None:
# Endpoints (e1 is on a vertical beam, e2 is on the tilted one)
e1, e2 = points
# If we can actually reach the second point from vertical
if (not helpers.compare(helpers.distance(pivot, e2), 0)
and
helpers.distance(pivot, e2) <= BEAM['length']):
# Distance between the two endpoints
dist = helpers.distance(e1, e2)
# Vector of beam we want to construct and angle from base_vector
construction_vector = helpers.make_vector(
pivot, e2)
angle = helpers.smallest_angle(
base_vector, construction_vector)
# Add to dictionary
if e2 in dictionary:
assert helpers.compare(dictionary[e2], angle)
else:
dictionary[e2] = angle
# Get the points at which the beam intersects the sphere created by
# the vertical beam
sphere_points = helpers.sphere_intersection(
beam.endpoints, pivot, BEAM['length'])
if sphere_points != None:
# Cycle through intersection points (really, should be two, though
# it is possible for it to be one, in
# which case, we would have already taken care of this). Either way,
# we just cycle
for point in sphere_points:
# Vector to the beam we want to construct
construction_vector = helpers.make_vector(
pivot, point)
angle = helpers.smallest_angle(
base_vector, construction_vector)
# Add to dictionary
if point in dictionary:
assert helpers.compare(dictionary[point],
angle)
else:
dictionary[point] = angle
# Endpoints are also included
for e in beam.endpoints:
v = helpers.make_vector(pivot, e)
l = helpers.length(v)
if (e not in dictionary and not helpers.compare(l, 0)
and (helpers.compare(l, BEAM['length'])
or l < BEAM['length'])):
angle = helpers.smallest_angle(base_vector, v)
dictionary[e] = angle
return dictionary
def 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 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 PROGRAM['robot_load_case'] == data[3][index])
or data[3][index] != PROGRAM['robot_load_case']):
indeces.append(index)
index += 1
# Delete the loads off the beam
ret = self.model.FrameObj.DeleteLoadPoint(
self.beam.name, PROGRAM['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 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
