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 get_repair_beam_direction(self): ''' Returns the xy direction at which the support beam should be set (if none is found). Currently, we just add a bit of disturbace while remaining within the range that the robot was set to search. ''' direction = self.memory['preferred_direction'] # No preferred direction, so beam was vertically above use if xy is None: return None # Add a bit of disturbace else: # Project onto xy_plane and make_unit xy = helpers.make_unit((direction[0],direction[1],0)) xy_perp = (-1 * xy[1],xy[0],0) # Obtain disturbance based on "search_angle" limit = helpers.ratio(construction.beam['direction_tolerance_angle']) scale = random.uniform(-1 * limit,limit) disturbance = helpers.scale(scale,xy_perp) return helpers.sum_vectors(disturbance,xy)
def get_disturbance(self): ''' Returns the disturbance level for adding a new beam at the tip (in this class, the disturbance is random at a level set in variables.random) ''' change = variables.random return helpers.make_unit((random.uniform(-change,change), random.uniform(-change,change),0))
def support_beam_endpoint(self): ''' Returns the endpoint for construction of a support beam ''' # Add beam_directions plus vertical change based on angle ratio (tan) ratio = helpers.ratio(self.get_angle('support_angle')) vertical = self.support_vertical_change() xy_dir = self.support_xy_direction() if xy_dir is None or vertical is None: direction = (0,0,1) else: xy_dir = helpers.make_unit(xy_dir) direction = helpers.make_unit(helpers.sum_vectors(xy_dir,vertical)) # Calculate endpoints endpoint = helpers.sum_vectors(self.location,helpers.scale( construction.beam['length'],direction)) return endpoint
def wander(self): ''' When a robot is not on a structure, it wanders around randomly. The wandering is restricted to the 1st octant in global coordinates. If the robot is near enough a beam to be on it in the next time step, it jumps on the beam. The robots have a tendency to scale the structure, per se, but are restricted to their immediate surroundings. ''' # Check to see if robot is on a beam. If so, pick between moving on it or # off it. result = self.ground() # Nothign nearby if result is None: # Get direction direction = self.get_ground_direction() new_location = helpers.sum_vectors(self.location,helpers.scale(self.step, helpers.make_unit(direction))) # Move self.change_location_local(new_location) # A beam is nearby else: dist, close_beam, direction = (result['distance'], result['beam'], result['direction']) # If close enough, just jump on it if dist < self.step: self.move(direction,close_beam) # Otherwise, walk towards it else: # Scale direction to be step_size direction = helpers.scale(self.step,helpers.make_unit(direction)) new_location = helpers.sum_vectors(self.location,helpers.scale( self.step, helpers.make_unit(direction))) # Move self.change_location_local(new_location)
def 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 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_ground_direction(self): ''' In future classes, this function can be altered to return a preferred direction, but currently it only returns a random feasable direction if no direction is assigned for the robot (self.ground_direction) ''' def random_direction(): ''' Returns a random, new location (direction) ''' # obtain a random direction direction = (random.uniform(-1 * self.step, self.step), random.uniform( -1 * self.step, self.step), 0) # The they can't all be zero! if helpers.compare(helpers.length(direction),0): return random_direction() else: step = helpers.scale(self.step,helpers.make_unit(direction)) predicted_location = helpers.sum_vectors(step, self.location) # Check the location if helpers.check_location(predicted_location): return direction else: return random_direction() # If we have a currently set direction, check to see if we will go out of # bounds. if self.ground_direction != None: step = helpers.scale(self.step,helpers.make_unit(self.ground_direction)) predicted_location = helpers.sum_vectors(step, self.location) # We are going out of bounds, so set the direction to none and call # yourself again (to find a new location) if not helpers.check_location(predicted_location): self.ground_direction = None return self.get_ground_direction() # Here, we return the right direction else: assert self.ground_direction != None return self.ground_direction # We don't have a direction, so pick a random one (it is checked when we # pick it) else: self.ground_direction = random_direction() return self.ground_direction
def get_preferred_direction(self,beam): ''' Returns the preferred direction - this is the direction towards which the robot wants to move when looking for an already set support tube. The direction is a unit vector ''' # Calculate direction of repair (check 0 dist, which means it is perfectly # vertical!) i, j = beam.endpoints.i, beam.endpoints.j v1 = helpers.make_vector(self.location,j) v2 = helpers.make_vector(i,self.location) l1,l2 = helpers.length(v1), helpers.length(v2) # v1 is non-zero and it is not vertical if not (helpers.compare(l1,0) or helpers.is_vertical(v1)): return helpers.make_unit(v1) # v2 is non-zero and it is not vertical elif not (helpers.compare(l2,0) or helpers.is_vertical(v2)): return helpers.make_unit(v2) # No preferred direction because the beam is perfectly vertical else: return None
def get_moment_magnitudes(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.current_state()) return 0 # Find index of closest data_point close_index, i = 0, 0 shortest_distance = None distances = results[3] for i_distance in distances: # Get beam endpoints to calculate global position of moment i_end,j_end = self.structure.get_endpoints(name,self.location) beam_direction = helpers.make_unit(helpers.make_vector(i_end,j_end)) point = helpers.sum_vectors(i_end,helpers.scale(i_distance, beam_direction)) distance = helpers.distance(pivot,point) # If closer than the current closes point, update information if shortest_distance is None or distance < shortest_distance: close_index = i shortest_distance = distance i += 1 # Make sure index is indexable assert close_index < results[1] # Now that we have the closest moment, calculate sqrt(m2^2+m3^2) m11 = results[12][close_index] m22 = results[13][close_index] m33 = results[14][close_index] return m11,m22,m33
def support_xy_direction(self): ''' Returns the direction in which the support beam should be constructed ''' # Check to see if direction is vertical default_direction = self.get_repair_beam_direction() # The beam was vertical if default_direction is None: xy_dir = self.non_zero_xydirection() # Use the default direction else: xy_dir = default_direction return helpers.make_unit(xy_dir)
def non_zero_xydirection(self): ''' Returns a non_zero list of random floats with zero z component. The direction returned is a unit vector. ''' # Random list tuple_list = ([random.uniform(-1,1),random.uniform(-1,1), random.uniform(-1,1)]) # All are non-zero if all(tuple_list): tuple_list[2] = 0 return helpers.make_unit(tuple(tuple_list)) # All are zero - try again else: return self.non_zero_xydirection()
def add_beam(self,name,i,j): ''' Visualization for the wiggling effect when adding a beam to the structure ''' scale = 1 change = 1 unit_axis = helpers.make_unit(helpers.make_vector(i,j)) # Create the beam self.beams[name] = cylinder(pos=i,axis=unit_axis, radius=variables.outside_diameter,color=(0,1,0)) # Extrude the beam from the robot while scale <= construction.beam['length']: axis = helpers.scale(scale,unit_axis) self.beams[name].axis = axis scale += change time.sleep((change / 120) * self.inverse_speed)
def get_default(self,ratio_coord,vertical_coord): ''' Returns the coordinate onto which the j-point of the beam to construct should lie ''' # No vertical coordinate this time, since we will use a leaning one coord = super(LeanRepairer,self).get_default(ratio_coord,None) if coord is not None: return coord # We need to return one that leans else: xy_dir = self.non_zero_xydirection() scale = 1 / helpers.ratio(construction.beam['construction_angle']) vertical = helpers.scale(scale,construction.beam['vertical_dir_set']) direction = helpers.make_unit(helpers.sum_vectors(xy_dir,vertical)) endpoint = helpers.sum_vectors(self.location,helpers.scale( construction.beam['length'],direction)) return endpoint
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 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 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 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)