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));
}