예제 #1
0
  public AngleLocalization(
      List<PolygonObstacle> cSpaceObstacles,
      Rectangle2D.Double cSpaceWorld,
      Point2D.Double robotStart,
      Fiducial[] fiducialPairs) {

    this.cSpaceObstacles = cSpaceObstacles;
    this.cSpaceWorld = cSpaceWorld;
    this.robotStart = robotStart;
    this.fiducialPairs = fiducialPairs;

    particles = new ArrayList<OdoPoint>();
    probabilities = new ArrayList<Double>();

    double delta_x = 0;
    double delta_y = 0;
    double delta_theta = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (1 - 2 * Math.random());
      delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (1 - 2 * Math.random());
      delta_theta = 2 * Math.PI * INITIAL_NOISE * (1 - 2 * Math.random());
      particles.add(new OdoPoint(robotStart.x + delta_x, robotStart.y + delta_y, delta_theta));
      probabilities.add(1.0 / NUM_PARTICLES);
    }

    prev_odo_x = robotStart.x;
    prev_odo_y = robotStart.y;
    prev_odo_theta = 0;
    localPoint = new OdoPoint(robotStart.x, robotStart.y, 0);
  }
예제 #2
0
 public List<Vector2D> getShapeForDrawing() {
   List<Vector2D> theList = new ArrayList<Vector2D>();
   theList.add(d.getPosition());
   for (Node n : Arrays.asList(dt, tv, v)) {
     theList.add(n.getPosition());
   }
   return theList;
 }
예제 #3
0
  /**
   * Compute the (inward) normal of each node by taking a weighted average of the (inward) surface
   * normals of the edges connected to it. Longer edges are given more weight.
   */
  private void computeWeightedNormals() {
    for (int i = 0, n = nodeList.size(); i < n; i++) {
      Node prevNode = nodeList.get(i);
      Node curNode = nodeList.get((i + 1) % n);
      Node nextNode = nodeList.get((i + 2) % n);

      Vector2D prevVector = curNode.getPosition().subtract(prevNode.getPosition()).rotate270();
      Vector2D nextVector = nextNode.getPosition().subtract(curNode.getPosition()).rotate270();

      normals.put(curNode, prevVector.add(nextVector).normalize());
    }
  }
예제 #4
0
 public static String getInheritedAttribute(
     String name, DOMInput in, List<Map<String, String>> styles) {
   List<String> values = in.getInheritedAttribute(name);
   for (int i = values.size() - 1; i >= 0; i--) {
     if (values.get(i) != null) {
       return values.get(i);
     }
     if (styles.get(i) != null && styles.get(i).containsKey(name)) {
       return styles.get(i).get(name);
     }
   }
   return null;
 }
예제 #5
0
  /**
   * Compute the (inward) normal at each node by averaging the unit direction vectors of the two
   * edges connected to it.
   */
  private void computeNormals() {
    for (int i = 0, n = nodeList.size(); i < n; i++) {
      Node prevNode = nodeList.get(i);
      Node curNode = nodeList.get((i + 1) % n);
      Node nextNode = nodeList.get((i + 2) % n);

      Vector2D prevEdge = curNode.getPosition().subtract(prevNode.getPosition());
      Vector2D nextEdge = nextNode.getPosition().subtract(curNode.getPosition());

      double crossProd = prevEdge.crossMag(nextEdge);
      Vector2D normal =
          prevEdge.normalize().subtract(nextEdge.normalize()).scaleTo(Math.signum(crossProd));

      normals.put(curNode, normal);
    }
  }
예제 #6
0
  public void updateInfluences() {
    computeArea();

    pressureShapes.clear();
    pressure.update();

    dorsal.update();
    transversal.update();
    ventral.update();
  }
예제 #7
0
  public Localization(
      List<PolygonObstacle> cSpaceObstacles,
      Rectangle2D.Double cSpaceWorld,
      Point2D.Double robotStart,
      Map<int[], Point2D.Double> fiducialPairs) {

    this.cSpaceObstacles = cSpaceObstacles;
    this.cSpaceWorld = cSpaceWorld;
    this.robotStart = robotStart;
    this.fiducialPairs = fiducialPairs;

    particles = new ArrayList<Point2D.Double>();
    probabilities = new ArrayList<Double>();

    double delta_x = 0;
    double delta_y = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (2 * Math.random() - 1);
      delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (2 * Math.random() - 1);
      particles.add(new Point2D.Double(robotStart.x + delta_x, robotStart.y + delta_y));
      probabilities.add(1.0 / NUM_PARTICLES);
    }
  }
  private void addTileOrDescendands(MeshTile tile, List<MeshTile> tileList) {
    if (!tile.getSector().intersects(this.levelSet.getSector())) return;

    if (tile.getLevelNumber() == this.visibleLevel) {
      tileList.add(tile);
      return;
    }

    if (this.levelSet.isFinalLevel(tile.getLevelNumber())) {
      return;
    }

    MeshTile[] subTiles = tile.subdivide(this.levelSet.getLevel(tile.getLevelNumber() + 1));
    for (MeshTile subTile : subTiles) {
      addTileOrDescendands(subTile, tileList);
    }
  }
예제 #9
0
  public void navigateRobot() {
    System.out.println("Path Index: " + pathIndex);
    System.out.println("goalPath Size: " + goalPath.size());
    System.out.println("locX, locY, locTheta" + locX + ", " + locY + ", " + locTheta);
    System.out.println("goalX, goalY, goalTheta: " + goalX + ", " + goalY + ", " + goalTheta);
    if (goalPath.size() > 0 && !done) {
      if (pathIndex < goalPath.size() - 1
          && Math.abs(goalX - locX) < 0.2
          && Math.abs(goalY - locY) < 0.2) {
        pathIndex++;
      }
      if (pathIndex == goalPath.size() - 1
          && Math.abs(goalX - locX) < 0.05
          && Math.abs(goalY - locY) < 0.05) {
        pathIndex++;
      }
      if (pathIndex >= goalPath.size()) {
        done = true;
        setVelocities(0.0, 0.0);
      }

      goalX = goalPath.get(pathIndex).x;
      goalY = goalPath.get(pathIndex).y;
      goalTheta = Math.atan2(goalY - locY, goalX - locX);
      if (goalTheta < 0) {
        goalTheta += 2 * Math.PI;
      }

      double error = goalTheta - locTheta;
      if (error > Math.PI) {
        error -= 2 * Math.PI;
      } else if (error < -Math.PI) {
        error += 2 * Math.PI;
      }
      if (Math.abs(error) < Math.PI / 8.0) {
        setVelocities(0.3, anglePID.update(error));
      } else {
        setVelocities(0, anglePID.update(error));
      }
      // setVelocities(0.2, anglePID.update(error));
    }
  }
예제 #10
0
  // public Point2D.Double localize(double odo_x, double odo_y, double odo_theta, List<Fiducial>
  // fiducials){
  public Point2D.Double localize(double odo_x, double odo_y, List<Fiducial> fiducials) {
    double x_disp = odo_x - prev_odo_x;
    double y_disp = odo_y - prev_odo_y;

    prev_odo_x = odo_x;
    prev_odo_y = odo_y;

    // double theta_disp = odo_theta - prev_odo_theta;
    // prev_odo_theta = odo_theta;

    List<Point2D.Double> new_particles = new ArrayList<Point2D.Double>();
    List<Double> new_probs = new ArrayList<Double>();
    List<Double> cumulative_probs = new ArrayList<Double>();

    Point2D.Double curr_point;
    double curr_prob, distance, mapx, mapy;
    double probsum = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      // Motion Update
      curr_point = new_particles.get(i);
      new_particles.add(new Point2D.Double(curr_point.x + x_disp, curr_point.y + y_disp));

      // Sensor Update
      curr_prob = 1;
      for (Fiducial fid : fiducials) {
        mapx = fiducialPairs.get(fid.colors).x;
        mapy = fiducialPairs.get(fid.colors).y;

        distance =
            Math.sqrt(
                (curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
                    + (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));

        curr_prob *=
            (Math.exp(-0.5 * (fid.distance - distance) * (fid.distance - distance) / SIGMA)
                / Math.sqrt(2 * Math.PI * SIGMA * SIGMA));
      }

      probsum += curr_prob;
      new_probs.add(curr_prob);
      cumulative_probs.add(probsum);
    }

    particles.clear();
    double rand_val;
    int index;
    int max_index = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      rand_val = probsum * Math.random();
      index = Math.abs(Collections.binarySearch(cumulative_probs, rand_val));

      particles.add(new_particles.get(index));

      if (new_probs.get(max_index) < new_probs.get(i)) {
        max_index = i;
      }
    }

    localPoint = new_particles.get(max_index);

    return localPoint;
  }
예제 #11
0
  public OdoPoint localize(
      double odo_x, double odo_y, double odo_theta, List<FiducialBlob> fiducials) {
    double x_disp = odo_x - prev_odo_x;
    double y_disp = odo_y - prev_odo_y;
    double theta_disp = odo_theta - prev_odo_theta;

    prev_odo_x = odo_x;
    prev_odo_y = odo_y;
    prev_odo_theta = odo_theta;

    localPoint =
        new OdoPoint(localPoint.x + x_disp, localPoint.y + y_disp, localPoint.theta + theta_disp);

    List<FiducialBlob> revFiducials = new ArrayList<FiducialBlob>();
    double mapx, mapy;
    Color[] colors = new Color[2];
    Fiducial mapFid;

    double mapbearing;
    double localAngle = localPoint.theta;
    localAngle = localAngle - ((int) (localAngle / (2 * Math.PI)));
    if (localAngle > Math.PI) {
      localAngle -= 2 * Math.PI;
    }

    List<OdoPoint> new_particles = new ArrayList<OdoPoint>();
    List<Double> new_probs = new ArrayList<Double>();
    List<Double> cumulative_probs = new ArrayList<Double>();

    OdoPoint curr_point;
    double curr_prob, distance, fid_distance, fid_bearing;
    double probsum = 0;

    // Filter out unreasonable fiducials
    for (FiducialBlob fid : fiducials) {
      colors[0] = fid.colors[0];
      colors[1] = fid.colors[1];

      if (findFiducial(colors[0], colors[1]) != null) {
        mapFid = findFiducial(colors[0], colors[1]);
        mapx = mapFid.getPosition().x;
        mapy = mapFid.getPosition().y;

        mapbearing = Math.atan2(mapy - localPoint.y, mapx - localPoint.x);

        distance =
            Math.sqrt(
                (localPoint.x - mapx) * (localPoint.x - mapx)
                    + (localPoint.y - mapy) * (localPoint.y - mapy));

        fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());

        System.out.println(
            "Localization: map and camera bearing = " + mapbearing + ", " + localAngle);
        System.out.println(
            "Localization: map and camera distance = " + distance + ", " + fid_distance);

        if ((Math.abs(mapbearing - localAngle) < Math.PI / 6
                || Math.abs(mapbearing - localAngle + 2 * Math.PI) < Math.PI / 6
                || Math.abs(mapbearing - localAngle - 2 * Math.PI) < Math.PI / 6)
            && fid_distance < distance * 1.3
            && distance < fid_distance * 1.3) {

          System.out.println("Localization: added fiducial");
          revFiducials.add(fid);
        } else {
          System.out.println("Localization: eliminated fiducial from bearing and distance");
        }
      } else {
        System.out.println("Localization: eliminated fiducial from colors");
      }
    }

    /*// Use Odometry if no fiducials
    if (revFiducials.size() == 0){
    	System.out.println("Localization: no fiducials to localize with");
    	particles.clear();
    	OdoPoint curr_point;
    	for (int i = 0; i < NUM_PARTICLES; i++){
    		curr_point = particles.get(i);
    		particles.add(new OdoPoint(curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));
    	}
    	return localPoint;
    }*/

    for (int i = 0; i < NUM_PARTICLES; i++) {
      // Motion Update
      curr_point = particles.get(i);
      new_particles.add(
          new OdoPoint(
              curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));

      // Sensor Update
      curr_prob = 1;
      for (FiducialBlob fid : revFiducials) {
        colors[0] = fid.colors[0];
        colors[1] = fid.colors[1];

        mapFid = findFiducial(colors[0], colors[1]);
        mapx = mapFid.getPosition().x;
        mapy = mapFid.getPosition().y;

        distance =
            Math.sqrt(
                (curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
                    + (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));

        fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());

        curr_prob *=
            NORMALIZATION
                * (Math.exp(
                        -0.5 * (fid_distance - distance) * (fid_distance - distance) / DIST_SIGMA)
                    / Math.sqrt(2 * Math.PI * DIST_SIGMA * DIST_SIGMA));

        fid_bearing = curr_point.theta + theta_disp - ImageBlob.computeBearing(fid.x);
        fid_bearing = fid_bearing - ((int) (fid_bearing / (2 * Math.PI)));
        if (fid_bearing > Math.PI) {
          fid_bearing -= 2 * Math.PI;
        }
        mapbearing = Math.atan2(mapy - curr_point.y - y_disp, mapx - curr_point.x - x_disp);

        curr_prob *=
            NORMALIZATION
                * (Math.exp(
                        -0.5 * (mapbearing - fid_bearing) * (mapbearing - fid_bearing) / ROT_SIGMA)
                    / Math.sqrt(2 * Math.PI * ROT_SIGMA * ROT_SIGMA));
      }

      probsum += curr_prob;
      new_probs.add(curr_prob);
      cumulative_probs.add(probsum);
    }

    if (revFiducials.size() > 0) {
      System.out.println("Localization: localizing with fiducials");
      particles.clear();
      double rand_val;
      int index;
      int max_index = 0;
      for (int i = 0; i < NUM_PARTICLES; i++) {
        rand_val = probsum * Math.random();
        index = Collections.binarySearch(cumulative_probs, rand_val);
        if (index < 0) {
          index = -(index + 1);
        }

        particles.add(new_particles.get(index));

        if (new_probs.get(max_index) < new_probs.get(i)) {
          max_index = i;
        }
      }

      localPoint = new_particles.get(max_index);
    } else {
      System.out.println("Localization: no fiducials to localize with");
      particles = new_particles;
    }

    return localPoint;
  }