private void generateRandomSlipParamters() {
double randomSlipTranslateX =
pseudoRandomRealNumberWithinRange(
minTranslationToSlipNextStep.getX(), maxTranslationToSlipNextStep.getX());
double randomSlipTranslateY =
pseudoRandomRealNumberWithinRange(
minTranslationToSlipNextStep.getY(), maxTranslationToSlipNextStep.getY());
double randomSlipTranslateZ =
pseudoRandomRealNumberWithinRange(
minTranslationToSlipNextStep.getZ(), maxTranslationToSlipNextStep.getZ());
nextTranslationToSlip.set(randomSlipTranslateX, randomSlipTranslateY, randomSlipTranslateZ);
nextRotationToSlip.setYawPitchRoll(
pseudoRandomRealNumberWithinRange(
minRotationToSlipNextStep.getYawPitchRoll(),
maxRotationToSlipNextStep.getYawPitchRoll()));
double randomSlipAfterTimeDelta =
pseudoRandomPositiveNumberWithinRange(
minSlipAfterTimeDelta.getDoubleValue(), maxSlipAfterTimeDelta.getDoubleValue());
double randomPercentToSlipPerTick =
pseudoRandomPositiveNumberWithinRange(
minSlipPercentSlipPerTick.getDoubleValue(), maxSlipPercentSlipPerTick.getDoubleValue());
nextSlipAfterTimeDelta.set(randomSlipAfterTimeDelta);
nextSlipPercentSlipPerTick.set(randomPercentToSlipPerTick);
}
private void startSlipping(RobotSide robotSide) {
GroundContactPointsSlipper groundContactPointsSlipper =
groundContactPointsSlippers.get(robotSide);
generateRandomSlipParamters();
groundContactPointsSlipper.setGroundContactPoints(groundContactPointsMap.get(robotSide));
groundContactPointsSlipper.setPercentToSlipPerTick(nextSlipPercentSlipPerTick.getDoubleValue());
groundContactPointsSlipper.setDoSlip(true);
groundContactPointsSlipper.setSlipTranslation(nextTranslationToSlip.getVector3dCopy());
groundContactPointsSlipper.setSlipRotationYawPitchRoll(nextRotationToSlip.getYawPitchRoll());
// System.out.println("Slip of " + robotSide.getLowerCaseName() + " foot with amount" +
// nextTranslationToSlip.getVector3dCopy().toString()
// + " with rotation amount " +
// nextRotationToSlip.getFrameOrientationCopy().toStringAsYawPitchRoll()
// + " with slippercentage per tick " +
// nextSlipPercentSlipPerTick.getDoubleValue() + ", " + nextSlipAfterTimeDelta.getDoubleValue()
// + " [s] after touchdown.");
}
public void getOrientation(FrameOrientation orientationToPack) {
frameOrientation.getFrameOrientationIncludingFrame(orientationToPack);
}