FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("List of Points", footstepSnapper, new Graphics3DObject(), visualizeAndKeepUp); helper.createHeightMap(points, rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes); helper.drawPoints(points, resolution / 2.0, YoAppearance.Grey());
FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("List of Points", footstepSnapper, new Graphics3DObject(), visualizeAndKeepUp); helper.createHeightMap(points, rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes); helper.drawPoints(points, resolution / 2.0, YoAppearance.Blue());
FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Steps", footstepSnapper, groundProfile.getLinkGraphics(), visualizeAndKeepUp); int maxNodes = 1000000; helper.createHeightMap(groundProfile, rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes);
FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Steps", footstepSnapper, groundProfile.getLinkGraphics(), visualizeAndKeepUp); int maxNodes = 1000000; helper.createHeightMap(groundProfile, rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes);
FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Steps", footstepSnapper, groundProfile.getLinkGraphics(), visualizeAndKeepUp); int maxNodes = 1000000; helper.createHeightMap(groundProfile, rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes);
FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Steps", footstepSnapper, groundProfile.getLinkGraphics(), visualizeAndKeepUp); int maxNodes = 1000000; helper.createHeightMap(groundProfile, rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes);
public void createHeightMap(us.ihmc.graphicsDescription.HeightMap inputHeightMap, BoundingBox2D testingRange, double resolution, double heightThreshold, double quadTreeMaxMultiLevelZChangeToFilterNoise, int maxSameHeightPointsPerNode, double maxAllowableXYDistanceForAPointToBeConsideredClose, int maxNodes) { double minX = testingRange.getMinPoint().getX(); double maxX = testingRange.getMaxPoint().getX(); double minY = testingRange.getMinPoint().getY(); double maxY = testingRange.getMaxPoint().getY(); ArrayList<Point3D> listOfPoints = new ArrayList<Point3D>(); for (double x = minX; x < maxX; x = x + resolution) { for (double y = minY; y < maxY; y = y + resolution) { double z = inputHeightMap.heightAt(x, y, 0.0); listOfPoints.add(new Point3D(x, y, z)); if (visualize) { Graphics3DObject staticLinkGraphics = new Graphics3DObject(); staticLinkGraphics.translate(new Vector3D(x, y, z + 0.001)); staticLinkGraphics.addCube(0.002, 0.002, 0.002, YoAppearance.Blue()); scs.addStaticLinkGraphics(staticLinkGraphics); } } } createHeightMap(listOfPoints, testingRange, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes); }
public void testSimpleFootstepSnapperOnBumpyGround() throws InsufficientDataException { boolean assertPositionConditions = true; boolean assertPointConditions = false; boolean visualizeAndKeepUp = false; GroundProfile3D groundProfile = createBumpyGroundProfile(); SimpleFootstepSnapper footstepSnapper = createSimpleFootstepSnapper(); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); BoundingBox2D rangeOfPointsToTest = new BoundingBox2D(-1.0, -1.0, 1.0, 1.0); FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Simple Bumpy Ground", footstepSnapper, null, visualizeAndKeepUp); double resolution = 0.02; double heightThreshold = 0.002; double quadTreeMaxMultiLevelZChangeToFilterNoise = 0.2; int maxSameHeightPointsPerNode = 20; double maxAllowableXYDistanceForAPointToBeConsideredClose = 0.2; int maxNodes = 1000000; helper.createHeightMap(groundProfile.getHeightMapIfAvailable(), rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes); int numberOfPointsToTest = 1000; helper.testRandomPoints(numberOfPointsToTest, rangeOfPointsToTest, assertPositionConditions, assertPointConditions); if (visualizeAndKeepUp) { ThreadTools.sleepForever(); } }
private void testRandomPoints(int numberOfPointsToTest, BoundingBox2D rangeOfPointsToTest, boolean assertPositionConditions, boolean assertPointConditions) throws InsufficientDataException { double minX = rangeOfPointsToTest.getMinPoint().getX(); double maxX = rangeOfPointsToTest.getMaxPoint().getX(); double minY = rangeOfPointsToTest.getMinPoint().getY(); double maxY = rangeOfPointsToTest.getMaxPoint().getY(); for (int i = 0; i < numberOfPointsToTest; i++) { soleX.set(RandomNumbers.nextDouble(random, minX, maxX)); soleY.set(RandomNumbers.nextDouble(random, minY, maxY)); soleYaw.set(RandomNumbers.nextDouble(random, Math.PI)); testAPoint(assertPositionConditions, assertPointConditions); } }
private void testAPoint(boolean assertPositionConditions, boolean assertPointConditions) throws InsufficientDataException { FootSpoof footSpoof = new FootSpoof("footSpoof"); testAPoint(assertPositionConditions, assertPointConditions, footSpoof); }
public void testAPoint(double soleX, double soleY, double soleYaw, boolean assertPositionConditions, boolean assertPointConditions, ContactablePlaneBody footstepBody) throws InsufficientDataException { this.soleX.set(soleX); this.soleY.set(soleY); this.soleYaw.set(soleYaw); testAPoint(assertPositionConditions, assertPointConditions, footstepBody); }
private void testAPoint(double soleX, double soleY, double soleYaw, boolean assertPositionConditions, boolean assertPointConditions) throws InsufficientDataException { this.soleX.set(soleX); this.soleY.set(soleY); this.soleYaw.set(soleYaw); testAPoint(assertPositionConditions, assertPointConditions); }