예제 #1
0
  private void checkExtrema(StoredJointState newState) {

    if (newState.compareTo(getMinMovementStep().move) < 0) {
      minId = savedMovementFlow.size() - 1;
      minIds.add(minId);
    }
    if (newState.compareTo(getMaxMovementStep().move) > 0) {
      maxId = savedMovementFlow.size() - 1;
      maxIds.add(maxId);
    }
  }
예제 #2
0
  public void update(Joint updatedBy) {

    for (MovementStep s : savedMovementFlow) {
      s.getMove().updateWorldOrientation();
    }

    // check if updateBy is parent of currently observed joint
    // if yes its not neccessary to update
    if (observedJoint.hasParent(updatedBy)) {
      return;
    }

    StoredJointState newState = new StoredJointState(observedJoint, observedJoint.parent, true);

    if (lastChanged != null) {
      StoredJointState lastState = lastChanged.getMove();

      if (!newState.hasAngelDifferenceGreaterThan(lastState, MIN_ANGLE_DIFFERENCE)) {
        // LOGGER.debug("Difference to low");
        return;
      }
    }

    // check if an almost same position is already saved and its not
    // the last one (not increasing counter if we move slightly on
    // position)
    // if yes increase counter
    for (int i = 0; i < savedMovementFlow.size() - 1; i++) {
      MovementStep m = savedMovementFlow.get(i);
      if (m != lastChanged && m.getMove().equals(newState)) {
        m.incCount();
        maxCount = Math.max(maxCount, m.getCount());
        // LOGGER.debug("Find existing position - Increase count");
        return;
      }
    }

    // LOGGER.debug("Created new one");
    lastChanged = new MovementStep(newState);
    savedMovementFlow.addLast(lastChanged);

    // get right parent
    // save extrema
    checkExtrema(newState);
  }