コード例 #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);
  }
 @Override
 public void doPostBehaviorCleanup() {
   debrisDataList.clear();
   sortedDebrisDataList.clear();
   isDone.set(false);
   haveInputsBeenSet.set(false);
 }
 public void clear() {
   currentTask = null;
   taskQueue.clear();
   isDone.set(true);
   hasAborted.set(false);
   currentTaskIndex.set(0);
 }
 @Override
 public void initialize() {
   debrisDataList.clear();
   sortedDebrisDataList.clear();
   isDone.set(false);
   haveInputsBeenSet.set(false);
 }
コード例 #5
0
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 300000)
  public void testGetBooleanValue() {
    BooleanYoVariable booleanVariable = new BooleanYoVariable("booleanVar", registry);
    booleanVariable.set(true);

    assert booleanVariable.getBooleanValue();
    booleanVariable.set(false);
    assert !booleanVariable.getBooleanValue();
  }
  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();
      }
    }
  }
 public void setInputs(ArrayList<DebrisData> debrisDataList) {
   sortDebrisFromCloserToFarther(debrisDataList);
   submitArmsSafePosition();
   for (int i = 0; i < sortedDebrisDataList.size(); i++)
     submitRemoveDebrisTasks(sortedDebrisDataList.get(i));
   haveInputsBeenSet.set(true);
 }
  public UserDesiredHandstepProvider(
      FullHumanoidRobotModel fullRobotModel,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    this.handstepHelper = new HandstepHelper(fullRobotModel);

    userHandstepTakeIt.set(false);
    userHandstepNormal.set(-1.0, 0.0, 0.0);
    userHandstepRobotSide.set(RobotSide.LEFT);

    userDesiredHandstepCoordinateSystem =
        new YoGraphicCoordinateSystem("userHandstepViz", "", parentRegistry, 0.3);

    VariableChangedListener listener =
        new VariableChangedListener() {
          public void variableChanged(YoVariable<?> v) {
            Vector3d position = userHandstepPosition.getVector3dCopy();
            Vector3d surfaceNormal = userHandstepNormal.getVector3dCopy();
            double rotationAngleAboutNormal = userHandstepRotationAboutNormal.getDoubleValue();
            userDesiredHandstepCoordinateSystem.setTransformToWorld(
                HandstepHelper.computeHandstepTransform(
                    false, position, surfaceNormal, rotationAngleAboutNormal));
          }
        };

    yoGraphicsListRegistry.registerYoGraphic(
        "UserDesiredHandstep", userDesiredHandstepCoordinateSystem);

    userHandstepNormal.attachVariableChangedListener(listener);
    userHandstepRotationAboutNormal.addVariableChangedListener(listener);
    userHandstepPosition.attachVariableChangedListener(listener);

    parentRegistry.addChild(registry);
  }
  protected void setupTest(DRCRobotModel robotModel)
      throws SimulationExceededMaximumTimeException, InterruptedException {
    boolean runMultiThreaded = false;
    setupTrack(runMultiThreaded, robotModel);
    FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
    pushRobotController =
        new PushRobotController(
            drcFlatGroundWalkingTrack.getDrcSimulation().getRobot(), fullRobotModel);

    if (VISUALIZE_FORCE) {
      drcFlatGroundWalkingTrack
          .getSimulationConstructionSet()
          .addYoGraphic(pushRobotController.getForceVisualizer());
    }

    SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
    BooleanYoVariable enable =
        (BooleanYoVariable) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery");

    // enable push recovery
    enable.set(true);

    for (RobotSide robotSide : RobotSide.values) {
      String prefix = fullRobotModel.getFoot(robotSide).getName();
      scs.getVariable(prefix + "FootControlModule", prefix + "State");
      @SuppressWarnings("unchecked")
      final EnumYoVariable<WalkingStateEnum> walkingState =
          (EnumYoVariable<WalkingStateEnum>)
              scs.getVariable("WalkingHighLevelHumanoidController", "walkingState");
      doubleSupportStartConditions.put(
          robotSide, new DoubleSupportStartCondition(walkingState, robotSide));
    }
  }
 @Override
 public boolean getForceMagnitudePastThreshhold() {
   // a more liberal version of hasFootHitGround
   double thisFootZ = getPointInWorld(foot.getSoleFrame()).getZ();
   double lowestFootZ = getLowestFootZInWorld();
   fixedOnGround.set((thisFootZ - lowestFootZ) < switchZThreshold.getDoubleValue() * 2);
   return fixedOnGround.getBooleanValue();
 }
  /** is the foot in question within the switchZThreshold of the lowest foot */
  @Override
  public boolean hasFootHitGround() {
    double thisFootZ = getPointInWorld(foot.getSoleFrame()).getZ();
    double lowestFootZ = getLowestFootZInWorld();

    hitGround.set((thisFootZ - lowestFootZ) < switchZThreshold.getDoubleValue());
    soleZ.set(thisFootZ);
    ankleZ.set(getPointInWorld(foot.getFrameAfterParentJoint()).getZ());
    return hitGround.getBooleanValue();
  }
  @Override
  public void compute(double time) {
    double trajectoryTime = stepTime.getDoubleValue();
    isDone.set(time >= trajectoryTime);

    time = MathTools.clipToMinMax(time, 0.0, trajectoryTime);
    timeIntoStep.set(time);

    double percent = time / trajectoryTime;
    trajectory.compute(percent);
  }
  @Ignore("Needs to be improved")
  @ContinuousIntegrationTest(estimatedDuration = 67.1)
  @Test(timeout = 340000)
  public void testMultiStepForwardAndContinueWalking()
      throws SimulationExceededMaximumTimeException, InterruptedException,
          ControllerFailureException {
    BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
    setupTest(getRobotModel());
    SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
    setForwardPushParameters();
    Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
    blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
    BooleanYoVariable walk =
        (BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");

    // disable walking
    walk.set(false);
    blockingSimulationRunner.simulateAndBlock(4.0);

    // push the robot
    pushRobotController.applyForceDelayed(
        pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);

    // simulate for a little while longer
    blockingSimulationRunner.simulateAndBlock(forceDuration + 6.0);

    // re-enable walking
    walk.set(true);
    blockingSimulationRunner.simulateAndBlock(6.0);

    Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
    Vector3d plusMinusVector =
        new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
    BoundingBox3d boundingBox =
        BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
    DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
        boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());

    createVideo(scs);
    BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
  }
  private double adjustCurrentDesiredRelativeAngle(double currentDesiredRelativeAngle) {
    if (controlledFrame == null) {
      isCurrentAngleBeingAdjusted.set(false);
      return currentDesiredRelativeAngle;
    }

    currentControlledFramePosition.setToZero(controlledFrame);
    currentControlledFramePosition.changeFrame(circleFrame);

    double x = currentControlledFramePosition.getX();
    double y = currentControlledFramePosition.getY();

    currentControlledFrameRelativeAngle.set(
        computeAngleDifferenceMinusPiToPi(Math.atan2(y, x), initialAngle.getDoubleValue()));

    currentAngleTrackingError.set(
        computeAngleDifferenceMinusPiToPi(
            currentDesiredRelativeAngle, currentControlledFrameRelativeAngle.getDoubleValue()));

    if (computeAngleDifferenceMinusPiToPi(
            currentAngleTrackingError.getDoubleValue(),
            maximumAngleTrackingErrorTolerated.getDoubleValue())
        > 0.0) {
      isCurrentAngleBeingAdjusted.set(true);
      return trimAngleMinusPiToPi(
          currentControlledFrameRelativeAngle.getDoubleValue()
              + maximumAngleTrackingErrorTolerated.getDoubleValue());
    } else if (trimAngleMinusPiToPi(
            currentAngleTrackingError.getDoubleValue()
                + maximumAngleTrackingErrorTolerated.getDoubleValue())
        < 0.0) {
      isCurrentAngleBeingAdjusted.set(true);
      return computeAngleDifferenceMinusPiToPi(
          currentControlledFrameRelativeAngle.getDoubleValue(),
          maximumAngleTrackingErrorTolerated.getDoubleValue());
    } else {
      isCurrentAngleBeingAdjusted.set(false);
      return currentDesiredRelativeAngle;
    }
  }
  @Override
  public void initialize() {
    timeIntoStep.set(0.0);
    isDone.set(false);

    initialPosition.changeFrame(worldFrame);
    finalPosition.changeFrame(worldFrame);
    double maxStepZ = Math.max(initialPosition.getZ(), finalPosition.getZ());

    switch (trajectoryType.getEnumValue()) {
      case OBSTACLE_CLEARANCE:
        for (int i = 0; i < numberWaypoints; i++) {
          waypointPositions
              .get(i)
              .interpolate(initialPosition, finalPosition, waypointProportions[i]);
          waypointPositions.get(i).setZ(maxStepZ + swingHeight.getDoubleValue());
        }
        break;
      case PUSH_RECOVERY:
      case BASIC:
      case DEFAULT:
        for (int i = 0; i < numberWaypoints; i++) {
          waypointPositions
              .get(i)
              .interpolate(initialPosition, finalPosition, waypointProportions[i]);
          waypointPositions.get(i).add(0.0, 0.0, swingHeight.getDoubleValue());
        }
        break;
      default:
        throw new RuntimeException("Trajectory type not implemented");
    }

    stanceFootPosition.changeFrame(worldFrame);
    double maxWaypointZ =
        Math.max(
            stanceFootPosition.getZ() + maxSwingHeight.getDoubleValue(),
            maxStepZ + defaultSwingHeight);
    for (int i = 0; i < numberWaypoints; i++) {
      waypointPositions.get(i).setZ(Math.min(waypointPositions.get(i).getZ(), maxWaypointZ));
    }

    trajectory.setEndpointConditions(
        initialPosition, initialVelocity, finalPosition, finalVelocity);
    trajectory.setWaypoints(waypointPositions);
    trajectory.initialize();

    visualize();
  }
  public Handstep getDesiredHandstep(RobotSide robotSide) {
    if (!userHandstepTakeIt.getBooleanValue()) return null;
    if (userHandstepRobotSide.getEnumValue() != robotSide) return null;

    Vector3d surfaceNormal = userHandstepNormal.getVector3dCopy();
    double rotationAngleAboutNormal = userHandstepRotationAboutNormal.getDoubleValue();
    Vector3d position = userHandstepPosition.getVector3dCopy();

    Handstep handstep =
        handstepHelper.getDesiredHandstep(
            robotSide,
            position,
            surfaceNormal,
            rotationAngleAboutNormal,
            swingTrajectoryTime.getDoubleValue());
    userHandstepTakeIt.set(false);
    return handstep;
  }
コード例 #17
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 reset() {
   updateHasBeenCalled = false;
   useDecay.set(true);
 }
  public void hideVisualization() {
    if (!visualize) return;

    showViz.set(false);
  }
  public void showVisualization() {
    if (!visualize) return;

    showViz.set(true);
  }
コード例 #21
0
 public void setIsSlipping(boolean isSlipping) {
   slip.set(isSlipping);
 }
 private boolean doSlipThisStance() {
   slipNextStep.set(random.nextDouble() < probabilitySlip);
   return slipNextStep.getBooleanValue();
 }
コード例 #23
0
  public static void setupJoyStickAndTreadmill(YoVariableRegistry registry) {
    Joystick joystickUpdater;
    try {
      joystickUpdater = new Joystick();
    } catch (IOException e) {
      e.printStackTrace();
      return;
    }

    double deadZone = 0.02;
    final double desiredVelocityX_Bias = 0.0;
    double desiredVelocityY_Bias = 0.0;
    final double desiredHeadingDot_Bias = 0.0;

    final double minHeadingDot = -0.1, minVelocityX = -0.2, maxHeadingDot = 0.1, maxVelocityX = 0.2;
    boolean signFlip = true;

    // Start the heading and speed updater thread
    // --(!) Requires data from MultisenseHeadingSubscriber
    final ValkyrieHeadingUpdater valkyrieHeadingUpdater =
        new ValkyrieHeadingUpdater(
            registry, minHeadingDot, maxHeadingDot, minVelocityX, maxVelocityX, signFlip);
    final Thread headingUpdaterThread = new Thread(valkyrieHeadingUpdater);
    headingUpdaterThread.start();
    // --------------------
    // Speed and Heading Controller
    // --------------------
    final BooleanYoVariable isMultisenseControllingSpeedAndHeading =
        new BooleanYoVariable("isMultisenseControllingSpeedAndHeading", registry);
    final DoubleYoVariable headingDotConstant =
        new DoubleYoVariable("headingDotConstant", registry);
    final DoubleYoVariable velocityXConstant = new DoubleYoVariable("velocityXConstant", registry);

    final DoubleYoVariable desiredHeadingDot =
        (DoubleYoVariable)
            registry.getVariable("RateBasedDesiredHeadingControlModule", "desiredHeadingDot");
    final DoubleYoVariable desiredVelocityX =
        (DoubleYoVariable)
            registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityX");
    if (desiredVelocityX == null || joystickUpdater == null) return;

    isMultisenseControllingSpeedAndHeading.set(false);
    isMultisenseControllingSpeedAndHeading.addVariableChangedListener(
        new VariableChangedListener() {
          @Override
          public void variableChanged(YoVariable<?> v) {
            // Reset heading and speed when toggling controller
            if (v.getValueAsDouble() != 0) {
              desiredVelocityX.set(desiredVelocityX_Bias);
              desiredHeadingDot.set(desiredHeadingDot_Bias);
            }
          }
        });

    headingDotConstant.set(0.5);
    velocityXConstant.set(0.1);
    desiredHeadingDot.set(desiredHeadingDot_Bias);

    desiredHeadingDot.addVariableChangedListener(
        new VariableChangedListener() {
          @Override
          public void variableChanged(YoVariable<?> v) {
            // Overwrite joystick's input if it is disabled for heading control.
            if (isMultisenseControllingSpeedAndHeading.getBooleanValue())
              desiredHeadingDot.set(valkyrieHeadingUpdater.currentHeadingDot);
          }
        });

    desiredVelocityX.set(desiredVelocityX_Bias);
    joystickUpdater.addJoystickEventListener(
        new DoubleYoVariableJoystickEventListener(
            desiredVelocityX,
            joystickUpdater.findComponent(Component.Identifier.Axis.Y),
            minVelocityX + desiredVelocityX_Bias,
            maxVelocityX + desiredVelocityX_Bias,
            deadZone,
            true));
    desiredVelocityX.addVariableChangedListener(
        new VariableChangedListener() {
          @Override
          public void variableChanged(YoVariable<?> v) {
            // Overwrite joystick's input if it is disabled for speed control.
            if (isMultisenseControllingSpeedAndHeading.getBooleanValue())
              v.setValueFromDouble(valkyrieHeadingUpdater.currentVelocityX);
            if (v.getValueAsDouble() < minVelocityX) v.setValueFromDouble(minVelocityX, false);
          }
        });

    DoubleYoVariable desiredVelocityY =
        (DoubleYoVariable)
            registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityY");
    desiredVelocityY.set(desiredVelocityY_Bias);
    joystickUpdater.addJoystickEventListener(
        new DoubleYoVariableJoystickEventListener(
            desiredVelocityY,
            joystickUpdater.findComponent(Component.Identifier.Axis.X),
            -0.1 + desiredVelocityY_Bias,
            0.1 + desiredVelocityY_Bias,
            deadZone,
            true));

    joystickUpdater.addJoystickEventListener(
        new DoubleYoVariableJoystickEventListener(
            desiredHeadingDot,
            joystickUpdater.findComponent(Component.Identifier.Axis.RZ),
            minHeadingDot + desiredHeadingDot_Bias,
            maxHeadingDot + desiredHeadingDot_Bias,
            deadZone,
            signFlip));

    BooleanYoVariable walk =
        (BooleanYoVariable)
            registry.getVariable("DesiredFootstepCalculatorFootstepProviderWrapper", "walk");
    joystickUpdater.addJoystickEventListener(
        new BooleanYoVariableJoystickEventListener(
            walk, joystickUpdater.findComponent(Component.Identifier.Button.TRIGGER), true));
  }
コード例 #24
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);
  }
 @Override
 public boolean isDone() {
   if (pipeLine.isDone() && hasInputBeenSet()) isDone.set(true);
   return isDone.getBooleanValue();
 }