public PlanarRegionsList getPlanarRegionsList() { ensureHasBeenGenerated(); PlanarRegionsList planarRegionsList = new PlanarRegionsList(); for (int i = 0; i < planarRegionsLists.size(); i++) { planarRegionsList.getPlanarRegionsAsList().addAll(planarRegionsLists.get(i).getPlanarRegionsAsList()); } return planarRegionsList; }
/** * Snaps an XY polygon down onto a PlanarRegionsList. Returns the RigidBodyTransform required to perform the snap. * * @param polygonToSnap ConvexPolygon2d that is to be snapped to the PlanarRegionsList. * @param planarRegionsListToSnapTo PlanarRegionsList that the polygon will be snapped to. * @param regionToPack the planar region that this snaps to will be packed here (can be null). * @return RigidBodyTransform Transform required to snap the polygon down onto the PlanarRegion. */ public static RigidBodyTransform snapPolygonToPlanarRegionsList(ConvexPolygon2DBasics polygonToSnap, PlanarRegionsList planarRegionsListToSnapTo, PlanarRegion regionToPack) { return snapPolygonToPlanarRegionsList(polygonToSnap, planarRegionsListToSnapTo.getPlanarRegionsAsList(), regionToPack); }
/** * Projects the given point onto a planar region from the list. The projection is done along the * z axis in world frame and if there is multiple regions that the point can be projected onto, * the highest intersection point will be returned. * <p> * Will return null if the is no planar region above or below the point. */ public static Point3DReadOnly projectPointToPlanesVertically(Point3DReadOnly pointInWorld, PlanarRegionsList regions) { return projectPointToPlanesVertically(pointInWorld, regions.getPlanarRegionsAsList()); }
/** * Find all the planar regions that intersect with the given convex polygon. The algorithm is * equivalent to projecting all the regions onto the XY-plane and then finding the regions * intersecting with the given convex polygon. * * @param convexPolygon the query. * @return the list of planar regions intersecting with the given polygon. Returns null when no * region intersects. */ public static List<PlanarRegion> findPlanarRegionsIntersectingPolygon(ConvexPolygon2DReadOnly convexPolygon, PlanarRegionsList regions) { return findPlanarRegionsIntersectingPolygon(convexPolygon, regions.getPlanarRegionsAsList()); }
private void randomizeIDs(PlanarRegionsList planarRegionsList) { for (PlanarRegion region : planarRegionsList.getPlanarRegionsAsList()) region.setRegionId(random.nextInt()); messager.submitMessage(dataTopic, planarRegionsList); }
/** * Finds and returns the closest point the the provided point on the planar regions. */ public static Point3D projectPointToPlanes(Point3DReadOnly point, PlanarRegionsList regions) { double smallestDistance = Double.POSITIVE_INFINITY; Point3D closestPoint = null; for (PlanarRegion region : regions.getPlanarRegionsAsList()) { Point3D intersection = closestPointOnPlane(point, region); double distance = intersection.distance(point); if (closestPoint == null || distance < smallestDistance) { smallestDistance = distance; closestPoint = intersection; } } return closestPoint; }
public static Point3D intersectRegionsWithRay(PlanarRegionsList regions, Point3D rayStart, Vector3D rayDirection) { double smallestDistance = Double.POSITIVE_INFINITY; Point3D closestIntersection = null; for (PlanarRegion region : regions.getPlanarRegionsAsList()) { Point3D intersection = intersectRegionWithRay(region, rayStart, rayDirection); if (intersection == null) { continue; } double distance = intersection.distance(rayStart); if (distance < smallestDistance) { smallestDistance = distance; closestIntersection = intersection; } } return closestIntersection; }
@Override public void start(Stage primaryStage) throws Exception { primaryStage.setTitle(getClass().getSimpleName()); PlanarRegionsList planarRegionData; if (defaultFile != null) planarRegionData = PlanarRegionDataImporter.importPlanarRegionData(defaultFile); else planarRegionData = PlanarRegionDataImporter.importUsingFileChooser(primaryStage); if (planarRegionData == null) Platform.exit(); View3DFactory view3dFactory = new View3DFactory(600, 480); planarRegionData.getPlanarRegionsAsList().stream().map(this::createRegionGraphics).forEach(view3dFactory::addNodeToView); view3dFactory.addCameraController(true); view3dFactory.addWorldCoordinateSystem(0.3); primaryStage.setScene(view3dFactory.getScene()); primaryStage.show(); }
private static void writePlanarRegionsData(Path folderPath, PlanarRegionsList planarRegionData) throws IOException { File header = new File(folderPath.toFile(), "header.txt"); FileWriter fileWriter = new FileWriter(header); Map<Integer, MutableInt> regionIdToIndex = new HashMap<>(); for (PlanarRegion region : planarRegionData.getPlanarRegionsAsList()) { Point3D origin = new Point3D(); Vector3D normal = new Vector3D(); region.getPointInRegion(origin); region.getNormal(normal); int numberOfConvexPolygons = region.getNumberOfConvexPolygons(); int[] convexPolygonsSizes = new int[numberOfConvexPolygons]; for (int i = 0; i < numberOfConvexPolygons; i++) convexPolygonsSizes[i] = region.getConvexPolygon(i).getNumberOfVertices(); int regionId = region.getRegionId(); MutableInt regionIndex = regionIdToIndex.getOrDefault(regionId, new MutableInt(0)); regionIdToIndex.put(regionId, regionIndex); regionIndex.increment(); fileWriter.write("regionId: " + Integer.toString(regionId)); fileWriter.write(", index: " + Integer.toString(regionIndex.getValue().intValue())); fileWriter.write(", origin: " + origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: " + normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write(", concave hull size: " + region.getConcaveHullSize()); fileWriter.write(", number of convex polygons: " + numberOfConvexPolygons + ", " + Arrays.toString(convexPolygonsSizes)); fileWriter.write("\n"); writePlanarRegionVertices(folderPath, region, regionIndex.intValue()); } fileWriter.close(); }
private void runTest(PlanarRegionsList planarRegionsList) { Point3D start = new Point3D(); Point3D goal = new Point3D(2.0, -1.0, 0.0); NavigableRegionsManager navigableRegionsManager = new NavigableRegionsManager(planarRegionsList.getPlanarRegionsAsList()); List<Point3DReadOnly> path = navigableRegionsManager.calculateBodyPathWithOcclusions(start, goal); if(visualize) { visualize(path, planarRegionsList, start, goal); } }
public List<PlanarRegion> getOrCreateNearbyRegions(double roundedX, double roundedY) { int hashCode = FootstepNode.computePlanarRegionsHashCode(roundedX, roundedY); if (nearbyPlanarRegions.containsKey(hashCode)) return nearbyPlanarRegions.get(hashCode); Point2DReadOnly centerPoint = new Point2D(roundedX, roundedY); List<PlanarRegion> nearbyRegions = PlanarRegionTools .filterPlanarRegionsWithBoundingCircle(centerPoint, proximityForPlanarRegionsNearby, planarRegionsList.getPlanarRegionsAsList()); nearbyPlanarRegions.put(hashCode, nearbyRegions); return nearbyRegions; }
List<PlanarRegion> planarRegions = planarRegionsList.getPlanarRegionsAsList();
public ConvexPolygon2D snapAndWiggle(FramePose3D solePose, ConvexPolygon2DReadOnly footStepPolygon, boolean walkingForward) throws SnappingFailedException { if (planarRegionsList == null) { return null; } planarRegionsList.getPlanarRegionsAsList().removeIf(region -> region.getConvexHull().getArea() < parameters.getMinPlanarRegionArea()); planarRegionsList.getPlanarRegionsAsList().removeIf(region -> region.getNormal().getZ() < Math.cos(parameters.getMaxPlanarRegionAngle())); FramePose3D solePoseBeforeSnapping = new FramePose3D(solePose); PoseReferenceFrame soleFrameBeforeSnapping = new PoseReferenceFrame("SoleFrameBeforeSnapping", solePose); FrameConvexPolygon2D footPolygon = new FrameConvexPolygon2D(soleFrameBeforeSnapping, footStepPolygon); footPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); // this works if the soleFrames are z up. if(isOnBoundaryOfPlanarRegions(planarRegionsList, footPolygon)) { /* * If foot is on the boundary of planar regions, don't snap/wiggle but * set it to the nearest plane's height */ FixedFramePoint3DBasics footPosition = solePose.getPosition(); PlanarRegion closestRegion = planarRegionsList.findClosestPlanarRegionToPointByProjectionOntoXYPlane(footPosition.getX(), footPosition.getY()); solePose.setZ(closestRegion.getPlaneZGivenXY(footPosition.getX(), footPosition.getY())); return new ConvexPolygon2D(footStepPolygon); } ConvexPolygon2D foothold = doSnapAndWiggle(solePose, footStepPolygon, footPolygon); checkAndHandleTopOfCliff(solePoseBeforeSnapping, solePose, walkingForward, footStepPolygon, footPolygon); checkAndHandleBottomOfCliff(solePose); return foothold; }
@Override public void setPlanarRegions(PlanarRegionsList planarRegions) { super.setPlanarRegions(planarRegions); if (!hasPlanarRegions()) return; planarRegionPolytopes.clear(); for (PlanarRegion planarRegion : planarRegions.getPlanarRegionsAsList()) { ConvexPolytope planarRegionPolytope = new ConvexPolytope(); List<? extends Point2DReadOnly> pointsInPlanarRegion = planarRegion.getConvexHull().getVertexBufferView(); planarRegion.getTransformToWorld(tempTransform); for (Point2DReadOnly point : pointsInPlanarRegion) { tempPoint.setToZero(); tempPoint.set(point.getX(), point.getY(), 0.0); tempPoint.applyTransform(tempTransform); planarRegionPolytope.addVertex(tempPoint.getX(), tempPoint.getY(), tempPoint.getZ()); } planarRegionPolytopes.put(planarRegion, planarRegionPolytope); } }
navigableRegionsManager.setPlanarRegions(planarRegionsList.getPlanarRegionsAsList());
private void runTestAndAssert(PlannerTestEnvironments.PlannerTestData testData) { if (messager != null && visualize()) submitInfoToUI(testData); Random random = new Random(324); testData.getPlanarRegionsList().getPlanarRegionsAsList().forEach(region -> region.setRegionId(random.nextInt())); FootstepPlanner planner = getPlanner(); FootstepPlan footstepPlan = PlannerTools .runPlanner(planner, testData.getStartPose(), testData.getStartSide(), testData.getGoalPose(), testData.getPlanarRegionsList(), assertPlannerReturnedResult()); if (assertPlannerReturnedResult()) assertTrue(PlannerTools.isGoalNextToLastStep(testData.getGoalPose(), footstepPlan)); if (messager != null && visualize()) { messager.submitMessage(FootstepPlannerMessagerAPI.PlannerStatusTopic, FootstepPlannerStatus.PLANNING_STEPS); messager.registerTopicListener(PlannerParametersTopic, message -> { // TODO set parameters from message }); messager.submitMessage(FootstepPlannerMessagerAPI.FootstepPlanTopic, footstepPlan); messager.submitMessage(FootstepPlannerMessagerAPI.PlannerStatusTopic, FootstepPlannerStatus.IDLE); messager.submitMessage(FootstepPlannerMessagerAPI.PlannerTimeTakenTopic, planner.getPlanningDuration()); ThreadTools.sleep(10); messager.registerTopicListener(ComputePathTopic, request -> iterateOnPlan(testData)); if (keepUp()) ThreadTools.sleepForever(); } }
private boolean isOnBoundaryOfPlanarRegions(PlanarRegionsList planarRegionsList, FrameConvexPolygon2D footPolygonInWorld) { PoseReferenceFrame planarRegionFrame = new PoseReferenceFrame("PlanarRegionFrame", ReferenceFrame.getWorldFrame()); FrameConvexPolygon2D planarRegionPolygon = new FrameConvexPolygon2D(); ConvexPolygon2D planarRegionsBoundingPolygon = new ConvexPolygon2D(); planarRegionsBoundingPolygon.clear(); for(PlanarRegion region : planarRegionsList.getPlanarRegionsAsList()) { RigidBodyTransform transform = new RigidBodyTransform(); region.getTransformToWorld(transform); planarRegionFrame.setPoseAndUpdate(transform); planarRegionPolygon.set(region.getConvexHull()); planarRegionPolygon.setReferenceFrame(planarRegionFrame); planarRegionPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); planarRegionsBoundingPolygon.addVertices(planarRegionPolygon); } planarRegionsBoundingPolygon.update(); for (int i = 0; i < footPolygonInWorld.getNumberOfVertices(); i++) { if (!planarRegionsBoundingPolygon.isPointInside(footPolygonInWorld.getVertex(i))) { return true; } } return false; }
for (PlanarRegion regionA : regionsA.getPlanarRegionsAsList())
for (PlanarRegion regionA : regionsA.getPlanarRegionsAsList())
for (PlanarRegion regionA : regionsA.getPlanarRegionsAsList())