How to use

us.ihmc.humanoidRobotics.footstep.footstepSnapper.ConvexHullFootstepSnapper

constructor

public AdjustingFootstepSnapper(FootstepValueFunction valueFunction, FootstepSnappingParameters parameters)
{
 convexHullFootstepSnapper = new ConvexHullFootstepSnapper(valueFunction, parameters);
 this.footstepSnappingParameters = parameters;
 this.distanceAdjustment = parameters.getDistanceAdjustment();
 this.angleAdjustment = parameters.getAngleAdjustment();
}

public AdjustingFootstepSnapper(FootstepValueFunction valueFunction, QuadTreeFootstepSnappingParameters parameters)
{
 convexHullFootstepSnapper = new ConvexHullFootstepSnapper(valueFunction, parameters);
 this.footstepSnappingParameters = parameters;
 this.distanceAdjustment = parameters.getDistanceAdjustment();
 this.angleAdjustment = parameters.getAngleAdjustment();
}

private ConvexHullFootstepSnapper createConvexHullFootstepSnapper()
{
 QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters();
 ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);
 return footstepSnapper;
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testBasicCropping()
{
 QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters();
 ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);
 List<Point2D> pointsToCrop = new ArrayList<Point2D>();
 pointsToCrop.add(new Point2D(1,1));
 pointsToCrop.add(new Point2D(-1,1));
 pointsToCrop.add(new Point2D(-1,-1));
 pointsToCrop.add(new Point2D(1,-1));
 pointsToCrop.add(new Point2D(1.1,0));
 List<Point2D> finalPoints = footstepSnapper.reduceListOfPointsByArea(pointsToCrop, 4);
 assertTrue(finalPoints.size() == 4.0);
 ConvexPolygon2D endPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(finalPoints));
 assertEquals(4.0, endPolygon.getArea(), 1e-15);
}

ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);
double maskSafetyBuffer = 0.01;
double boundingBoxDimension = 0.3;

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

ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);
List<Point2D> pointsToCrop = new ArrayList<Point2D>();
Random random = new Random(82368L);

FootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);

QuadTreeFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters);