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 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); }
@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(); }
@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 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); }
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 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; }
@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()); }
@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()); }
@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); }
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 void update(double currentPosition) { if (!updateHasBeenCalled) { updateHasBeenCalled = true; lastPosChangeDirection.set(Direction.NONE); } timeSinceLastPosChange.set( time.getDoubleValue() - finiteDifferenceVelocity.getPreviousTimestamp()); double previousPositon = finiteDifferenceVelocity.getPreviousPosition(); velocityIfEncoderTicksNow.set( (positionPredicted.getDoubleValue() - previousPositon) / timeSinceLastPosChange.getDoubleValue()); if (currentPosition != previousPositon) { this.finiteDifferenceVelocity.update(); this.finiteDifferenceAccel.update(); if (determineIfDirectionChanged(currentPosition)) { unfilteredVelocity.set(0.0); positionPredicted.set(currentPosition - lastPositionIncrement.getDoubleValue()); } else { unfilteredVelocity.set(finiteDifferenceVelocity.getDoubleValue()); positionPredicted.set(currentPosition + lastPositionIncrement.getDoubleValue()); } lastPosChangeTimeInterval.set(timeSinceLastPosChange.getDoubleValue()); lastPositionIncrement.set(currentPosition - previousPositon); } else { velocityIfEncoderTicksNowConstantAccel.set( finiteDifferenceVelocity.getDoubleValue() + finiteDifferenceAccel.getDoubleValue() * timeSinceLastPosChange.getDoubleValue()); boolean velSlowedDownSincePosTakingTooLongToChange = timeSinceLastPosChange.getDoubleValue() > lastPosChangeTimeInterval.getDoubleValue(); if (velSlowedDownSincePosTakingTooLongToChange && useDecay.getBooleanValue()) { this.unfilteredVelocity.set(velocityIfEncoderTicksNow.getDoubleValue()); } } /** ************ SET VALUE ************************ */ this.set(alphaFilter(velocityIfEncoderTicksNowConstantAccel.getDoubleValue())); }
public void run() { while (true) { final DoubleYoVariable headingDotConstant = (DoubleYoVariable) registry.getVariable("headingDotConstant"); final DoubleYoVariable velocityXConstant = (DoubleYoVariable) registry.getVariable("velocityXConstant"); BooleanYoVariable multisenseControlsSpeedAndHeading = (BooleanYoVariable) registry.getVariable("isMultisenseControllingSpeedAndHeading"); DoubleYoVariable desiredHeadingDot = (DoubleYoVariable) registry.getVariable("RateBasedDesiredHeadingControlModule", "desiredHeadingDot"); DoubleYoVariable desiredVelocityX = (DoubleYoVariable) registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityX"); DoubleYoVariable multisenseHeading = (DoubleYoVariable) registry.getVariable("multisenseHeading"); DoubleYoVariable multisenseMagnitude = (DoubleYoVariable) registry.getVariable("multisenseMagnitude"); if (multisenseControlsSpeedAndHeading == null || desiredHeadingDot == null || desiredVelocityX == null || multisenseHeading == null || multisenseMagnitude == null) { continue; } if (multisenseControlsSpeedAndHeading.getBooleanValue()) { if (Math.abs(multisenseHeading.getDoubleValue()) > EPSILON) { currentHeadingDot = multisenseHeading.getDoubleValue() * headingDotConstant.getDoubleValue(); if (currentHeadingDot < minHeadingDot) currentHeadingDot = minHeadingDot; if (currentHeadingDot > maxHeadingDot) currentHeadingDot = maxHeadingDot; } else { currentHeadingDot = 0.0; } double newVelocity = multisenseMagnitude.getDoubleValue() * velocityXConstant.getDoubleValue(); // --(!) stop at half a meter from centroid of obj if (multisenseMagnitude.getValueAsDouble() > 0.5) { // -- This precaution may not be crucial, but // -- ramp up velocity for large jumps double deltaVelocity = newVelocity - currentVelocityX; if (Math.abs(deltaVelocity) > .1) currentVelocityX += deltaVelocity / Math.abs(deltaVelocity) * 0.05; else currentVelocityX = newVelocity; if (currentVelocityX < minVeclocity) currentVelocityX = minVeclocity; if (currentVelocityX > maxVelocity) currentVelocityX = maxVelocity; } else { currentVelocityX = 0.0; } desiredHeadingDot.set(currentHeadingDot); desiredVelocityX.set(currentVelocityX); } try { Thread.sleep(100); // 10Hz } catch (InterruptedException e) { e.printStackTrace(); } } }
public void hideVisualization() { if (!visualize) return; showViz.set(false); }
@Override public boolean hasInputBeenSet() { return haveInputsBeenSet.getBooleanValue(); }
public boolean isDone() { return currentTime.getDoubleValue() >= desiredTrajectoryTime.getDoubleValue() && !isCurrentAngleBeingAdjusted.getBooleanValue(); }
public void showVisualization() { if (!visualize) return; showViz.set(true); }
public CirclePoseTrajectoryGenerator( String namePrefix, ReferenceFrame trajectoryFrame, DoubleProvider trajectoryTimeProvider, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry); this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry); // this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2, // registry); this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry); // this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6, // registry); this.desiredTrajectoryTimeProvider = trajectoryTimeProvider; this.trajectoryFrame = trajectoryFrame; initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry); initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry); initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry); finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry); isCurrentAngleBeingAdjusted = new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry); maximumAngleTrackingErrorTolerated = new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry); maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0)); currentControlledFrameRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry); currentAngleTrackingError = new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry); currentAdjustedRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry); desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry); currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry); yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry); yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry); yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry); yoCurrentVelocity = new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry); yoCurrentAcceleration = new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry); yoInitialOrientation = new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry); yoFinalOrientation = new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry); yoCurrentOrientation = new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry); yoCurrentAngularVelocity = new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry); yoCurrentAngularAcceleration = new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry); yoInitialPositionWorld = new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry); yoFinalPositionWorld = new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry); yoCurrentPositionWorld = new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry); yoCurrentAdjustedPositionWorld = new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry); circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry); rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry); rotationAxis.set(0.0, 0.0, 1.0); circleFrame = new ReferenceFrame("CircleFrame", trajectoryFrame) { private static final long serialVersionUID = 9102217353690768074L; private final Vector3d localTranslation = new Vector3d(); private final Vector3d localRotationAxis = new Vector3d(); private final AxisAngle4d localAxisAngle = new AxisAngle4d(); @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { circleOrigin.get(localTranslation); rotationAxis.get(localRotationAxis); GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis); transformToParent.set(localAxisAngle, localTranslation); } }; tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame); yoTangentialCircleFramePose = new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry); if (this.visualize && yoGraphicsListRegistry != null) { final YoGraphicPosition currentPositionViz = new YoGraphicPosition( namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue()); final YoGraphicPosition currentAdjustedPositionViz = new YoGraphicPosition( namePrefix + "CurrentAdjustedPosition", yoCurrentAdjustedPositionWorld, 0.023, YoAppearance.Gold()); final YoGraphicPosition initialPositionViz = new YoGraphicPosition( namePrefix + "InitialPosition", yoInitialPositionWorld, 0.02, YoAppearance.BlueViolet()); final YoGraphicPosition finalPositionViz = new YoGraphicPosition( namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red()); final YoGraphicCoordinateSystem tangentialFrameViz = new YoGraphicCoordinateSystem( namePrefix + "TangentialFrame", yoTangentialCircleFramePose, 0.2, YoAppearance.Pink()); YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj"); yoGraphicsList.add(currentPositionViz); yoGraphicsList.add(currentAdjustedPositionViz); yoGraphicsList.add(initialPositionViz); yoGraphicsList.add(finalPositionViz); yoGraphicsList.add(tangentialFrameViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); bagOfBalls = new BagOfBalls( numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry); showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry); showViz.addVariableChangedListener( new VariableChangedListener() { public void variableChanged(YoVariable<?> v) { boolean visible = showViz.getBooleanValue(); currentPositionViz.setVisible(visible); currentAdjustedPositionViz.setVisible(visible); initialPositionViz.setVisible(visible); finalPositionViz.setVisible(visible); tangentialFrameViz.setVisible(visible); bagOfBalls.setVisible(visible); } }); showViz.notifyVariableChangedListeners(); } else { visualize = false; bagOfBalls = null; showViz = null; } parentRegistry.addChild(registry); }
@Override public void doControl() { if (!isDone.getBooleanValue() && !haveInputsBeenSet.getBooleanValue()) checkForNewInputs(); if (haveInputsBeenSet.getBooleanValue()) pipeLine.doControl(); }
public void setIsSlipping(boolean isSlipping) { slip.set(isSlipping); }
public boolean isSlipping() { return slip.getBooleanValue(); }
private boolean doSlipThisStance() { slipNextStep.set(random.nextDouble() < probabilitySlip); return slipNextStep.getBooleanValue(); }
@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 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)); }