private SideDependentList<Footstep> updatedStartFeet(double startX, double startY, double startYaw)
{
double leftXFactor = initialDeltaFeetLocalX / 2 * Math.cos(startYaw) - initialDeltaFeetLocalY / 2 * Math.sin(startYaw);
double leftYFactor = initialDeltaFeetLocalX / 2 * Math.sin(startYaw) + initialDeltaFeetLocalY / 2 * Math.cos(startYaw);
QuadTreeFootstepSnapper footstepSnapper = new SimpleFootstepSnapper();
Point2D leftFootStartPoint = new Point2D(startX + leftXFactor, startY + leftYFactor);
FramePose2D leftFootPose2d = new FramePose2D(WORLD_FRAME, leftFootStartPoint, startYaw + initialDeltaFeetYaw / 2);
Point2D rightFootStartPoint = new Point2D(startX - leftXFactor, startY - leftYFactor);
FramePose2D rightFootPose2d = new FramePose2D(WORLD_FRAME, rightFootStartPoint, startYaw - initialDeltaFeetYaw / 2);
FramePoint3D leftPosition = new FramePoint3D(WORLD_FRAME, leftFootStartPoint.getX(), leftFootStartPoint.getY(), 0);
FramePoint3D rightPosition = new FramePoint3D(WORLD_FRAME, rightFootStartPoint.getX(), rightFootStartPoint.getY(), 0);
FrameQuaternion leftOrientation = new FrameQuaternion(WORLD_FRAME, leftFootPose2d.getYaw(), 0.0, 0.0);
FrameQuaternion rightOrientation = new FrameQuaternion(WORLD_FRAME, rightFootPose2d.getYaw(), 0.0, 0.0);
leftContactableFoot.setSoleFrame(leftPosition, leftOrientation);
rightContactableFoot.setSoleFrame(rightPosition, rightOrientation);
Footstep leftStart = footstepSnapper.generateFootstepWithoutHeightMap(leftFootPose2d, feet.get(RobotSide.LEFT), contactableFeet.get(RobotSide.LEFT).getSoleFrame(),
RobotSide.LEFT, GROUND_HEIGHT, PLANE_NORMAL);
Footstep rightStart = footstepSnapper.generateFootstepWithoutHeightMap(rightFootPose2d, feet.get(RobotSide.RIGHT), contactableFeet.get(RobotSide.RIGHT).getSoleFrame(),
RobotSide.RIGHT, GROUND_HEIGHT, PLANE_NORMAL);
SideDependentList<Footstep> startFeet = new SideDependentList<Footstep>(leftStart, rightStart);
return startFeet;
}