示例#1
0
    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 > BConstants.beam['support_angle_difference']
示例#2
0
        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 > BConstants.beam[
                    'support_angle_difference']
示例#3
0
文件: robot.py 项目: kandluis/sap2000
  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]}
示例#4
0
    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]
                }
示例#5
0
 def go_to_construction_site(self):
   vector_to_site = helpers.make_vector(self.Body.getLocation(), CONSTRUCTION['center'])
   if helpers.length(vector_to_site) > 10: 
     direction_construction = helpers.make_vector(self.Body.getLocation(), CONSTRUCTION['center'])
     new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
                    self.Body.step, helpers.make_unit(direction_construction)))
     self.Body.changeLocalLocation(new_location)
     return True
   else:
     direction_construction = helpers.make_vector(self.Body.getLocation(), CONSTRUCTION['center'])
     new_location = helpers.sum_vectors(self.Body.getLocation(), direction_construction)
     self.Body.changeLocalLocation(new_location)
     return True
示例#6
0
  def __init__(self,name,structure,location,program):
    super(DumbMovable, self).__init__(name,program)
    # Access to my Python structure
    self.structure = structure

    # Number of steps left in movement
    self.step = ROBOT['step_length']

    # The current location of the robot on the designed structure
    self.location = location

    # The beam on which the robot currently is
    self.beam = None

    # The weight of the robot
    self.weight = ROBOT['load']

    # The direction in which we should move
    self.next_direction_info = None

    # Contains Errors from SAP 2000
    self.error_data = ''

    # The robots all initially move towards the centertower
    self.ground_direction = helpers.make_vector(location,
      CONSTRUCTION['corner'])
示例#7
0
  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(BConstants.beam['support_angle'] - rotation_angle)

      return super(Repairer,self).support_vertical_change(angle=vertical_angle)
示例#8
0
    def __init__(self, name, structure, location, program):
        super(DumbMovable, self).__init__(name, program)
        # Access to my Python structure
        self.structure = structure

        # Number of steps left in movement
        self.step = ROBOT['step_length']

        # The current location of the robot on the designed structure
        self.location = location

        # The beam on which the robot currently is
        self.beam = None

        # The weight of the robot
        self.weight = ROBOT['load']

        # The direction in which we should move
        self.next_direction_info = None

        # Contains Errors from SAP 2000
        self.error_data = ''

        # The robots all initially move towards the centertower
        self.ground_direction = helpers.make_vector(location,
                                                    CONSTRUCTION['corner'])
示例#9
0
    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(BConstants.beam['support_angle'] -
                                 rotation_angle)

            return super(Repairer,
                         self).support_vertical_change(angle=vertical_angle)
示例#10
0
    def get_disturbance(self):
        '''
    Returns the disturbance level for adding a new beam at the tip. This is
    modified so that the disturbance compensates for the angle at which the
    current beam lies (using basic math)
    '''
        def compensate_change(coord, change=PROGRAM['epsilon']):
            '''
      Returns a random direction that is the right sign so that it compensates 
      for the sign of change
      '''
            if helpers.compare(coord, 0):
                return random.uniform(-1 * change, change)
            elif coord < 0:
                return random.uniform(0, change)
            else:
                return random.uniform(-1 * change, 0)

        # We are currently on a beam
        if self.beam is not None:
            i, j = self.beam.endpoints
            v = helpers.make_vector(i, j)
            const_change = lambda x: compensate_change(x, BEAM['random'])
            delta_x, delta_y = const_change(v[0]), const_change(v[1])
            return (delta_x, delta_y, 0)
        else:
            return super(DeflectionRepairer, self).get_disturbance()
示例#11
0
 def go_to_construction_site(self):
     vector_to_site = helpers.make_vector(self.Body.getLocation(),
                                          CONSTRUCTION['center'])
     if helpers.length(vector_to_site) > 10:
         direction_construction = helpers.make_vector(
             self.Body.getLocation(), CONSTRUCTION['center'])
         new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
                        self.Body.step, helpers.make_unit(direction_construction)))
         self.Body.changeLocalLocation(new_location)
         return True
     else:
         direction_construction = helpers.make_vector(
             self.Body.getLocation(), CONSTRUCTION['center'])
         new_location = helpers.sum_vectors(self.Body.getLocation(),
                                            direction_construction)
         self.Body.changeLocalLocation(new_location)
         return True
示例#12
0
    def pickup_beams(self, num=ROBOT["beam_capacity"]):
        """
    Pickup beams by adding weight to the robot and by adding num to number 
    carried
    """
        self.num_beams = self.num_beams + num
        self.weight = self.weight + MATERIAL["beam_load"] * num

        # Set the direction towards the structure
        self.ground_direction = helpers.make_vector(self.location, CONSTRUCTION["center"])
示例#13
0
    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 = BEAM['length'] * math.cos(
            math.radians(BConstants.beam['support_angle']))
        self.memory['new_beam_steps'] = math.floor(
            length / ROBOT['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
示例#14
0
文件: robot.py 项目: pingss/sap2000
    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
示例#15
0
  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 = BEAM['length'] * math.cos(
      math.radians(BConstants.beam['support_angle']))
    self.memory['new_beam_steps'] = math.floor(length/ROBOT['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
示例#16
0
 def move(self, angle='random', step=10):
   def random_NWSE():
     rand = randint(0,3)
     if rand == 0: return 90   #forward
     if rand == 1: return 180  #left
     if rand == 2: return 270  #backward
     if rand == 3: return 0    #right
   if angle == 'NWSE': angle = random_NWSE()
   if angle == 'random': angle = random()*360
   rad = radians(angle)
   direction = helpers.make_vector((0,0,0),(cos(rad),sin(rad),0))
   new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
     step, helpers.make_unit(direction)))
   self.Body.changeLocalLocation(new_location)
   return True
示例#17
0
    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
示例#18
0
  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
示例#19
0
文件: robot.py 项目: kandluis/sap2000
  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
示例#20
0
    def move(self, angle='random', step=10):
        def random_NWSE():
            rand = randint(0, 3)
            if rand == 0: return 90  #forward
            if rand == 1: return 180  #left
            if rand == 2: return 270  #backward
            if rand == 3: return 0  #right

        if angle == 'NWSE': angle = random_NWSE()
        if angle == 'random': angle = random() * 360
        rad = radians(angle)
        direction = helpers.make_vector((0, 0, 0), (cos(rad), sin(rad), 0))
        new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
          step, helpers.make_unit(direction)))
        self.Body.changeLocalLocation(new_location)
        return True
示例#21
0
 def go_home_and_pick_up_beam(self, num_beams = ROBOT['beam_capacity']):
   self.Body.beam = None
   if not self.Body.atHome():
     direction_home = helpers.make_vector(self.Body.getLocation(), HOME['center'])
     new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
                    self.Body.step, helpers.make_unit(direction_home)))
     self.Body.changeLocalLocation(new_location)
   else: 
     self.Body.pickupBeams(num_beams)
     self.Body.addToMemory('wandering',-1)
     self.Body.addToMemory('density_decisions', 0)
     self.Body.addToMemory('climbing_back', 0)
     self.Body.addToMemory('current_beam', None)
     self.Body.addToMemory('prev_beam', None)
     self.Body.addToMemory('same_loc_count', 0)
     self.Body.addToMemory('stuck', False)
   return True
示例#22
0
 def go_home_and_pick_up_beam(self, num_beams=ROBOT['beam_capacity']):
     self.Body.beam = None
     if not self.Body.atHome():
         direction_home = helpers.make_vector(self.Body.getLocation(),
                                              HOME['center'])
         new_location = helpers.sum_vectors(self.Body.getLocation(), helpers.scale( \
                        self.Body.step, helpers.make_unit(direction_home)))
         self.Body.changeLocalLocation(new_location)
     else:
         self.Body.pickupBeams(num_beams)
         self.Body.addToMemory('wandering', -1)
         self.Body.addToMemory('density_decisions', 0)
         self.Body.addToMemory('climbing_back', 0)
         self.Body.addToMemory('current_beam', None)
         self.Body.addToMemory('prev_beam', None)
         self.Body.addToMemory('same_loc_count', 0)
         self.Body.addToMemory('stuck', False)
     return True
示例#23
0
  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] = visual.cylinder(pos=i,axis=unit_axis,
      radius=MATERIAL['outside_diameter'],color=(0,1,0))

    # Extrude the beam from the robot
    while scale <= BEAM['length']:
      axis = helpers.scale(scale,unit_axis)
      self.beams[name].axis = axis
      scale += change
      time.sleep((change / 120) * self.inverse_speed)
示例#24
0
    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] = visual.cylinder(pos=i,
                                           axis=unit_axis,
                                           radius=MATERIAL['outside_diameter'],
                                           color=(0, 1, 0))

        # Extrude the beam from the robot
        while scale <= BEAM['length']:
            axis = helpers.scale(scale, unit_axis)
            self.beams[name].axis = axis
            scale += change
            time.sleep((change / 120) * self.inverse_speed)
示例#25
0
    def climb_off(self, loc):
        """
    Returns whether or not the robot should climb off the structure. Additionally,
    sets some special variables
    """
        # On the xy-plane with no beams OR repairing
        if helpers.compare(loc[2], 0) and (self.num_beams == 0 or self.search_mode):

            # Not repairing, so calculate direction
            if not self.search_mode:
                direction = helpers.make_vector(self.location, HOME["center"])
                direction = (direction[0], direction[1], 0)
                self.ground_direction = direction

            return True

        else:

            # Resetting to None if not in search_mode
            self.ground_direction = None if not self.search_mode else self.ground_direction

            return False
示例#26
0
    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
示例#27
0
  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
示例#28
0
    def add_beam(self, p1, p1_name, p2, p2_name, name):
        ''' 
    Function to add the name and endpoint combination of a beam
    to all of the boxes that contain it. Returns the number of boxes (which 
    should be at least 1)
    '''
        def addbeam(beam, p):
            '''
      Function to add an arbitrary beam to its respective box. Returns number of
      boxes changed. Also takes care of finding intersecting beams to the added
      beam and adding the joints to both beams. Uses the point p (which should 
      be on the beam), to calculate the box it should be added to
      '''
            # Getting indeces
            xi, yi, zi = self.__get_indeces(p)

            # Getting the box and all of the other beams in the box
            try:
                box = self.model[xi][yi][zi]
            except IndexError:
                print("Addbeam is incorrect. Accessing box not defined.")
                return False

            # Finding intersection points with other beams
            for key in box:
                point = helpers.intersection(box[key].endpoints,
                                             beam.endpoints)
                # If they intersect, add the joint to both beams
                if point != None:
                    assert key == box[key].name
                    if not beam.addjoint(point, box[key]):
                        sys.exit("Could not add joint to {} at {}".format(
                            beam.name, str(point)))
                    if not box[key].addjoint(point, beam):
                        sys.exit("Could not add joint to {} at {}".format(
                            box[key].name, str(point)))

            # update the box
            self.model[xi][yi][zi] = box

            # Adding beam to boxes that contain it based on the point p.
            try:
                if beam.name in self.model[xi][yi][zi]:
                    return 0
                else:
                    self.model[xi][yi][zi][beam.name] = beam
                    return 1
            except IndexError:
                raise OutofBox(
                    "The coordinate {}, is not in the structure. Something\
          went wront in addpoint()".format(p))

        # Create the beam
        new_beam = Beam(name, (p1, p2), (p1_name, p2_name))

        # Add to all boxes it is located in
        total_boxes = 0
        try:
            for point in self.__path(p1, p2):
                total_boxes += addbeam(new_beam, point)
        except OutofBox as e:
            print(e)
            return False

        # If something went wrong, kill the program
        assert total_boxes > 0

        # If showing the visualization, add the cylinder to the structure
        if self.visualization:
            temp = visual.cylinder(pos=p1,
                                   axis=helpers.make_vector(p1, p2),
                                   radius=MATERIAL['outside_diameter'])
            temp.color = (0, 1, 1)

        # Safe visualization data
        self.visualization_data += "{}:{}-{}<>".format(
            str(new_beam.name), str(helpers.round_tuple(p1, 3)),
            str(helpers.round_tuple(p2, 3)))

        # Add a beam to the structure count and increase height if necessary
        self.tubes += 1
        self.height = max(p1[2], p2[2], self.height)

        return total_boxes
示例#29
0
  def get_build_vector(self, build_angle, direction):
    current_beam_direction = self.Body.beam.global_default_axes()[0]
    (x_dir, y_dir, z_dir) = current_beam_direction
    (x, y, z) = (0, 0, 0)
    # convert to complement of polar angle (=angle up from x-y)
    build_angle = radians(90 - build_angle) 
    #direction = None
    '''
    #connect to nearby beam if possible.
    best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
      height = -1*sin(build_angle)
      radius = cos(build_angle)
      random_angle = radians(random()*360)
      for theta in range(0,360,1):
        rads = radians(theta)
        x, y, z = radius*cos(random_angle+rads), radius*sin(random_angle+rads), height
        x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
        x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
          BEAM['length'], helpers.make_unit((x,y,z))))
        if z_r <= 0: return (x,y,z)
        if z_r < best_z: best_x, best_y, best_z = x, y, z
      return (x,y,z)
    '''
    if direction == None:
      random_angle = radians(random()*360)
      height = sin(build_angle)
      radius = cos(build_angle)
      x, y, z = radius*cos(random_angle), radius*sin(random_angle), height
      print(x,y,z)
      print(current_beam_direction)

    if direction == 'center': 
      height = sin(build_angle)
      radius = cos(build_angle)
      position_center = CONSTRUCTION['center']
      position_center = (position_center[0], \
        position_center[1], self.Body.getLocation()[2])
      direction_construction = helpers.make_vector(self.Body.getLocation(), position_center)
      endpoint = helpers.scale(radius, helpers.make_unit(direction_construction))
      x, y, z = endpoint[0], endpoint[1], height

    if direction == 'outward':
      height = sin(build_angle)
      radius = cos(build_angle)
      position_center = CONSTRUCTION['center']
      position_center = (position_center[0], \
        position_center[1], self.Body.getLocation()[2])
      direction_construction = helpers.make_vector(self.Body.getLocation(), position_center)
      endpoint = helpers.scale(radius, helpers.make_unit(direction_construction))
      x, y, z = -1*endpoint[0], -1*endpoint[1], height

    if direction == 'upwards':
      x, y, z = 0, 0, 1

    if direction == 'ground':
      best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
      height = -1*sin(build_angle)
      radius = cos(build_angle)
      random_angle = radians(random()*360)
      for theta in range(0,360,1):
        rads = radians(theta)
        x, y, z = radius*cos(random_angle+rads), radius*sin(random_angle+rads), height
        x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
        x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
          BEAM['length'], helpers.make_unit((x,y,z))))
        if z_r <= 0: return (x,y,z)
        if z_r < best_z: best_x, best_y, best_z = x, y, z
      return (x,y,z)

    x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
    # prevent hanging beams that are just above the ground after rotation
    if direction == 'outward' or direction == 'center' and z < 0:
      z = -1*z

    return (x, y, z)
示例#30
0
  def run(self,fullscreen = True, inverse_speed=.25):
    if self.data == []:
      print("No data has been loaded. Cannot run simulation.")
    else:
      # Store inverse speed
      self.inverse_speed = inverse_speed

      # Setup the scene
      scene = setup_scene(fullscreen)

      # Setup basic
      setup_base()

      # Cycle through timestep data
      timestep = 1
      for swarm_step, swarm_color,structure_step,struct_color in self.data:
        for name, locations in swarm_step:

          # Create the object
          if name not in self.workers:
            self.workers[name] = visual.sphere(pos=locations[0],
              radius=VISUALIZATION['robot_size']/2,make_trail=False)
            self.workers[name].color = (1,0,1)

          # Change the objects position
          else:
            self.workers[name].pos = locations[0]

        # Set the color
        for name, colors in swarm_color:
          self.workers[name].color = colors[0]

        # Add beams if any
        for name,coords in structure_step:
          i,j = coords

          # Add new beam if not in dictionary
          if name not in self.beams:
            self.add_beam(name,i,j)
          # Otherwise, this means the beam has deflected, so change the position
          else:
            # Scale visualization
            scale = VISUALIZATION['scaling']

            # Old endpoints (we use this to scale at different values each time
            # we run)
            old_i = self.beams[name].pos
            old_j = helpers.sum_vectors(self.beams[name].pos,self.beams[name].axis)

            # Calculate changes from old location to new location
            i_change = helpers.scale(scale,helpers.make_vector(old_i,i))
            j_change = helpers.scale(scale,helpers.make_vector(old_j,j))

            # Calculate the new location based on the scaled chage
            new_i = helpers.sum_vectors(old_i,i_change) 
            new_j = helpers.sum_vectors(old_j,j_change)
            new_axis = helpers.make_vector(new_i,new_j)

            # Update the visualization
            self.beams[name].pos = new_i
            self.beams[name].axis = new_axis
          
          # Update window dimensions
          limit = max(j)
          if limit > max(scene.range):
            scene.range = (limit,limit,limit)
            scene.center = helpers.scale(.5,helpers.sum_vectors(
              CONSTRUCTION['corner'],scene.range))

        # Change the color of the beams
        for name,colors in struct_color:
          try:
            self.beams[name].color = colors[0]
          except IndexError:
            print("A nonexistant beam is beam is to be recolored!")

        # Check key_presses
        if scene.kb.keys:
          s = scene.kb.getkey()
          if len(s) == 1:
            # Move faster
            if s == 'f':
              inverse_speed /= 2
            # Move more slowly
            elif s == 's':
              inverse_speed *= 2
            # Pause or continue
            elif s == ' ':
              self.pause(scene)
            else:
              pass

        time.sleep(inverse_speed)
        timestep += 1
示例#31
0
    def __path(self, coord1, coord2):
        '''
    Traverses the line formed between coord1 and coord2. Returns a list of 
    points on the line that lie in different boxes. This will NOT miss any 
    points that are in different boxes. The basic method is to find the 
    intersection of the line with one of the faces of the cube formed by the 
    box. There should not be multiple points, but this has not been proven. It 
    might return two points that are in the same box.
    '''
        def get_sign(n):
            '''
      Returns the sign of the number
      '''
            if n == 0:
                return None
            else:
                return n > 0

        # move from coord1 to coord2. Here, we determine the sign of the change
        # (pos = True, neg = False, or None)
        signs = (get_sign(coord2[0] - coord1[0]),
                 get_sign(coord2[1] - coord1[1]),
                 get_sign(coord2[2] - coord1[2]))
        line = helpers.make_vector(coord1, coord2)

        def crawl(point):
            # get the current box boundaries (bottom left corner-(0,0,0) is starting)
            # and coordinates
            xi, yi, zi = self.__get_indeces(point)
            bounds = xi * self.box_size[0], yi * self.box_size[
                1], zi * self.box_size[2]

            # This is defined here to have access to the above signs and bounds
            def closest(p):
                '''
        Returns which coordinate in p is closest to the boundary of a box 
        (x = 0, y = 1, z = 2) if moving along the line, and the absolute change
        in that coordinate.
        '''
                def distance(i):
                    '''
          i is 0,1,2 for x,y,z
          '''
                    if signs[i] == None:
                        # This will never be the minimum. Makes later code easier
                        return None
                    elif signs[i]:
                        return abs(p[i] - (bounds[i] + self.box_size[i]))
                    else:
                        return abs(p[i] - bounds[i])

                # Find the shortest time distance (ie, distance/velocity)
                index = None
                for i in range(3):
                    dist, vel = distance(i), abs(line[i])
                    if dist is not None and vel != 0:
                        if index is None:
                            index = i
                        else:
                            min_time = distance(index) / abs(line[index])
                            index = i if dist / vel < min_time else index

                return index, distance(index)

            # In crawl, we obtain the coordinate closests to an edge (index), and its
            # absolute distance from that edge
            index, distance = closest(point)

            # The change is the line scaled so that the right coordinate changes the
            # amount necessary to cross into the next box. This means that we scale it
            # and also add a teeny bit so as to push it into the right box. This is
            # the scaled version, exactly the distance we need to move
            move = helpers.scale(distance / abs(line[index]), line)
            # Here we scale the line again by epsilon/2. This is our push
            push = helpers.scale(PROGRAM['epsilon'] / 2, line)
            # The total change is the addition of these two
            change = helpers.sum_vectors(move, push)

            # make sure we are changing the right amount
            assert helpers.compare(abs(move[index]), distance)

            # The new initial coordinate in the next box
            new_point = helpers.sum_vectors(point, change)

            return new_point

        points, passed, temp = [coord2], False, coord1
        while not passed:
            points.append(temp)
            temp = crawl(temp)

            # Check the next coordinate to see if we have moved past the endpoint
            for i in range(3):
                if signs[i] != None:
                    # Movings positively, so set to True if our new_point has a larger
                    # positive coordinate
                    # Moving negatively, so set to True if our new_point has a smaller
                    # positive coordinate
                    passed = (temp[i] > coord2[i] + PROGRAM['epsilon'] / 2
                              if signs[i] else
                              temp[i] < coord2[i] - PROGRAM['epsilon'] / 2)

        return points
示例#32
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
示例#33
0
    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 < BConstants.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()
示例#34
0
文件: robot.py 项目: pingss/sap2000
        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
示例#35
0
    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 * BEAM['random'], BEAM['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 > BConstants.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()
示例#36
0
  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*BEAM['random'],BEAM['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 > BConstants.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()
示例#37
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
示例#38
0
    def get_build_vector(self, build_angle, direction):
        current_beam_direction = self.Body.beam.global_default_axes()[0]
        (x_dir, y_dir, z_dir) = current_beam_direction
        (x, y, z) = (0, 0, 0)
        # convert to complement of polar angle (=angle up from x-y)
        build_angle = radians(90 - build_angle)
        #direction = None
        '''
    #connect to nearby beam if possible.
    best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
      height = -1*sin(build_angle)
      radius = cos(build_angle)
      random_angle = radians(random()*360)
      for theta in range(0,360,1):
        rads = radians(theta)
        x, y, z = radius*cos(random_angle+rads), radius*sin(random_angle+rads), height
        x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
        x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
          BEAM['length'], helpers.make_unit((x,y,z))))
        if z_r <= 0: return (x,y,z)
        if z_r < best_z: best_x, best_y, best_z = x, y, z
      return (x,y,z)
    '''
        if direction == None:
            random_angle = radians(random() * 360)
            height = sin(build_angle)
            radius = cos(build_angle)
            x, y, z = radius * cos(random_angle), radius * sin(
                random_angle), height
            print(x, y, z)
            print(current_beam_direction)

        if direction == 'center':
            height = sin(build_angle)
            radius = cos(build_angle)
            position_center = CONSTRUCTION['center']
            position_center = (position_center[0], \
              position_center[1], self.Body.getLocation()[2])
            direction_construction = helpers.make_vector(
                self.Body.getLocation(), position_center)
            endpoint = helpers.scale(radius,
                                     helpers.make_unit(direction_construction))
            x, y, z = endpoint[0], endpoint[1], height

        if direction == 'outward':
            height = sin(build_angle)
            radius = cos(build_angle)
            position_center = CONSTRUCTION['center']
            position_center = (position_center[0], \
              position_center[1], self.Body.getLocation()[2])
            direction_construction = helpers.make_vector(
                self.Body.getLocation(), position_center)
            endpoint = helpers.scale(radius,
                                     helpers.make_unit(direction_construction))
            x, y, z = -1 * endpoint[0], -1 * endpoint[1], height

        if direction == 'upwards':
            x, y, z = 0, 0, 1

        if direction == 'ground':
            best_x, best_y, best_z = float('Inf'), float('Inf'), float('Inf')
            height = -1 * sin(build_angle)
            radius = cos(build_angle)
            random_angle = radians(random() * 360)
            for theta in range(0, 360, 1):
                rads = radians(theta)
                x, y, z = radius * cos(random_angle +
                                       rads), radius * sin(random_angle +
                                                           rads), height
                x, y, z = helpers.rotate_vector_3D((x, y, z),
                                                   current_beam_direction)
                x_r, y_r, z_r = helpers.sum_vectors(self.Body.getLocation(),helpers.scale(\
                  BEAM['length'], helpers.make_unit((x,y,z))))
                if z_r <= 0: return (x, y, z)
                if z_r < best_z: best_x, best_y, best_z = x, y, z
            return (x, y, z)

        x, y, z = helpers.rotate_vector_3D((x, y, z), current_beam_direction)
        # prevent hanging beams that are just above the ground after rotation
        if direction == 'outward' or direction == 'center' and z < 0:
            z = -1 * z

        return (x, y, z)
示例#39
0
  def __path(self,coord1, coord2):
    '''
    Traverses the line formed between coord1 and coord2. Returns a list of 
    points on the line that lie in different boxes. This will NOT miss any 
    points that are in different boxes. The basic method is to find the 
    intersection of the line with one of the faces of the cube formed by the 
    box. There should not be multiple points, but this has not been proven. It 
    might return two points that are in the same box.
    '''
    def get_sign(n):
      '''
      Returns the sign of the number
      '''
      if n == 0:
        return None
      else:
        return n > 0

    # move from coord1 to coord2. Here, we determine the sign of the change 
    # (pos = True, neg = False, or None)
    signs = (get_sign(coord2[0] - coord1[0]), get_sign(coord2[1] - coord1[1]),
      get_sign(coord2[2] - coord1[2]))
    line = helpers.make_vector(coord1,coord2)

    def crawl(point):
      # get the current box boundaries (bottom left corner-(0,0,0) is starting)
      # and coordinates
      xi, yi, zi = self.__get_indeces(point)
      bounds = xi*self.box_size[0], yi*self.box_size[1], zi*self.box_size[2]

      # This is defined here to have access to the above signs and bounds
      def closest(p):
        '''
        Returns which coordinate in p is closest to the boundary of a box 
        (x = 0, y = 1, z = 2) if moving along the line, and the absolute change
        in that coordinate.
        '''
        def distance(i):
          '''
          i is 0,1,2 for x,y,z
          '''
          if signs[i] == None:
            # This will never be the minimum. Makes later code easier
            return None
          elif signs[i]:
            return abs(p[i] - (bounds[i] + self.box_size[i]))
          else:
            return abs(p[i] - bounds[i])

        # Find the shortest time distance (ie, distance/velocity)
        index = None
        for i in range(3):
          dist, vel = distance(i), abs(line[i])
          if dist is not None and vel != 0:
            if index is None:
              index = i
            else:
              min_time = distance(index) / abs(line[index])
              index = i if dist / vel < min_time else index

        return index, distance(index)

      # In crawl, we obtain the coordinate closests to an edge (index), and its 
      # absolute distance from that edge
      index, distance = closest(point)

      # The change is the line scaled so that the right coordinate changes the 
      # amount necessary to cross into the next box. This means that we scale it
      # and also add a teeny bit so as to push it into the right box. This is 
      # the scaled version, exactly the distance we need to move
      move = helpers.scale(distance / abs(line[index]), line)
      # Here we scale the line again by epsilon/2. This is our push
      push = helpers.scale(PROGRAM['epsilon'] / 2, line)
      # The total change is the addition of these two
      change = helpers.sum_vectors(move,push)

      # make sure we are changing the right amount
      assert helpers.compare(abs(move[index]), distance)

      # The new initial coordinate in the next box
      new_point = helpers.sum_vectors(point,change)

      return new_point

    points, passed,temp = [coord2], False, coord1
    while not passed:
      points.append(temp)
      temp = crawl(temp)

      # Check the next coordinate to see if we have moved past the endpoint
      for i in range(3):
        if signs[i] != None:
          # Movings positively, so set to True if our new_point has a larger 
          # positive coordinate
          # Moving negatively, so set to True if our new_point has a smaller 
          # positive coordinate
          passed = (temp[i] > coord2[i] + PROGRAM['epsilon'] / 2 if signs[i] 
            else temp[i] < coord2[i] - PROGRAM['epsilon'] / 2)

    return points
示例#40
0
    def run(self, fullscreen=True, inverse_speed=.25):
        if self.data == []:
            print("No data has been loaded. Cannot run simulation.")
        else:
            # Store inverse speed
            self.inverse_speed = inverse_speed

            # Setup the scene
            scene = setup_scene(fullscreen)

            # Setup basic
            setup_base()

            # Cycle through timestep data
            timestep = 1
            for swarm_step, swarm_color, structure_step, struct_color in self.data:
                for name, locations in swarm_step:

                    # Create the object
                    if name not in self.workers:
                        self.workers[name] = visual.sphere(
                            pos=locations[0],
                            radius=VISUALIZATION['robot_size'] / 2,
                            make_trail=False)
                        self.workers[name].color = (1, 0, 1)

                    # Change the objects position
                    else:
                        self.workers[name].pos = locations[0]

                # Set the color
                for name, colors in swarm_color:
                    self.workers[name].color = colors[0]

                # Add beams if any
                for name, coords in structure_step:
                    i, j = coords

                    # Add new beam if not in dictionary
                    if name not in self.beams:
                        self.add_beam(name, i, j)
                    # Otherwise, this means the beam has deflected, so change the position
                    else:
                        # Scale visualization
                        scale = VISUALIZATION['scaling']

                        # Old endpoints (we use this to scale at different values each time
                        # we run)
                        old_i = self.beams[name].pos
                        old_j = helpers.sum_vectors(self.beams[name].pos,
                                                    self.beams[name].axis)

                        # Calculate changes from old location to new location
                        i_change = helpers.scale(scale,
                                                 helpers.make_vector(old_i, i))
                        j_change = helpers.scale(scale,
                                                 helpers.make_vector(old_j, j))

                        # Calculate the new location based on the scaled chage
                        new_i = helpers.sum_vectors(old_i, i_change)
                        new_j = helpers.sum_vectors(old_j, j_change)
                        new_axis = helpers.make_vector(new_i, new_j)

                        # Update the visualization
                        self.beams[name].pos = new_i
                        self.beams[name].axis = new_axis

                    # Update window dimensions
                    limit = max(j)
                    if limit > max(scene.range):
                        scene.range = (limit, limit, limit)
                        scene.center = helpers.scale(
                            .5,
                            helpers.sum_vectors(CONSTRUCTION['corner'],
                                                scene.range))

                # Change the color of the beams
                for name, colors in struct_color:
                    try:
                        self.beams[name].color = colors[0]
                    except IndexError:
                        print("A nonexistant beam is beam is to be recolored!")

                # Check key_presses
                if scene.kb.keys:
                    s = scene.kb.getkey()
                    if len(s) == 1:
                        # Move faster
                        if s == 'f':
                            inverse_speed /= 2
                        # Move more slowly
                        elif s == 's':
                            inverse_speed *= 2
                        # Pause or continue
                        elif s == ' ':
                            self.pause(scene)
                        else:
                            pass

                time.sleep(inverse_speed)
                timestep += 1
示例#41
0
  def add_beam(self,p1,p1_name,p2,p2_name,name):
    ''' 
    Function to add the name and endpoint combination of a beam
    to all of the boxes that contain it. Returns the number of boxes (which 
    should be at least 1)
    '''
    def addbeam(beam,p):
      '''
      Function to add an arbitrary beam to its respective box. Returns number of
      boxes changed. Also takes care of finding intersecting beams to the added
      beam and adding the joints to both beams. Uses the point p (which should 
      be on the beam), to calculate the box it should be added to
      '''
      # Getting indeces
      xi,yi,zi = self.__get_indeces(p)

      # Getting the box and all of the other beams in the box
      try:
        box = self.model[xi][yi][zi]
      except IndexError:
        print ("Addbeam is incorrect. Accessing box not defined.")
        return False

      # Finding intersection points with other beams
      for key in box:
        point = helpers.intersection(box[key].endpoints, beam.endpoints)
        # If they intersect, add the joint to both beams
        if point != None:
          assert key == box[key].name
          if not beam.addjoint(point, box[key]):
            sys.exit("Could not add joint to {} at {}".format(beam.name,
              str(point)))
          if not box[key].addjoint(point, beam):
            sys.exit("Could not add joint to {} at {}".format(box[key].name,
              str(point)))

      # update the box
      self.model[xi][yi][zi] = box

      # Adding beam to boxes that contain it based on the point p.
      try:
        if beam.name in self.model[xi][yi][zi]:
          return 0
        else:
          self.model[xi][yi][zi][beam.name] = beam
          return 1
      except IndexError:
        raise OutofBox ("The coordinate {}, is not in the structure. Something\
          went wront in addpoint()".format(p))

    # Create the beam
    new_beam = Beam(name,(p1,p2),(p1_name,p2_name))

    # Add to all boxes it is located in
    total_boxes = 0
    try:
      for point in self.__path(p1, p2):
        total_boxes += addbeam(new_beam,point)
    except OutofBox as e:
      print (e)
      return False

    # If something went wrong, kill the program
    assert total_boxes > 0

    # If showing the visualization, add the cylinder to the structure
    if self.visualization:
      temp = visual.cylinder(pos=p1,axis=helpers.make_vector(p1,p2),
        radius=MATERIAL['outside_diameter'])
      temp.color = (0,1,1)

    # Safe visualization data
    self.visualization_data += "{}:{}-{}<>".format(str(new_beam.name),str(
      helpers.round_tuple(p1,3)),str(helpers.round_tuple(p2,3)))

    # Add a beam to the structure count and increase height if necessary
    self.tubes += 1
    self.height = max(p1[2],p2[2],self.height)

    return total_boxes
示例#42
0
  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 < BConstants.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()
示例#43
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
示例#44
0
    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, 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 <= ROBOT["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)