How to use

getPosition2d

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private FramePoint2D createReferenceLocations(Footstep upcomingFootstep)
{
 FramePoint2D referenceLocation = new FramePoint2D();
 upcomingFootstep.getPosition2d(referenceLocation);
 return referenceLocation;
}

public void addFootstepToPlan(Footstep footstep)
{
 if (footstep != null)
 {
   upcomingFootsteps.add(footstep);
   footstep.getPosition2d(tmpFramePoint2d);
   upcomingFootstepLocations.get(upcomingFootsteps.size() - 1).set(tmpFramePoint2d);
   inputHandler.addFootstepToPlan(footstep);
   footstepSolutions.get(upcomingFootsteps.size() - 1).set(tmpFramePoint2d);
 }
}

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