public GapPlanarRegionEnvironment(double platform1Length, double platform2Length, double platformWidth, double forwardGapSize, double sideGapSize) { generator.translate(0.0, 0.0, -0.01); generator.addCubeReferencedAtBottomMiddle(platform1Length, platformWidth, 0.01); // ground double platform2Center = 0.5 * (platform1Length + platform2Length) + forwardGapSize; generator.translate(0.5 * (platform1Length + platform2Length) + forwardGapSize, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(platform2Length, platformWidth, 0.01); // ground double sideWidth = 0.18; double sideLength = platform1Length + platform2Length + forwardGapSize; double distanceToCenter = 0.5 * sideLength - 0.5 * platform1Length; generator.translate(-platform2Center + distanceToCenter, 0.5 * platformWidth + sideGapSize + 0.5 * sideWidth, 0.0); generator.addCubeReferencedAtBottomMiddle(sideLength, sideWidth, 0.01); // ground addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public LittleWallsWithIncreasingHeightPlanarRegionEnvironment() { generator.translate(2.0, 0.0, -0.01); generator.addCubeReferencedAtBottomMiddle(6.0, 1.0, 0.01); generator.translate(-2.0, 0.0, 0.0); generator.translate(0.35, 0.2, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.1); generator.translate(0.62, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.14); generator.translate(0.3, -0.3, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.15); generator.translate(0.4, 0.1, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 1.0, 0.11); addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public static PlanarRegionsList generateFlatGround(double lengthX, double widthY) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addCubeReferencedAtBottomMiddle(lengthX, widthY, 0.001); PlanarRegionsList flatGround = generator.getPlanarRegionsList(); return flatGround; }
private static void setUpFlatBlock(PlanarRegionsListGenerator generator, double xInWorld, double yInWorld, double bottomSurfaceHeightLowestPoint, double blockHeight) { generator.translate(xInWorld, yInWorld, bottomSurfaceHeightLowestPoint); generator.addCubeReferencedAtBottomMiddle(cinderBlockLength, cinderBlockLength, blockHeight); generator.identity(); }
public static void generateSingleCiderBlock(PlanarRegionsListGenerator generator, double cinderBlockSize, double cinderBlockHeight, int angleType, int axisType) { double angle = 0; switch (angleType) { case 0: angle = 0.0; break; case 1: angle = Math.toRadians(15); break; case 2: angle = -Math.toRadians(15); break; } Axis axis = null; switch (axisType) { case 0: axis = Axis.X; break; case 1: axis = Axis.Y; break; } generator.rotate(angle, axis); generator.addCubeReferencedAtBottomMiddle(cinderBlockSize, cinderBlockSize, cinderBlockHeight); generator.rotate(-angle, axis); }
generator.addCubeReferencedAtBottomMiddle(cinderBlockSize, cinderBlockSize, cinderBlockHeight); generator.rotate(-angle, axis);
public static PlanarRegionsList generateBumpyGround(Random random, double maxX, double maxY, double maxZ) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); double length = 0.5; double width = 0.5; generator.translate(maxX/2.0 + length/2.0, maxY/2.0 - width/2.0, 0.0); generator.addCubeReferencedAtBottomMiddle(1.5 * maxX, 1.25 * maxY, 0.01); generator.identity(); int sizeX = (int) (maxX/length); int sizeY = (int) (maxY/width); for (int i=0; i<sizeY; i++) { generator.identity(); generator.translate(0.0, i * width, 0.0); for (int j=0; j<sizeX; j++) { generator.translate(length, 0.0, 0.0); double height = RandomNumbers.nextDouble(random, 0.01, maxZ); generator.addCubeReferencedAtBottomMiddle(length, width, height + random.nextDouble() * 0.1); } } PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); return planarRegionsList; }
generator.addCubeReferencedAtBottomMiddle(stepLength, 1.75, (i + 1) * stepHeight); generator.addCubeReferencedAtBottomMiddle(startingBlockLength, 1.75, numberOfSteps * stepHeight); generator.addCubeReferencedAtBottomMiddle(stepLength, 1.75, (numberOfSteps - i) * stepHeight);
public HoleTestData() { super(hole); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(0.0, 0.0, -0.5); generator.addRectangle(10.0, 10.0); generator.addCubeReferencedAtBottomMiddle(0.5, 0.5, 0.5); generator.translate(0.7, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(0.3, 0.5, 0.5); generator.translate(0.0, 0.5, 0.0); generator.addCubeReferencedAtBottomMiddle(1.9, 0.5, 0.5); generator.translate(0.0, -0.5, 0.0); generator.translate(0.7, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(0.5, 0.5, 0.5); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); // define start and goal conditions FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); initialStanceFootPose.setPosition(0.0, 0.15, 0.0); RobotSide initialStanceSide = RobotSide.LEFT; FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(1.3, 0.0, 0.0); setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(planarRegionsList); setStartSide(initialStanceSide); } }
generator.addCubeReferencedAtBottomMiddle(0.6, 1.0, 0.5); generator.translate(0.55, 0.0, 0.0); for (int i = 0; i < 10; i++) generator.addCubeReferencedAtBottomMiddle(0.1, 1.0, 0.5); generator.rotate(-rotationAngle, Axis.Y); double translation = i % 2 == 0 ? -0.05 : 0.45; generator.addCubeReferencedAtBottomMiddle(0.6, 1.0, 0.5);
public TwoBollardEnvironment(double barrelSeparation) { // ground plane generator.addRectangle(1.0, barrelSeparation + 0.5); generator.translate(2.0, 0.0, 0.0); generator.addRectangle(3.0, 3.0); generator.translate(-4.0, 0.0, 0.0); generator.addRectangle(3.0, 3.0); addPlanarRegionsToTerrain(YoAppearance.RGBColor(110 / 256.0, 121 / 256.0, 121 / 256.0)); // first bollard generator.identity(); generator.translate(0.0, 0.5 * barrelSeparation, 0.0); generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight); addPlanarRegionsToTerrain(YoAppearance.Yellow()); // second bollard generator.identity(); generator.translate(0.0, -0.5 * barrelSeparation, 0.0); generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight); addPlanarRegionsToTerrain(YoAppearance.Yellow()); }
private static PlanarRegionsList generateRandomTerrain(Random random) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(0.0, 0.0, 0.001); generator.addRectangle(14.0, 3.0); // floor plane double length = RandomNumbers.nextDouble(random, 0.3, 1.0); double width = RandomNumbers.nextDouble(random, 0.3, 1.0); double height = RandomNumbers.nextDouble(random, 0.07, 0.3); for (int i = 0; i < 100; i++) { generator.identity(); Vector3D translationVector = RandomGeometry.nextVector3D(random, -5.0, -1.0, -0.05, 5.0, 1.0, 0.0); generator.translate(translationVector); Quaternion rotation = RandomGeometry.nextQuaternion(random, Math.toRadians(15.0)); generator.rotate(rotation); generator.addCubeReferencedAtBottomMiddle(length, width, height); } PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); return planarRegionsList; }
public static PlanarRegionsList generateRandomObjects(Random random, int numberOfRandomObjects, double maxX, double maxY, double maxZ) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); double length = RandomNumbers.nextDouble(random, 0.2, 1.0); double width = RandomNumbers.nextDouble(random, 0.2, 1.0); double height = RandomNumbers.nextDouble(random, 0.2, 1.0); for (int i = 0; i < numberOfRandomObjects; i++) { generator.identity(); Vector3D translationVector = RandomGeometry.nextVector3D(random, -maxX, -maxY, 0.0, maxX, maxY, maxZ); generator.translate(translationVector); Quaternion rotation = RandomGeometry.nextQuaternion(random); generator.rotate(rotation); generator.addCubeReferencedAtBottomMiddle(length, width, height); } PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); return planarRegionsList; }
public CorridorTestData() { super(corridor); double corridorStartDistance = 0.5; double corridorWidth = 0.4; double corridorHeight = 2.0; double corridorLength = 0.25; double blockWidth = 2.0; PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(5.0, 0.0, 0.0); generator.addRectangle(20.0, 2.0 * blockWidth + corridorWidth); generator.identity(); generator.translate(corridorStartDistance + corridorLength / 2.0, (blockWidth + corridorWidth) / 2.0, 0.0); generator.addCubeReferencedAtBottomMiddle(corridorLength, blockWidth, corridorHeight); generator.translate(0.0, -blockWidth - corridorWidth, 0.0); generator.addCubeReferencedAtBottomMiddle(corridorLength, blockWidth, corridorHeight); PlanarRegionsList regions = generator.getPlanarRegionsList(); FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); RobotSide initialStanceSide = RobotSide.LEFT; initialStanceFootPose.setY(initialStanceSide.negateIfRightSide(0.15)); initialStanceFootPose.setX(-2.0); FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(corridorStartDistance + corridorLength + 1.0, 0.0, 0.0); setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(regions); setStartSide(initialStanceSide); } }
public static PlanarRegionsList generateStairCase(Vector3D translationVector, Vector3D rotationVector) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(translationVector); int numberOfSteps = 5; double length = 0.4; double width = 0.8; double height = 0.1; generator.translate(length * numberOfSteps / 2.0, 0.0, 0.001); generator.addRectangle(1.2 * length * numberOfSteps, 1.2 * width); generator.identity(); generator.translate(translationVector); generator.translate(length, 0.0, 0.0); generator.rotateEuler(rotationVector); for (int i = 0; i < numberOfSteps; i++) { generator.addCubeReferencedAtBottomMiddle(length, width, height); generator.translate(length, 0.0, 0.0); height = height + 0.1; } PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); return planarRegionsList; }
public BoxTestData() { super(box); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(2.0, 0.0, 0.0001); generator.addRectangle(5.0, 5.0); generator.addCubeReferencedAtBottomMiddle(1.0, 1.0, 1.0); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); // define start and goal conditions FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); initialStanceFootPose.setPosition(0.0, 0.15, 0.0); RobotSide initialStanceSide = RobotSide.LEFT; FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(4.0, 0.0, 0.0); setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(planarRegionsList); setStartSide(initialStanceSide); } }
public SimpleStepOnBoxTestData() { super(simpleStepOnBox); // create planar regions double stepHeight = 0.2; double boxSize = 1.0; PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(1.0 + boxSize / 2.0, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(boxSize, boxSize, stepHeight); generator.translate(0.0, 0.0, 0.001); generator.addRectangle(5.0, 5.0); // floor plane // define start and goal conditions FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); RobotSide initialStanceSide = RobotSide.LEFT; FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(1.0 + boxSize / 2.0, 0.0, stepHeight); // run the test PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(planarRegionsList); setStartSide(initialStanceSide); } }
generator.addCubeReferencedAtBottomMiddle(1.0, 1.0, 0.001); generator.translate(-0.15, 0.0, 0.0001); generator.translate(0.0, 0.0, 0.25); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.1);
generator.setTransform(planarRegionTransform); generator.addCubeReferencedAtBottomMiddle(1.0, 0.5, 0.7); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList();
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleVerticalSnap() { boolean visualize = false; ConvexPolygon2D polygonToSnap = PlanarRegionsListExamples.createRectanglePolygon(0.5, 0.25); RigidBodyTransform nonSnappedTransform = new RigidBodyTransform(); PolygonSnapperVisualizer polygonSnapperVisualizer = null; if (visualize) { polygonSnapperVisualizer = new PolygonSnapperVisualizer(polygonToSnap); } PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addCubeReferencedAtBottomMiddle(1.0, 0.5, 0.7); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); PlanarRegion planarRegion = new PlanarRegion(); RigidBodyTransform snapTransform = PlanarRegionsListPolygonSnapper.snapPolygonToPlanarRegionsList(polygonToSnap, planarRegionsList, planarRegion); if (polygonSnapperVisualizer != null) { polygonSnapperVisualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.Gray()); polygonSnapperVisualizer.setSnappedPolygon(nonSnappedTransform, snapTransform); } RigidBodyTransform expectedTransform = new RigidBodyTransform(); expectedTransform.setTranslation(0.0, 0.0, 0.7); assertTrue(expectedTransform.epsilonEquals(snapTransform, 1e-7)); if (visualize) { ThreadTools.sleepForever(); } }