How to use

getZ

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private void assertAllStepsLevelAndZeroHeight(String message, ArrayList<Footstep> footSteps, double zToAnkle)
{
 double eps = 1e-15;
 for (Footstep footstep : footSteps)
 {
   assertEquals(message + " foot should be level and zero height.", 0.0, footstep.getZ(), eps);
 }
}

@Override
public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide,
    HeightMapWithPoints heightMap)
    throws InsufficientDataException
{
 FramePose2d footPose2d = new FramePose2d(ReferenceFrame.getWorldFrame(), new Point2d(soleX, soleY), yaw);
 Footstep footstep = generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap);
 double z = footstep.getZ();
 if (Double.isInfinite(z) || Double.isNaN(z))
 {
   System.out.println("Houston, we have a problem in the SimpleFootstepSnapper");
 }
 return footstep;
}

@Override
public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide,
    HeightMapWithPoints heightMap)
    throws InsufficientDataException
{
 FramePose2D footPose2d = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(soleX, soleY), yaw);
 Footstep footstep = generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap);
 double z = footstep.getZ();
 if (Double.isInfinite(z) || Double.isNaN(z))
 {
   System.out.println("Houston, we have a problem in the SimpleFootstepSnapper");
 }
 return footstep;
}

public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime)
{
 FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime,transferTime);
 for (int i = 0; i < footsteps.size(); i++)
 {
   Footstep footstep = footsteps.get(i);
   Point3d location = new Point3d(footstep.getX(), footstep.getY(), footstep.getZ());
   Quat4d orientation = new Quat4d();
   footstep.getOrientation(orientation);
   RobotSide footstepSide = footstep.getRobotSide();
   FootstepDataMessage footstepData = new FootstepDataMessage(footstepSide, location, orientation);
   footstepDataList.add(footstepData);
 }
 set(footstepDataList);
}

 transferToFootstep.setZ(transferToFootstep.getZ() + RandomNumbers.nextDouble(random, 0.0, maxZChange));
previousFootstep = transferToFootstep;