def support_vertical_change(self): # Get vertical vector change_vector = super(Repairer,self).support_vertical_change() # If we're on the ground, just return (no rotation necessary) if self.beam is None: return change_vector # Otherwise, rotate it based on our current beam else: # Debugging # pdb.set_trace() if self.memory['previous_direction'] is None: #pdb.set_trace() pass # Get the correct vector for the current beam i,j = self.beam.endpoints current_vector = helpers.make_vector(i,j) # Find rotation from vertical angle = helpers.smallest_angle((0,0,1),current_vector) rotation_angle = 180 - angle if angle > 90 else angle vertical_angle = abs(construction.beam['support_angle'] - rotation_angle) return super(Repairer,self).support_vertical_change(angle=vertical_angle)
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 > construction.beam['support_angle_difference']
def preferred(self,vector): ''' Returns True if vector is preferred, False if it is not ''' xy = self.memory['preferred_direction'] xy = (xy[0],xy[1],0) if (helpers.compare_tuple(xy,(0,0,0)) or helpers.compare_tuple(( vector[0],vector[1],0),(0,0,0))): return True return (helpers.smallest_angle((vector[0],vector[1],0),xy) <= construction.beam['direction_tolerance_angle'])
def start_repair(self,beam): ''' Initializes the repair of the specified beam. Figures out which direction to travel in and stores it within the robot's memory, then tells it to climb down in a specific direction if necessary. Also sets the number of steps to climb down looking for a support beam. ''' def set_dir(string,coord): ''' Figures out what pos_var should be in order to travel in that direction NO LONGER USED ''' if helpers.compare(coord,0): self.memory[string] = None if coord > 0: self.memory[string] = True else: self.memory[string] = False angle_with_vertical = helpers.smallest_angle(helpers.make_vector( beam.endpoints.i,beam.endpoints.j),(0,0,1)) # Get direction of travel self.memory['preferred_direction'] = self.get_preferred_direction(beam) # Get direction of travel if on the ground based on preferred direction on # the structure self.ground_direction = self.get_preferred_ground_direction( self.memory['preferred_direction']) # Travel down ! self.memory['pos_z'] = False # Store name of repair beam self.memory['broken_beam_name'] = beam.name # Number of steps to search once we find a new beam that is close to # parallel to the beam we are repairing (going down, ie NOT support beam) length = construction.beam['length'] * math.cos( math.radians(construction.beam['support_angle'])) self.memory['new_beam_steps'] = math.floor(length/variables.step_length)+1 self.memory['new_beam_ground_steps'] = (self.memory['new_beam_steps'] if self.ground_direction is None else self.memory['new_beam_steps'] - 1 + math.floor( math.sin(math.radians(angle_with_vertical)) * self.memory['new_beam_steps'])) # So the entire robot knows that we are in repair mode self.repair_mode = True self.search_mode = True
def 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 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*variables.random,variables.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 > construction.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 < construction.beam['direct_repair_limit']: return None else: vertical = (0,0,1) v1,v2 = helpers.make_vector(b_i,c_i), helpers.make_vector(b_i,c_j) # We can't get a direction based on beam locations if helpers.parallel(vertical,v1) and helpers.parallel(vertical,v2): return super(Repairer,self).support_xy_direction() # We can use the current beam to decide the direction elif not helpers.parallel(vertical,current_vector): # pdb.set_trace() # Project onto the xy-plane and negate if current_vector[2] > 0: projection = helpers.make_unit(helpers.scale(-1,(current_vector[0], current_vector[1],0))) else: projection = helpers.make_unit((current_vector[0],current_vector[1],0)) # Add some small disturbance disturbance = helpers.scale(random.uniform(-1,1),(-projection[1], projection[0],projection[2])) result = helpers.sum_vectors(projection,disturbance) # TODO return result elif not helpers.parallel(vertical,repair_vector): return super(Repairer,self).support_xy_direction() else: return super(Repairer,self).support_xy_direction()
def add_angles(box,dictionary): for name, beam in box.items(): # Ignore the beam you're on. if self.beam == None or self.beam.name != name: # Base vector (from which angles are measured) base_vector = helpers.make_vector(pivot,endpoint) # Get the closest points between the beam we want to construct and the # current beam points = helpers.closest_points(beam.endpoints,(pivot,endpoint)) if points != None: # Endpoints (e1 is on a vertical beam, e2 is on the tilted one) e1,e2 = points # If we can actually reach the second point from vertical if (not helpers.compare(helpers.distance(pivot,e2),0) and helpers.distance(pivot,e2) <= 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