@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);
}
@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);
}
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);
}
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);
}
@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;
}
}
public int getCollisionCount() {
return collisionCount.getIntegerValue();
}
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());
}
}
@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);
}