private FramePoint computeDesiredFootPosition(
RobotSide upcomingSwingLegSide,
ReferenceFrame upcomingSupportAnkleZUpFrame,
ReferenceFrame upcomingSupportAnkleFrame,
FrameVector2d desiredOffsetFromAnkle,
Matrix3d swingFootToWorldRotation) {
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
desiredOffsetFromAnkle.changeFrame(upcomingSupportAnkleFrame);
FramePoint footstepPosition =
new FramePoint(
upcomingSupportAnkleFrame,
desiredOffsetFromAnkle.getX(),
desiredOffsetFromAnkle.getY(),
0.0);
footstepPosition.changeFrame(worldFrame);
return footstepPosition;
}
private void computeDistanceAndAngleToDestination(
ReferenceFrame supportAnkleZUpFrame,
RobotSide swingLegSide,
FramePoint2d desiredDestination) {
FramePoint2d destinationInAnkleFrame = new FramePoint2d(desiredDestination);
destinationInAnkleFrame.changeFrame(supportAnkleZUpFrame);
FramePoint2d squaredUpMidpointInAnkleFrame =
new FramePoint2d(
supportAnkleZUpFrame,
0.0,
swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue() / 2.0));
FrameVector2d midpointToDestination = new FrameVector2d(destinationInAnkleFrame);
midpointToDestination.sub(squaredUpMidpointInAnkleFrame);
distanceToDestination.set(midpointToDestination.length());
angleToDestination.set(Math.atan2(midpointToDestination.getY(), midpointToDestination.getX()));
}
private FrameVector2d computeDesiredOffsetFromSupportAnkle(
ReferenceFrame supportAnkleZUpFrame,
RobotSide swingLegSide,
double angleToDestination,
double distanceToDestination) {
if (distanceToDestination < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
return new FrameVector2d(
supportAnkleZUpFrame,
0.0,
swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue()));
}
double absoluteAngleToDestination;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
absoluteAngleToDestination =
Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI));
break;
}
case LEFT:
{
absoluteAngleToDestination =
Math.abs(
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0));
break;
}
case RIGHT:
{
absoluteAngleToDestination =
Math.abs(
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0));
break;
}
case FORWARD:
{
absoluteAngleToDestination = Math.abs(angleToDestination);
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
double minAngleBeforeShorterSteps = 0.1;
double maxAngleBeforeTurnInPlace = 0.5;
double percentToStepInXY =
1.0
- (absoluteAngleToDestination - minAngleBeforeShorterSteps)
/ (maxAngleBeforeTurnInPlace - minAngleBeforeShorterSteps);
if (percentToStepInXY > 1.0) percentToStepInXY = 1.0;
if (percentToStepInXY < 0.0) percentToStepInXY = 0.0;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
double backwardsDistanceReduction = 0.75;
desiredOffsetFromSquaredUp.set(
-backwardsDistanceReduction * percentToStepInXY * desiredStepForward.getDoubleValue(),
0.0);
break;
}
case LEFT:
{
desiredOffsetFromSquaredUp.set(
0.0, percentToStepInXY * desiredStepSideward.getDoubleValue());
break;
}
case RIGHT:
{
desiredOffsetFromSquaredUp.set(
0.0, -percentToStepInXY * desiredStepSideward.getDoubleValue());
break;
}
case FORWARD:
{
desiredOffsetFromSquaredUp.set(
percentToStepInXY * desiredStepForward.getDoubleValue(), 0.0);
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
double stepLength = desiredOffsetFromSquaredUp.length();
double maxDistanceToAllow =
distanceToDestination - 0.5 * DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE;
if (maxDistanceToAllow < 0.0) maxDistanceToAllow = 0.0;
if (stepLength > maxDistanceToAllow) {
desiredOffsetFromSquaredUp.scale(maxDistanceToAllow / stepLength);
stepLength = desiredOffsetFromSquaredUp.length();
}
if (stepLength > maxStepLength.getDoubleValue()) {
desiredOffsetFromSquaredUp.scale(maxStepLength.getDoubleValue() / stepLength);
}
FrameVector2d desiredOffsetFromAnkle =
new FrameVector2d(
supportAnkleZUpFrame,
desiredOffsetFromSquaredUp.getX(),
desiredOffsetFromSquaredUp.getY()
+ swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue()));
if (swingLegSide == RobotSide.LEFT) {
desiredOffsetFromAnkle.setY(
MathTools.clipToMinMax(
desiredOffsetFromAnkle.getY(),
minStepWidth.getDoubleValue(),
maxStepWidth.getDoubleValue()));
} else {
desiredOffsetFromAnkle.setY(
MathTools.clipToMinMax(
desiredOffsetFromAnkle.getY(),
-maxStepWidth.getDoubleValue(),
-minStepWidth.getDoubleValue()));
}
return desiredOffsetFromAnkle;
}