예제 #1
0
  /*
   * Initialise Method
   * @params start and goal coordinates
   */
  public void init(int sX, int sY, int gX, int gY) {
    cellHash.clear();
    path.clear();
    openHash.clear();
    while (!openList.isEmpty()) openList.poll();

    k_m = 0;

    s_start.x = sX;
    s_start.y = sY;
    s_goal.x = gX;
    s_goal.y = gY;

    CellInfo tmp = new CellInfo();
    tmp.g = 0;
    tmp.rhs = 0;
    tmp.cost = C1;

    cellHash.put(s_goal, tmp);

    tmp = new CellInfo();
    tmp.g = tmp.rhs = heuristic(s_start, s_goal);
    tmp.cost = C1;
    cellHash.put(s_start, tmp);
    s_start = calculateKey(s_start);

    s_last = s_start;
  }
예제 #2
0
  /*
   * Update the position of the agent/robot.
   * This does not force a replan.
   */
  public void updateStart(int x, int y) {
    s_start.x = x;
    s_start.y = y;

    k_m += heuristic(s_last, s_start);

    s_start = calculateKey(s_start);
    s_last = s_start;
  }
예제 #3
0
  /*
   * updateCell as per [S. Koenig, 2002]
   */
  public void updateCell(int x, int y, double val) {
    State u = new State();
    u.x = x;
    u.y = y;

    if ((u.eq(s_start)) || (u.eq(s_goal))) return;

    makeNewCell(u);
    cellHash.get(u).cost = val;
    updateVertex(u);
  }
예제 #4
0
  /*
   * This is somewhat of a hack, to change the position of the goal we
   * first save all of the non-empty nodes on the map, clear the map, move the
   * goal and add re-add all of the non-empty cells. Since most of these cells
   * are not between the start and goal this does not seem to hurt performance
   * too much. Also, it frees up a good deal of memory we are probably not
   * going to use.
   */
  public void updateGoal(int x, int y) {
    List<Pair<ipoint2, Double>> toAdd = new ArrayList<Pair<ipoint2, Double>>();
    Pair<ipoint2, Double> tempPoint;

    for (Map.Entry<State, CellInfo> entry : cellHash.entrySet()) {
      if (!close(entry.getValue().cost, C1)) {
        tempPoint =
            new Pair(new ipoint2(entry.getKey().x, entry.getKey().y), entry.getValue().cost);
        toAdd.add(tempPoint);
      }
    }

    cellHash.clear();
    openHash.clear();

    while (!openList.isEmpty()) openList.poll();

    k_m = 0;

    s_goal.x = x;
    s_goal.y = y;

    CellInfo tmp = new CellInfo();
    tmp.g = tmp.rhs = 0;
    tmp.cost = C1;

    cellHash.put(s_goal, tmp);

    tmp = new CellInfo();
    tmp.g = tmp.rhs = heuristic(s_start, s_goal);
    tmp.cost = C1;
    cellHash.put(s_start, tmp);
    s_start = calculateKey(s_start);

    s_last = s_start;

    Iterator<Pair<ipoint2, Double>> iterator = toAdd.iterator();
    while (iterator.hasNext()) {
      tempPoint = iterator.next();
      updateCell(tempPoint.first().x, tempPoint.first().y, tempPoint.second());
    }
  }
예제 #5
0
  /**
   * 次の状態へ遷移する。 台車、棒の位置はシミュレーション計算で求める。
   *
   * @param action 力を加える方向。
   */
  public void nextState(int action) {
    // 台車の加速度
    double xAcc;
    // 棒の角加速度
    double thetaAcc;
    // 台車に加える力
    double force;
    // cosθ
    double cosTheta;
    // sinθ
    double sinTheta;
    double temp;

    // actionに応じて力を加える方向を変える
    force = 0;
    switch (action) {
      case 0:
        force = -FORCE_MAG;
        break;
      case 1:
        force = FORCE_MAG;
        break;
    }

    // 台車と棒の運動方程式にしたがって次の状態を決める
    // 台車の詳しい運動方程式はWebを参照してください
    cosTheta = Math.cos(state.theta);
    sinTheta = Math.sin(state.theta);
    temp = (force + POLE_MASS_LENGTH * state.thetaDot * state.thetaDot * sinTheta) / TOTAL_MASS;
    thetaAcc =
        (GRAVITY * sinTheta - cosTheta * temp)
            / (LENGTH * (FOUR_THIRDS - MASS_POLE * cosTheta * cosTheta / TOTAL_MASS));
    xAcc = temp - POLE_MASS_LENGTH * thetaAcc * cosTheta / TOTAL_MASS;

    // 状態を更新する
    state.x += TAU * state.xDot;
    state.xDot += TAU * xAcc;
    state.theta += TAU * state.thetaDot;
    state.thetaDot += TAU * thetaAcc;
  }