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.");
}
@Override
public void doControl() {
super.doControl();
for (RobotSide robotSide : RobotSide.values()) {
GroundContactPointsSlipper groundContactPointsSlipper =
groundContactPointsSlippers.get(robotSide);
switch (slipStateMap.get(robotSide).getEnumValue()) {
case NO_CONTACT:
{
if (footTouchedDown(robotSide)) {
if (doSlipThisStance()) {
slipStateMap.get(robotSide).set(SlipState.CONTACT_WILL_SLIP);
touchdownTimeForSlipMap.get(robotSide).set(robot.getTime());
} else
// Wait till foot lift back up before allowing a slip.
{
slipStateMap.get(robotSide).set(SlipState.CONTACT);
}
}
break;
}
case CONTACT_WILL_SLIP:
{
if (robot.getTime()
> (touchdownTimeForSlipMap.get(robotSide).getDoubleValue()
+ nextSlipAfterTimeDelta.getDoubleValue())) {
if (slipStateMap.get(robotSide.getOppositeSide()).getEnumValue()
== SlipState.CONTACT_SLIP) {
// Stop other foot from slipping, two slipping feet no implemented yet
groundContactPointsSlipper.setDoSlip(false);
slipStateMap.get(robotSide.getOppositeSide()).set(SlipState.CONTACT_DONE_SLIP);
}
slipStateMap.get(robotSide).set(SlipState.CONTACT_SLIP);
startSlipping(robotSide);
}
break;
}
case CONTACT_SLIP:
{
if (groundContactPointsSlipper.isDoneSlipping()) {
slipStateMap.get(robotSide).set(SlipState.CONTACT_DONE_SLIP);
}
break;
}
case CONTACT_DONE_SLIP:
case CONTACT:
{
if (footLiftedUp(robotSide)) {
slipStateMap.get(robotSide).set(SlipState.NO_CONTACT);
}
break;
}
}
}
}
