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()));
}