@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);
}
@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;
}
@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);
}
public void setIsSlipping(boolean isSlipping) {
slip.set(isSlipping);
}
private boolean doSlipThisStance() {
slipNextStep.set(random.nextDouble() < probabilitySlip);
return slipNextStep.getBooleanValue();
}
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));
}
@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();
}