public void initializeDesiredFootstep(RobotSide supportLegSide) {
RobotSide swingLegSide = supportLegSide.getOppositeSide();
ReferenceFrame supportAnkleZUpFrame = ankleZUpFrames.get(supportLegSide);
ReferenceFrame supportAnkleFrame = ankleFrames.get(supportLegSide);
computeDistanceAndAngleToDestination(
supportAnkleZUpFrame, swingLegSide, desiredDestination.getFramePoint2dCopy());
if (distanceToDestination.getDoubleValue() < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
numberBlindStepsInPlace.increment();
}
Matrix3d footToWorldRotation =
computeDesiredFootRotation(
angleToDestination.getDoubleValue(), swingLegSide, supportAnkleFrame);
FramePoint footstepPosition =
getDesiredFootstepPositionCopy(
supportAnkleZUpFrame,
supportAnkleFrame,
swingLegSide,
desiredDestination.getFramePoint2dCopy(),
footToWorldRotation);
setYoVariables(swingLegSide, footToWorldRotation, footstepPosition);
}
@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);
}
@Override
public void updateCoP() {
yoResolvedCoP.setToZero();
}