How to use us.ihmc.humanoidRobotics.footstep

  public static FootstepTiming[] createTimings(int numberOfTimings)
  {
   FootstepTiming[] timings = new FootstepTiming[numberOfTimings];
   for (int i = 0; i < numberOfTimings; i++)
   {
     timings[i] = new FootstepTiming();
   }
   return timings;
  }
}

public Footstep(RigidBody endEffector, RobotSide robotSide, ReferenceFrame soleFrame, PoseReferenceFrame poseReferenceFrame, boolean trustHeight,
        List<Point2d> predictedContactPoints)
{
 this(createAutomaticID(endEffector), endEffector, robotSide, soleFrame, poseReferenceFrame, trustHeight, predictedContactPoints, TrajectoryType.DEFAULT,
    0.0);
}

public static Footstep[] createFootsteps(int numberOfSteps)
{
 Footstep[] footsteps = new Footstep[numberOfSteps];
 for (int i = 0; i < numberOfSteps; i++)
 {
   footsteps[i] = new Footstep();
 }
 return footsteps;
}

private void assertStepSide(String message, Footstep footstep, RobotSide expectedSide)
{
 if (expectedSide != null)
   assertTrue(message + " first step should be " + expectedSide, expectedSide == footstep.getRobotSide());
}

  public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot)
  {
   RobotSide robotSide = footstep.getRobotSide();
   int index = indices.get(robotSide);

   SingleFootstepVisualizer singleFootstepVisualizer = singleFootstepVisualizers.get(robotSide).get(index);

   singleFootstepVisualizer.visualizeFootstep(footstep, bipedFoot);
   index++;

   if (index >= singleFootstepVisualizers.get(robotSide).size())
   {
     index = 0;
   }

   indices.set(robotSide, index);
  }
}

private void testAPoint(boolean assertPositionConditions, boolean assertPointConditions) throws InsufficientDataException
{
  FootSpoof footSpoof = new FootSpoof("footSpoof");
  testAPoint(assertPositionConditions, assertPointConditions, footSpoof);
}

public FootstepTiming(double swingTime, double transferTime)
{
 setTimings(swingTime, transferTime);
}

public void setPositionChangeOnlyXY(FramePoint2DReadOnly position2d)
{
 position2d.checkReferenceFrameMatch(footstepPose);
 setX(position2d.getX());
 setY(position2d.getY());
}

public void setCurrentDesiredFootstep(Footstep currentDesiredFootstep)
{
 this.currentDesiredFootstep.setPose(currentDesiredFootstep);
}

public static FramePoint3D getCenterOfFootstep(Footstep footstep)
{
 return getCenterOfFootstep(footstep, RobotSide.LEFT, 0.0, 0.0);
}

public void visualizeInitialFootsteps(SideDependentList<? extends ContactablePlaneBody> bipedFeet, SideDependentList<Footstep> initialFeet)
{
 bagOfSingleFootstepVisualizers.visualizeFootstep(initialFeet.get(RobotSide.LEFT), bipedFeet.get(RobotSide.LEFT));
 bagOfSingleFootstepVisualizers.visualizeFootstep(initialFeet.get(RobotSide.RIGHT), bipedFeet.get(RobotSide.RIGHT));
}

private boolean betterThanType(Footstep.FootstepType type, Footstep.FootstepType typeToCompareAgainst)
{
 return type.ordinal() <= typeToCompareAgainst.ordinal();
}

public boolean isNextFootstepFor(RobotSide swingSide)
{
 if (!hasUpcomingFootsteps())
   return false;
 else
   return peek(0).getRobotSide() == swingSide;
}

public FootstepTask(FullHumanoidRobotModel fullRobotModel, RobotSide robotSide, FootstepListBehavior footstepListBehavior, FramePose3D footPose)
{
 super(footstepListBehavior);
 footsteps.add(new Footstep(robotSide, footPose));
 this.footstepListBehavior = footstepListBehavior;
}

public FootstepTiming(double swingTime, double touchdownTime, double transferTime)
{
 setTimings(swingTime, touchdownTime, transferTime);
}

public static FramePoint getCenterOfFootstep(Footstep footstep)
{
 return getCenterOfFootstep(footstep, RobotSide.LEFT, 0.0, 0.0);
}

public Footstep(RigidBody endEffector, RobotSide robotSide, ReferenceFrame soleFrame, PoseReferenceFrame poseReferenceFrame)
{
 this(createAutomaticID(endEffector), endEffector, robotSide, soleFrame, poseReferenceFrame, true);
}

public static Footstep generateStandingFootstep(RobotSide side, RigidBodyBasics foot, ReferenceFrame soleFrame)
{
 FramePose3D footFramePose = new FramePose3D(soleFrame);
 footFramePose.changeFrame(worldFrame);
 boolean trustHeight = false;
 Footstep footstep = new Footstep(side, footFramePose, trustHeight);
 return footstep;
}

public FootstepTiming(double swingTime, double transferTime)
{
 setTimings(swingTime, transferTime);
}

public Footstep(RigidBody endEffector, RobotSide robotSide, ReferenceFrame soleFrame, PoseReferenceFrame poseReferenceFrame, boolean trustHeight)
{
 this(createAutomaticID(endEffector), endEffector, robotSide, soleFrame, poseReferenceFrame, trustHeight);
}