Esempio n. 1
0
  def get_preferred_direction(self,beam):
    # Obtain the moment vector
    u1,u2,u3 = beam.global_default_axes()
    m11,m22,m33 = self.get_moment_magnitudes(beam.name)

    # Quick debugging - making sure the torsion doesn't get too high
    if not helpers.compare(m11,0,4):
      #pdb.set_trace()
      pass

    # Sum m22 and m33 (m11 is torsion, which doesn't play a role in the direction)
    moment_vector = helpers.sum_vectors(helpers.scale(m22,u2),helpers.scale(m33,u3))
    '''
    Cross axis-1 with the moment vector to obtain the positive clockwise direction
    This keeps the overall magnitude of moment_vector since u1 is a unit vector
      We are always attempting to repair further up the broken beam (closer to 
      the j-end). The cross will always give us the vector in the right direction
      if we do it this way.
    '''
    twist_direction = helpers.cross(moment_vector,u1)

    # Project the twist direction onto the xy-plane and normalize to have a
    # maximum of 45 degree approach (or, as specified in the variables.py)
    # depending on the ratio of moment magnitude to max_magnitude
    xy_change = (twist_direction[0],twist_direction[1],0)

    # The rotation is parallel to the z-axis, so we don't add disturbance
    if helpers.compare(helpers.length(xy_change),0):
      # Return the normal direction - which way is the beam leaning???
      return super(MomentAwareBuilder,self).get_preferred_direction(beam)

    # We know the direction in which the beam is turning
    else:
      # Get direction of travel (this is a unit vector)
      travel = super(MomentAwareBuilder,self).get_preferred_direction(beam)
      
      # Normalize twist to the maximum moment of force -- structure_check
      normal = helpers.normalize(xy_change,construction.beam['structure_check'])

      # The beam is vertical - check to see how large the normalized moment is
      if travel is None:
        # The change is relatively small, so ignore it
        if helpers.length(normal) <= helpers.ratio(construction.beam['verticality_angle']):
          return travel
        else:
          return helpers.make_unit(normal)

      else:
        scalar = 1 / helpers.ratio(construction.beam['moment_angle_max'])
        scaled_travel = helpers.scale(scalar,travel)
        return helpesr.make_unit(helpers.sum_vectors(normal,scaled_travel))
Esempio n. 2
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,construction.home_center)
      self.ground_direction = (vector if not helpers.compare(helpers.length(
        vector),0) else self.non_zero_xydirection())

    # Find nearby beams to climb on
    result = self.ground()

    # Either there are no nearby beams, we are on repair_mode/search_mode, our beams are 0, or
    # we are constructing a support - so don't mess with direction
    if (result == None or self.repair_mode or self.search_mode or 
      self.num_beams == 0 or self.memory['construct_support']):
      direction = self.get_ground_direction()
      new_location = helpers.sum_vectors(self.location,helpers.scale(self.step,
        helpers.make_unit(direction)))
      self.change_location_local(new_location)

    # Nearby beam, jump on it
    else:
      dist, close_beam, direction = (result['distance'], result['beam'],
        result['direction'])
      # If the beam is within steping distance, just jump on it
      if self.num_beams > 0 and dist <= self.step:
        # Set the ground direction to None (so we walk randomly if we do get off
        # the beam again)
        self.ground_direction = None

        # Then move on the beam
        self.move(direction, close_beam)

      # If we can "detect" a beam, change the ground direction to approach it
      elif self.num_beams > 0 and dist <= variables.local_radius:
        self.ground_direction = direction
        new_location = helpers.sum_vectors(self.location, helpers.scale(
          self.step,helpers.make_unit(direction)))
        self.change_location_local(new_location)
      
      # Local beams, but could not detect (this is redundant)
      else:
        direction = self.get_ground_direction()
        new_location = helpers.sum_vectors(self.location,helpers.scale(
          self.step,helpers.make_unit(direction)))
        self.change_location_local(new_location)
Esempio n. 3
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] > variables.local_radius:
        return None
      else:
        # All the same beans 
        beams = [box[name] for box in boxes if name in box]

        return {  'beam'  : beams[0],
                  'distance' : distances[name],
                  'direction' : vectors[name]}
Esempio n. 4
0
  def move(self, direction, beam):
    '''
    Moves the robot in direction passed in and onto the beam specified
    '''
    length = helpers.length(direction)

    # The direction is smaller than the determined step, so move exactly by 
    # direction
    if length < self.step:
      new_location = helpers.sum_vectors(self.location, direction)
      self.change_location(new_location, beam)

      # call do_action again since we still have some distance left, and update
      # step to reflect how much distance is left to cover
      self.step = self.step - length

      # Reset step in preparation for next timestep
      if helpers.compare(self.step,0):
        self.step == variables.step_length

      # We still have steps to go, so run an analysis if necessary
      elif self.beam is not None:
        # Run analysis before deciding to get the next direction
        if not self.model.GetModelIsLocked() and self.need_data():
          errors = helpers.run_analysis(self.model)
          if errors != '':
            # pdb.set_trace()
            pass

        # Get next direction
        self.next_direction_info = self.get_direction()

        # Unlock the results so that we can actually move
        if self.model.GetModelIsLocked():
          self.model.SetModelIsLocked(False)

        # Move
        self.do_action()

      # We climbed off
      else:
        assert not self.model.GetModelIsLocked()
        
        self.do_action()

    # The direction is larger than the usual step, so move only the step in the 
    # specified direction
    else:
      movement = helpers.scale(self.step, helpers.make_unit(direction))
      new_location = helpers.sum_vectors(self.location, movement)
      self.change_location(new_location, beam)
Esempio n. 5
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
Esempio n. 6
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
Esempio n. 7
0
    def random_direction():
      '''
      Returns a random, new location (direction)
      '''
      # obtain a random direction
      direction = (random.uniform(-1 * self.step, self.step), random.uniform(
        -1 * self.step, self.step), 0)

      # The they can't all be zero!
      if helpers.compare(helpers.length(direction),0):
        return random_direction()
      else:
        step = helpers.scale(self.step,helpers.make_unit(direction))
        predicted_location = helpers.sum_vectors(step, self.location)

        # Check the location
        if helpers.check_location(predicted_location):
          return direction
        else:
          return random_direction()
Esempio n. 8
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
Esempio n. 9
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) <= variables.beam_length):

              # Distance between the two endpoints
              dist = helpers.distance(e1,e2)

              # Vector of beam we want to construct and angle from base_vector
              construction_vector = helpers.make_vector(pivot,e2)
              angle = helpers.smallest_angle(base_vector,construction_vector)

              # Add to dictionary
              if e2 in dictionary:
                assert helpers.compare(dictionary[e2],angle)
              else:
                dictionary[e2] = angle

          # Get the points at which the beam intersects the sphere created by 
          # the vertical beam      
          sphere_points = helpers.sphere_intersection(beam.endpoints,pivot,
            variables.beam_length)
          if sphere_points != None:

            # Cycle through intersection points (really, should be two, though 
            # it is possible for it to be one, in
            # which case, we would have already taken care of this). Either way,
            # we just cycle
            for point in sphere_points:

              # Vector to the beam we want to construct
              construction_vector = helpers.make_vector(pivot,point)
              angle = helpers.smallest_angle(base_vector,construction_vector)

              # Add to dictionary
              if point in dictionary:
                assert helpers.compare(dictionary[point],angle)
              else:
                dictionary[point] = angle

          # Endpoints are also included
          for e in beam.endpoints:
            v = helpers.make_vector(pivot,e)
            l = helpers.length(v)
            if (e not in dictionary and not helpers.compare(l,0) and (
              helpers.compare(l,variables.beam_length) or l < variables.beam_length)):
              angle = helpers.smallest_angle(base_vector,v)
              dictionary[e] = angle

      return dictionary