private void randomizeIDs(PlanarRegionsList planarRegionsList) { for (PlanarRegion region : planarRegionsList.getPlanarRegionsAsList()) region.setRegionId(random.nextInt()); messager.submitMessage(dataTopic, planarRegionsList); }
/** * @return a full depth copy of this region. The copy can be entirely modified without interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); List<Point2d[]> concaveHullsCopy = new ArrayList<>(); for (int hullIndex = 0; hullIndex < concaveHullsVertices.size(); hullIndex++) { Point2d[] hullVertices = new Point2d[concaveHullsVertices.get(hullIndex).length]; for (int i = 0; i < hullVertices.length; i++) hullVertices[i] = new Point2d(concaveHullsVertices.get(hullIndex)[i]); concaveHullsCopy.add(hullVertices); } List<ConvexPolygon2d> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2d(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullsCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
public void addPolygon(ConvexPolygon2d polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void addPolygons(ArrayList<ConvexPolygon2d> polygons) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void addPolygon(RigidBodyTransformGenerator transformGenerator, ConvexPolygon2D polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void addPolygons(ArrayList<ConvexPolygon2D> polygons) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
private static PlanarRegion nextPlanarRegion(Random random) { RigidBodyTransform transformToWorld = nextRegionTransform(random); Point2D[] concaveHullVertices = nextPoint2DArray(random); List<ConvexPolygon2D> convexPolygons = nextConvexPolygon2Ds(random); PlanarRegion next = new PlanarRegion(transformToWorld, concaveHullVertices, convexPolygons); next.setRegionId(random.nextInt()); return next; }
public void addPolygon(ConvexPolygon2D polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
planarRegion.setRegionId(idStart + i); planarRegions.add(planarRegion); platform.setRegionId(idStart + locations.size() - 2);
/** * @return a full depth copy of this region. The copy can be entirely modified without * interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); Point2D[] concaveHullCopy = new Point2D[concaveHullsVertices.length]; for (int i = 0; i < concaveHullsVertices.length; i++) concaveHullCopy[i] = new Point2D(concaveHullsVertices[i]); List<ConvexPolygon2D> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2D(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
private static PlanarRegion createRandomPlanarRegion(Random random, int idNumber) { int numberOfVertices = RandomNumbers.nextInt(random, 3, 50); int numberOfPolygons = RandomNumbers.nextInt(random, 1, 5); List<ConvexPolygon2D> polygons = new ArrayList<>(); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); for (int i = 0; i < numberOfPolygons; i++) { polygons.add(EuclidGeometryRandomTools.nextConvexPolygon2D(random, 10.0, numberOfVertices)); } PlanarRegion planarRegion = new PlanarRegion(transform, polygons); planarRegion.setRegionId(idNumber); return planarRegion; }
.forEach(regionsList -> regionsList.forEach(region -> region.setRegionId(random.nextInt())));
planarRegion.setRegionId(regionId); return planarRegion;
private void addRectangle(RigidBodyTransformGenerator transformGenerator, double lengthX, double widthY) { ConvexPolygon2D rectangle = createRectanglePolygon(lengthX, widthY); PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), rectangle); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
private void addRectangle(RigidBodyTransformGenerator transformGenerator, double lengthX, double widthY) { ConvexPolygon2d rectangle = createRectanglePolygon(lengthX, widthY); PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), rectangle); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public static PlanarRegion convertToPlanarRegion(PlanarRegionMessage planarRegionMessage) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); Vector3d regionOrigin = new Vector3d(planarRegionMessage.getRegionOrigin()); Vector3d regionNormal = new Vector3d(planarRegionMessage.getRegionNormal()); AxisAngle4d regionOrientation = GeometryTools.getAxisAngleFromZUpToVector(regionNormal); transformToWorld.set(regionOrientation, regionOrigin); List<Point2f[]> messageHullsVertices = planarRegionMessage.getConcaveHullsVertices(); List<Point2d[]> concaveHullsVertices = new ArrayList<>(); for (int hullIndex = 0; hullIndex < messageHullsVertices.size(); hullIndex++) { Point2f[] messageHullVertices = messageHullsVertices.get(hullIndex); Point2d[] hullVertices = new Point2d[messageHullVertices.length]; for (int vertexIndex = 0; vertexIndex < messageHullVertices.length; vertexIndex++) { hullVertices[vertexIndex] = new Point2d(messageHullVertices[vertexIndex]); } concaveHullsVertices.add(hullVertices); } List<ConvexPolygon2d> planarRegionConvexPolygons = new ArrayList<>(); List<Point2f[]> convexPolygonsVertices = planarRegionMessage.getConvexPolygonsVertices(); for (int polygonIndex = 0; polygonIndex < convexPolygonsVertices.size(); polygonIndex++) { ConvexPolygon2d convexPolygon = new ConvexPolygon2d(convexPolygonsVertices.get(polygonIndex)); planarRegionConvexPolygons.add(convexPolygon); } PlanarRegion planarRegion = new PlanarRegion(transformToWorld, concaveHullsVertices, planarRegionConvexPolygons); planarRegion.setRegionId(planarRegionMessage.getRegionId()); return planarRegion; }
planarRegion.setRegionId(rawData.getRegionId()); planarRegions.add(planarRegion);
originalRegion.getTransformToWorld(transform); PlanarRegion newRegion = new PlanarRegion(transform, polygon); newRegion.setRegionId(regions.getPlanarRegion(i).getRegionId()); visible.addPlanarRegion(newRegion);
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(); } }
combinedRegion.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(combinedRegion); foundMatchingRegion = true; region.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(region);