def next_dict(item,dictionary): ''' Returns whether or not the value (a direction vector) is found inside of dictionary (ie, looks for parallel directions) ''' key, value = item temp = {} for test_key,test_values in dictionary.items(): # Keys are the same, vectors are parallel (point in same dir too) if key == test_key: for test_value in test_values: if (helpers.parallel(value,test_value) and helpers.dot(value,test_value) > 0): if test_key in temp: temp[test_key].append(test_value) else: temp[test_key] = [test_value] # No values are parallel, so return None if temp == {}: return None # Pick a direction from those that are parallel (so we continue moving) # in our preferred direction else: return self.pick_direction(temp)
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 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()