예제 #1
0
  @Override
  public double weight(Observable observable) {

    // Shape/stack.
    if (!(observable instanceof Shape || observable instanceof Stack)) {
      return 0;
    }
    Position p1 = observable.referencePoint();

    // Landmarks.
    for (ObservableHypothesis hypothesis : landmarkDistribution) {
      Observable landmark = hypothesis.observable();
      double weight = hypothesis.weight();

      // Gripper / active position.
      if (landmark instanceof Gripper || landmark instanceof ActivePosition) {
        Position p2 = landmark.referencePoint();
        if (p1.x() == p2.x() && p1.y() == p2.y()) {
          return weight;
        }
        continue;
      }

      // Not supported.
      throw new RoboticException("%s below %s is not supported.", observable, landmark);
    }
    return 0;
  }
예제 #2
0
  @Override
  public DestinationDistribution destinations(PlannerContext context) {
    DestinationDistribution destinations = new DestinationDistribution(layout);
    for (ObservableHypothesis hypothesis : landmarkDistribution) {
      Observable landmark = hypothesis.observable();
      double weight = hypothesis.weight();

      // Gripper / active position.
      if (landmark instanceof Gripper || landmark instanceof ActivePosition) {
        destinations.add(
            new DestinationHypothesis(
                (context.simulator().dropPosition(landmark.referencePoint())), null, weight));
      }

      // Not supported.
      else {
        throw new RoboticException(
            "Below destination is not supported with landmark %s.", landmark);
      }
    }
    destinations.normalize();
    return destinations;
  }