コード例 #1
0
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 300000)
  public void testValueEquals() {
    BooleanYoVariable booleanVariable = new BooleanYoVariable("booleanVar", registry);
    DoubleYoVariable doubleVariable = new DoubleYoVariable("doubleVariable", registry);
    IntegerYoVariable intVariable = new IntegerYoVariable("intVariable", registry);
    EnumYoVariable<FooEnum> enumVariable =
        EnumYoVariable.create("enumVariable", FooEnum.class, registry);

    booleanVariable.set(true);
    doubleVariable.set(1.4);
    intVariable.set(7);
    enumVariable.set(FooEnum.THREE);

    assert booleanVariable.valueEquals(true);
    assert doubleVariable.valueEquals(1.4);
    assert intVariable.valueEquals(7);
    assert enumVariable.valueEquals(FooEnum.THREE);

    booleanVariable.set(false);
    doubleVariable.set(4.5);
    intVariable.set(9);
    enumVariable.set(FooEnum.TWO);

    assert booleanVariable.valueEquals(false);
    assert doubleVariable.valueEquals(4.5);
    assert intVariable.valueEquals(9);
    assert enumVariable.valueEquals(FooEnum.TWO);

    assert !booleanVariable.valueEquals(true);
  }
コード例 #2
0
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 300000)
  public void testGetIntegerValue() {
    IntegerYoVariable integerVariable = new IntegerYoVariable("integerVariable", registry);
    integerVariable.set(5);

    assertEquals(integerVariable.getIntegerValue(), 5);
  }
  public void initializeDesiredFootstep(RobotSide supportLegSide) {
    RobotSide swingLegSide = supportLegSide.getOppositeSide();
    ReferenceFrame supportAnkleZUpFrame = ankleZUpFrames.get(supportLegSide);
    ReferenceFrame supportAnkleFrame = ankleFrames.get(supportLegSide);

    computeDistanceAndAngleToDestination(
        supportAnkleZUpFrame, swingLegSide, desiredDestination.getFramePoint2dCopy());

    if (distanceToDestination.getDoubleValue() < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
      numberBlindStepsInPlace.increment();
    }

    Matrix3d footToWorldRotation =
        computeDesiredFootRotation(
            angleToDestination.getDoubleValue(), swingLegSide, supportAnkleFrame);
    FramePoint footstepPosition =
        getDesiredFootstepPositionCopy(
            supportAnkleZUpFrame,
            supportAnkleFrame,
            swingLegSide,
            desiredDestination.getFramePoint2dCopy(),
            footToWorldRotation);

    setYoVariables(swingLegSide, footToWorldRotation, footstepPosition);
  }
 public void clear() {
   currentTask = null;
   taskQueue.clear();
   isDone.set(true);
   hasAborted.set(false);
   currentTaskIndex.set(0);
 }
コード例 #5
0
  public IntGlobalParameter(
      String name, String description, int value, GlobalParameterChangedListener listener) {
    super(listener);

    yoVariable = new IntegerYoVariable(name, description, registry);
    ((IntegerYoVariable) yoVariable).set(value);

    if (changedListener != null) changedListener.globalParameterCreated(this);
  }
コード例 #6
0
  private void setIntegerValue(int newValue, String comment) {
    int previousValue = ((IntegerYoVariable) yoVariable).getIntegerValue();
    ((IntegerYoVariable) yoVariable).set(newValue);

    if (changedListener != null) {
      changedListener.integerValueChanged(this, comment, previousValue, newValue);
    }

    updateChildren(comment);
  }
コード例 #7
0
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 300000)
  public void testToString() {
    EnumYoVariable<FooEnum> enumyoVariable =
        EnumYoVariable.create("enumYoVariable", FooEnum.class, registry);
    enumyoVariable.set(FooEnum.THREE);
    assertEquals("enumYoVariable: THREE", enumyoVariable.toString());

    IntegerYoVariable intyoVariable = new IntegerYoVariable("intYoVariable", registry);
    intyoVariable.set(1);
    assertEquals("intYoVariable: 1", intyoVariable.toString());

    DoubleYoVariable yoVariable = new DoubleYoVariable("doubleYoVariable", registry);
    yoVariable.set(0.0112);
    assertEquals("doubleYoVariable: 0.0112", yoVariable.toString());

    BooleanYoVariable booleanyoVariable = new BooleanYoVariable("booleanYoVariable", registry);
    booleanyoVariable.set(false);
    assertEquals("booleanYoVariable: false", booleanyoVariable.toString());
  }
  public void doControl() {
    if (!isDone.getBooleanValue()) hasAborted.set(currentTask.abortRequested());

    if (!hasAborted.getBooleanValue()) {
      handleTransitions();
      numberOfTasksRemaining.set(taskQueue.size());
      isDone.set(isDone());

      if (!isDone.getBooleanValue()) {
        timeInCurrentTask.set(yoTime.getDoubleValue() - switchTime.getDoubleValue());
        currentTask.doAction();
      }
    }
  }
  private void handleTransitions() {
    if (currentTask != null) {
      if (!currentTask.isDone()) return;

      currentTask.doTransitionOutOfAction();

      if (logger != null) {
        logger.info("Diagnostic task completed.\n");
      }
    }

    if (!taskQueue.isEmpty()) {
      currentTask = taskQueue.poll();
      currentTaskIndex.increment();
      switchTime.set(yoTime.getDoubleValue());
      currentTask.doTransitionIntoAction();
    } else {
      currentTask = null;
    }
  }
コード例 #10
0
 public int getCollisionCount() {
   return collisionCount.getIntegerValue();
 }
コード例 #11
0
  public void performUpdate() {
    boolean positionChanged = rawTicks.getIntegerValue() != previousRawTicks.getIntegerValue();

    if (positionChanged) doStateTransitions();

    doStateActionsForPosition(positionChanged);

    if (positionChanged) {
      double positionChange =
          processedTicks.getDoubleValue() - previousProcessedTicks.getIntegerValue();
      int positionChangeInt = (int) positionChange;
      double timeChange = time.getDoubleValue() - previousTime.getDoubleValue();

      //       int positionChangeInt = processedTicks.getIntegerValue() -
      // previousProcessedTicksTwoBack.getIntegerValue();
      //       double positionChange = (double) positionChangeInt;
      //       double timeChange = time.getDoubleValue() - previousTimeTwoBack.getDoubleValue();

      if (positionChangeInt >= 2) {
        timeChange =
            dt; // If there were multiple time ticks and multiple position ticks, then can assume
        // only one time tick!

        minPriorRate.set((positionChange - 1.0) / timeChange);
        maxPriorRate.set((positionChange + 1.0) / timeChange);
        averagePriorRate.set(positionChange / timeChange);
      } else if (positionChangeInt <= -2) {
        timeChange =
            dt; // If there were multiple time ticks and multiple position ticks, then can assume
        // only one time tick!

        minPriorRate.set((positionChange + 1.0) / timeChange);
        maxPriorRate.set((positionChange - 1.0) / timeChange);
        averagePriorRate.set(positionChange / timeChange);
      } else if (timeChange > 1.5 * dt) {
        minPriorRate.set(positionChange / (timeChange + dt));
        maxPriorRate.set(positionChange / (timeChange - dt));
        averagePriorRate.set(positionChange / timeChange);
      } else if (positionChangeInt == 1) {
        minPriorRate.set(positionChange / (timeChange + dt));
        maxPriorRate.set((positionChange + 1.0) / timeChange);
        averagePriorRate.set(positionChange / timeChange);
      } else if (positionChangeInt == -1) {
        minPriorRate.set(positionChange / (timeChange + dt));
        maxPriorRate.set((positionChange - 1.0) / timeChange);
        averagePriorRate.set(positionChange / timeChange);
      } else if (positionChangeInt == 0) {
        maxPriorRate.set((positionChange + 1.0) / timeChange);
        minPriorRate.set((positionChange - 1.0) / timeChange);
        averagePriorRate.set(positionChange / timeChange);
      } else {
        System.err.println("Should never get here!");
        System.err.println("positionChangeInt = " + positionChangeInt);
        System.err.println("timeChange = " + timeChange);

        //          throw new RuntimeException("Should never get here!");
      }
    }

    doStateActionsForVelocity(positionChanged);

    if (positionChanged) {
      this.previousProcessedTicksTwoBack.set(this.previousProcessedTicks.getIntegerValue());
      this.previousProcessedTicks.set((int) processedTicks.getDoubleValue());
      this.previousRawTicksTwoBack.set(previousRawTicks.getIntegerValue());
      this.previousRawTicks.set(rawTicks.getIntegerValue());

      this.previousTimeTwoBack.set(this.previousTime.getDoubleValue());
      this.previousTime.set(this.time.getDoubleValue());
    }
  }
コード例 #12
0
  @DeployableTestMethod
  @Test(timeout = 300000)
  public void testDataFileWriterAndReader() throws IOException, RepeatDataBufferEntryException {
    int numDataPoints = 10000;

    DataBuffer dataBuffer = new DataBuffer(numDataPoints);

    YoVariableRegistry rootRegistry = new YoVariableRegistry("rootRegistry");
    YoVariableRegistry registryOne = new YoVariableRegistry("registryOne");
    YoVariableRegistry registryTwo = new YoVariableRegistry("registryTwo");
    YoVariableRegistry registryThree = new YoVariableRegistry("registryThree");

    rootRegistry.addChild(registryOne);
    rootRegistry.addChild(registryTwo);
    registryTwo.addChild(registryThree);

    DoubleYoVariable variableOne = new DoubleYoVariable("variableOne", rootRegistry);
    DoubleYoVariable variableTwo = new DoubleYoVariable("variableTwo", rootRegistry);
    DoubleYoVariable variableThree = new DoubleYoVariable("variableThree", rootRegistry);
    DoubleYoVariable variableFour = new DoubleYoVariable("variableFour", registryOne);
    DoubleYoVariable variableFive = new DoubleYoVariable("variableFive", registryTwo);
    BooleanYoVariable variableSix = new BooleanYoVariable("variableSix", rootRegistry);
    IntegerYoVariable variableSeven = new IntegerYoVariable("variableSeven", registryThree);

    dataBuffer.addVariable(variableOne);
    dataBuffer.addVariable(variableTwo);
    dataBuffer.addVariable(variableThree);
    dataBuffer.addVariable(variableFour);
    dataBuffer.addVariable(variableFive);
    dataBuffer.addVariable(variableSix);
    dataBuffer.addVariable(variableSeven);

    for (int i = 0; i < numDataPoints; i++) {
      variableOne.set(Math.random());
      variableTwo.set(Math.random());
      variableThree.set((int) (Math.random() * 100.0));
      variableFour.set((int) (Math.random() * 100.0));
      variableFive.set(Math.random());
      variableSix.set(Math.random() > 0.5);
      variableSeven.set((int) (Math.random() * 1000.0));

      dataBuffer.tickAndUpdate();
    }

    Robot robot = new Robot("testRobot");

    ArrayList<YoVariable<?>> allVariables = rootRegistry.getAllVariablesIncludingDescendants();

    boolean binary = false;
    boolean compress = false;
    boolean spreadsheetFormatted = true;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    spreadsheetFormatted = false;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    binary = true;
    compress = false;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    binary = false;
    compress = true;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    binary = true;
    compress = true;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
  }
 public void clearAllExceptCurrent() {
   taskQueue.clear();
   numberOfTasksRemaining.set(0);
 }
 public void setDesiredDestination(FramePoint2d desiredDestinationInWorld) {
   numberBlindStepsInPlace.set(0);
   this.desiredDestination.set(desiredDestinationInWorld);
 }
 public boolean isDone() {
   return (numberBlindStepsInPlace.getIntegerValue() >= 2);
 }