private AdjustingFootstepSnapper createAdjustingFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); AdjustingFootstepSnapper footstepSnapper = new AdjustingFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); return footstepSnapper; }
public SimpleFootstepValueFunction(QuadTreeFootstepSnappingParameters parameters) { this.parameters = parameters; updateFunction(); }
private ConvexHullFootstepSnapper createConvexHullFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); return footstepSnapper; }
public SimpleFootstepValueFunction(FootstepSnappingParameters parameters) { this.parameters = parameters; updateFunction(); }
@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);