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 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 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 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 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 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_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 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