How to use

generateFootstepUsingHeightMap

method

in

us.ihmc.humanoidRobotics.footstep.footstepSnapper.ConvexHullFootstepSnapper

@Override
public Footstep generateFootstepUsingHeightMap(FramePose2d desiredSolePosition, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide,
    HeightMapWithPoints heightMap)
    throws InsufficientDataException
{
 Footstep toReturn = convexHullFootstepSnapper.generateFootstepUsingHeightMap(desiredSolePosition, foot, soleFrame, robotSide, heightMap);
 snapFootstep(toReturn, heightMap);
 return toReturn;
}

@Override
public Footstep generateFootstepUsingHeightMap(FramePose2D desiredSolePosition, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide,
    HeightMapWithPoints heightMap)
    throws InsufficientDataException
{
 Footstep toReturn = convexHullFootstepSnapper.generateFootstepUsingHeightMap(desiredSolePosition, foot, soleFrame, robotSide, heightMap);
 snapFootstep(toReturn, heightMap);
 return toReturn;
}

@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);
 return generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap);
}

@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);
 return generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap);
}

public void testFootstepAndPointsFromDataFile() throws NumberFormatException, InsufficientDataException, IOException
{
 QuadTreeFootstepSnappingParameters snappingParameters = new AtlasFootstepSnappingParameters();
 ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);
 double maskSafetyBuffer = 0.01;
 double boundingBoxDimension = 0.3;
 footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension);
 String baseName = "footstepListsForTesting/";
 String resourceName = baseName + "DataFromConvexHullSnapper1422988400956.txt";
 InputStream resourceAsStream = getClass().getClassLoader().getResourceAsStream(resourceName);
 FootstepPointsDataReader dataReader = new FootstepPointsDataReader(resourceAsStream);
 FootstepDataMessage footstepData = new FootstepDataMessage();
 footstepData.setRobotSide(RobotSide.LEFT.toByte());
 FootSpoof spoof = new FootSpoof("basicSpoof");
 FramePose2D desiredPose = new FramePose2D(ReferenceFrame.getWorldFrame());
 List<Point3D> listOfPoints = new ArrayList<>();
 while (dataReader.hasAnotherFootstepAndPoints())
 {
   listOfPoints = dataReader.getNextSetPointsAndFootstep(footstepData);
   desiredPose.setIncludingFrame(ReferenceFrame.getWorldFrame(), footstepData.getLocation().getX(), footstepData.getLocation().getY(),
                    footstepData.getOrientation().getYaw());
   Footstep footstep = footstepSnapper.generateFootstepUsingHeightMap(desiredPose, spoof.getRigidBody(), spoof.getSoleFrame(),
              RobotSide.fromByte(footstepData.getRobotSide()), listOfPoints, 0.0);
   assertTrue(footstep.getFootstepType() != Footstep.FootstepType.BAD_FOOTSTEP);
 }
}