@Override
public Footstep predictFootstepAfterDesiredFootstep(
RobotSide supportLegSide, Footstep desiredFootstep) {
RobotSide futureSwingLegSide = supportLegSide;
ReferenceFrame futureSupportAnkleFrame = desiredFootstep.getPoseReferenceFrame();
ReferenceFrame futureSupportAnkleZUpFrame =
new ZUpFrame(ReferenceFrame.getWorldFrame(), futureSupportAnkleFrame, "ankleZUp");
futureSupportAnkleZUpFrame.update();
computeDistanceAndAngleToDestination(
futureSupportAnkleZUpFrame, futureSwingLegSide, desiredDestination.getFramePoint2dCopy());
Matrix3d footToWorldRotation =
computeDesiredFootRotation(
angleToDestination.getDoubleValue(), futureSwingLegSide, futureSupportAnkleFrame);
FramePoint footstepPosition =
getDesiredFootstepPositionCopy(
futureSupportAnkleZUpFrame,
futureSupportAnkleFrame,
futureSwingLegSide,
desiredDestination.getFramePoint2dCopy(),
footToWorldRotation);
FrameOrientation footstepOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame());
double[] yawPitchRoll = new double[3];
RotationTools.convertMatrixToYawPitchRoll(footToWorldRotation, yawPitchRoll);
footstepOrientation.setYawPitchRoll(yawPitchRoll);
FramePose footstepPose = new FramePose(footstepPosition, footstepOrientation);
footstepPose.changeFrame(ReferenceFrame.getWorldFrame());
PoseReferenceFrame poseReferenceFrame =
new PoseReferenceFrame("poseReferenceFrame", footstepPose);
ContactablePlaneBody foot = contactableBodies.get(futureSwingLegSide);
boolean trustHeight = true;
return new Footstep(
foot.getRigidBody(),
futureSwingLegSide,
foot.getSoleFrame(),
poseReferenceFrame,
trustHeight);
}
