/** * @return a full depth copy of this list of planar regions. The copy can be entirely modified * without interfering with this. */ public PlanarRegionsList copy() { List<PlanarRegion> planarRegionsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfPlanarRegions(); i++) planarRegionsCopy.add(regions.get(i).copy()); return new PlanarRegionsList(planarRegionsCopy); }
public static PlanarRegionsList convertToPlanarRegionsList(PlanarRegionsListMessage planarRegionsListMessage) { List<PlanarRegion> planarRegions = new ArrayList<>(); List<PlanarRegionMessage> planarRegionMessages = planarRegionsListMessage.getPlanarRegions(); for (int regionIndex = 0; regionIndex < planarRegionMessages.size(); regionIndex++) planarRegions.add(convertToPlanarRegion(planarRegionMessages.get(regionIndex))); return new PlanarRegionsList(planarRegions); } }
/** * @return a full depth copy of this list of planar regions. The copy can be entirely modified without interfering with this. */ public PlanarRegionsList copy() { List<PlanarRegion> planarRegionsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfPlanarRegions(); i++) planarRegionsCopy.add(regions.get(i).copy()); return new PlanarRegionsList(planarRegionsCopy); } }
public void exportPlanarRegionData(PlanarRegion planarRegion) { planarRegionsState.set(PlanarRegionMessageConverter.convertToPlanarRegionsListMessage(new PlanarRegionsList(planarRegion))); exportPlanarRegionData(true); }
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; }
public static PlanarRegionsList createPlanarRegionsList(List<PlanarRegionSegmentationRawData> rawData, ConcaveHullFactoryParameters concaveHullFactoryParameters, PolygonizerParameters polygonizerParameters, PlanarRegionSegmentationDataExporter dataExporter) { return new PlanarRegionsList(createPlanarRegions(rawData, concaveHullFactoryParameters, polygonizerParameters, dataExporter)); }
public static PlanarRegionsList generatePlanarRegionsListFromRandomPolygonsWithRandomTransform(Random random, int numberOfRandomlyGeneratedPolygons, double maxAbsoluteXYForPolygons, int numberOfPossiblePointsForPolygons, int numberOfPossiblePlanarRegions) { PlanarRegionsList planarRegionsList = new PlanarRegionsList(); int numberOfPlanarRegions = random.nextInt(numberOfPossiblePlanarRegions) + 1; while (planarRegionsList.getNumberOfPlanarRegions() < numberOfPlanarRegions) { planarRegionsList.addPlanarRegion(PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, numberOfRandomlyGeneratedPolygons, maxAbsoluteXYForPolygons, numberOfPossiblePointsForPolygons)); } return planarRegionsList; } }
private static PlanarRegionsList createRandomPlanarRegionList(Random random) { PlanarRegionsList planarRegionsList = new PlanarRegionsList(); for (int i = 0; i < RandomNumbers.nextInt(random, 1, 50); i++) planarRegionsList.addPlanarRegion(createRandomPlanarRegion(random, i)); return planarRegionsList; }
return new PlanarRegionsList(planarRegions);
return new PlanarRegionsList(planarRegions);
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testFootstepCacheing() { TestSnapper testSnapper = new TestSnapper(); PlanarRegionsList planarRegionsList = new PlanarRegionsList(new PlanarRegion()); testSnapper.setPlanarRegions(planarRegionsList); for (int i = 0; i < xIndices.length; i++) { for (int j = 0; j < yIndices.length; j++) { for (int k = 0; k < yawIndices.length; k++) { RobotSide robotSide = RobotSide.generateRandomRobotSide(random); testSnapper.snapFootstepNode(new FootstepNode(xIndices[i], yIndices[j], yawIndices[k], robotSide)); assertTrue(testSnapper.dirtyBit); testSnapper.dirtyBit = false; testSnapper.snapFootstepNode(new FootstepNode(xIndices[i], yIndices[j], yawIndices[k], robotSide)); assertTrue(!testSnapper.dirtyBit); } } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testStartNodeValid() { FootstepNodeSnapper snapper = new TestSnapper(); FootstepPlannerParameters parameters = new DefaultFootstepPlanningParameters(); SnapBasedNodeChecker checker = new SnapBasedNodeChecker(parameters, footPolygons, snapper); // the start node is valid even if it can not be snapped or the parent is null. checker.setPlanarRegions(new PlanarRegionsList()); FootstepNode node = new FootstepNode(0.0, 0.0, 0.0, RobotSide.LEFT); checker.addStartNode(node, new RigidBodyTransform()); Assert.assertTrue(checker.isNodeValid(node, null)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimplePartialFoothold() { FootstepNode nodeToSnap = new FootstepNode(1.0, 0.0); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setRotation(new AxisAngle(0.0, 1.0, 0.0, 0.25 * Math.PI)); transformToWorld.setTranslationZ(-1.0); ConvexPolygon2D partialFootholdPolygon = new ConvexPolygon2D(footPolygons.get(RobotSide.LEFT)); partialFootholdPolygon.scale(0.5); partialFootholdPolygon.translate(Math.sqrt(2.0), 0.0); PlanarRegion planarRegion = new PlanarRegion(transformToWorld, partialFootholdPolygon); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); snapper.setPlanarRegions(planarRegionsList); FootstepNodeSnapData snapData = snapper.snapFootstepNode(nodeToSnap); if(visualize) { visualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.AliceBlue()); visualizer.setSnappedPolygon(nodeTransform, snapData.getSnapTransform(), snapData.getCroppedFoothold()); ThreadTools.sleepForever(); } assertEquals(snapData.getCroppedFoothold().getArea(), partialFootholdPolygon.getArea(), epsilon); PlanarRegionPolygonSnapperTest.assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(snapData.getSnapTransform(), transformToWorld); }
private void doAFullFootholdTest(RigidBodyTransform regionToWorldFrameTransform, FootstepNode nodeToSnap) { RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); ConvexPolygon2D footholdPolygon = new ConvexPolygon2D(unitSquare); footholdPolygon.applyTransform(nodeTransform, false); PlanarRegion planarRegion = createPlanarRegion(regionToWorldFrameTransform, footholdPolygon); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); snapper.setPlanarRegions(planarRegionsList); FootstepNodeSnapData snapData = snapper.snapFootstepNode(nodeToSnap); if(visualize) { RigidBodyTransform snappedNodeTransform = new RigidBodyTransform(); FootstepNodeTools.getSnappedNodeTransform(nodeToSnap, snapData.getSnapTransform(), snappedNodeTransform); visualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.AliceBlue()); visualizer.setSnappedPolygon(nodeTransform, snapData.getSnapTransform()); ThreadTools.sleepForever(); } assertEquals(snapData.getCroppedFoothold().getArea(), footPolygons.get(RobotSide.LEFT).getArea(), epsilon); PlanarRegionPolygonSnapperTest.assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(snapData.getSnapTransform(), regionToWorldFrameTransform); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testStartNodeInAVoid() { ConvexPolygon2D groundPlane = new ConvexPolygon2D(); groundPlane.addVertex(-1.0, -1.0); groundPlane.addVertex(-1.0, 1.0); groundPlane.addVertex(1.0, -1.0); groundPlane.addVertex(1.0, 1.1); groundPlane.update(); PlanarRegion planarRegion = new PlanarRegion(new RigidBodyTransform(), groundPlane); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); Point2D goalPosition = new Point2D(0.5, 0.0); FramePose2D goalPose = new FramePose2D(ReferenceFrame.getWorldFrame(), goalPosition, 0.0); FramePose2D initialStanceFootPose = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(-1.2, 0.0), 0.0); RobotSide initialStanceFootSide = RobotSide.LEFT; FramePose3D initialStanceFootPose3d = FlatGroundPlanningUtils.poseFormPose2d(initialStanceFootPose); FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(planner, initialStanceFootPose3d, initialStanceFootSide, goalPose3d, planarRegionsList, !visualize); if (visualize) PlanningTestTools.visualizeAndSleep(planarRegionsList, footstepPlan, goalPose3d); } }
private PlanarRegionsList combine(PlanarRegionsList regionsA, PlanarRegionsList regionsB) PlanarRegionsList ret = new PlanarRegionsList();
private PlanarRegionsList combine(PlanarRegionsList regionsA, PlanarRegionsList regionsB) PlanarRegionsList ret = new PlanarRegionsList();
PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion);
private PlanarRegionsList combine(PlanarRegionsList regionsA, PlanarRegionsList regionsB) PlanarRegionsList ret = new PlanarRegionsList();
public void testConstructingGroundPlaneAroundFeet() PlanarRegionsList planarRegionsList = new PlanarRegionsList(); Vector3D translation = new Vector3D(2.1, -0.3, 0.4); RigidBodyTransform transformToWorld = new RigidBodyTransform(new Quaternion(), translation);