public void extractFootstepSolutions(ArrayList<YoFramePoint2d> footstepSolutionsToPack, ArrayList<YoFramePoint2d> referenceFootstepLocations,
ArrayList<Footstep> upcomingFootsteps, int numberOfFootstepsToConsider,
ICPOptimizationSolver solver)
{
boolean firstStepAdjusted = false;
for (int i = 0; i < numberOfFootstepsToConsider; i++)
{
solver.getFootstepSolutionLocation(i, locationSolution);
upcomingFootsteps.get(i).getPosition2d(upcomingFootstepLocation);
ReferenceFrame deadbandFrame = upcomingFootsteps.get(i).getSoleReferenceFrame();
boolean footstepWasAdjusted = applyLocationDeadband(locationSolution, upcomingFootstepLocation, referenceFootstepLocations.get(i).getFrameTuple2d(), deadbandFrame,
footstepDeadband.getDoubleValue(), footstepSolutionResolution.getDoubleValue());
if (i == 0)
firstStepAdjusted = footstepWasAdjusted;
footstepSolutionsToPack.get(i).set(locationSolution);
}
this.footstepWasAdjusted.set(firstStepAdjusted);
}