public PlanarRegionEnvironmentInterface() { hasBeenGenerated = false; generator = new PlanarRegionsListGenerator(); }
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 PlanarRegionsList createFlatGround(Point3D startPoseToPack, Point3D goalPoseToPack) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(50.0, 5.0); startPoseToPack.set(-18.005, -2.001, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), startPoseToPack, startPoseToPack); goalPoseToPack.set(18.005, 2.001, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), goalPoseToPack, goalPoseToPack); return generator.getPlanarRegionsList(); }
@Before public void setUp() { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(CINDER_BLOCK_START_X, CINDER_BLOCK_START_Y, 0.001); PlanarRegionsListExamples.generateCinderBlockField(generator, CINDER_BLOCK_SIZE, CINDER_BLOCK_HEIGHT, CINDER_BLOCK_COURSE_WIDTH_X_IN_NUMBER_OF_BLOCKS, CINDER_BLOCK_COURSE_LENGTH_Y_IN_NUMBER_OF_BLOCKS, CINDER_BLOCK_HEIGHT_VARIATION, - 0.03, 0.6); cinderBlockField = generator.getPlanarRegionsList(); SimulationTestingParameters parameters = SimulationTestingParameters.createFromSystemProperties(); simulationTestHelper = new DRCSimulationTestHelper(parameters, getRobotModel(), createCommonAvatarInterface(cinderBlockField)); }
private static PlanarRegionsList simpleOccludedEnvironment(boolean includeGoalPlane) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(2.0, 4.0); generator.translate(1.0, -1.0, 0.5); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(0.9, 1.9); if (includeGoalPlane) { generator.identity(); generator.translate(2.0, -1.0, 0.0); generator.addRectangle(1.0, 1.0); } return generator.getPlanarRegionsList(); }
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; }
public static PlanarRegionsList generateSteppingStoneField(double steppingStoneWidth, double steppingStoneLength, double stepWidth, double stepLength, int numberOfSteps) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); double platformLength = 0.6; double platformWidth = 1.0; generator.addRectangle(platformLength, platformWidth); generator.translate(0.5 * platformLength, 0.0, 0.0); for (int i = 0; i < numberOfSteps; i++) { RobotSide side = (i % 2 == 0) ? RobotSide.LEFT : RobotSide.RIGHT; double xOffset = stepLength; double yOffset = side.negateIfRightSide(0.5 * stepWidth); generator.translate(xOffset, yOffset, 0.0); generator.addRectangle(steppingStoneLength, steppingStoneWidth); } generator.translate(stepLength + 0.5 * platformLength, 0.0, 0.0); generator.addRectangle(platformLength, platformWidth); return generator.getPlanarRegionsList(); }
private PlanarRegionsList createSimpleOcclusionField(FramePose3D startPoseToPack, FramePose3D goalPoseToPack) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.rotate(Math.toRadians(10.0), Axis.X); generator.addRectangle(6.0, 6.0); generator.translate(-1.0, -1.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 4.0); generator.identity(); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(1.0, 1.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 4.0); startPoseToPack.setToZero(ReferenceFrame.getWorldFrame()); startPoseToPack.setOrientationYawPitchRoll(Math.PI / 2.0, 0.0, 0.0); startPoseToPack.setPosition(-2.0, -2.0, 0.0); startPoseToPack.prependRollRotation(Math.toRadians(10.0)); goalPoseToPack.setToZero(ReferenceFrame.getWorldFrame()); goalPoseToPack.setOrientationYawPitchRoll(Math.PI / 2.0, 0.0, 0.0); goalPoseToPack.setPosition(2.0, 2.0, 0.0); goalPoseToPack.prependRollRotation(Math.toRadians(10.0)); return generator.getPlanarRegionsList(); }
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; }
@Test(timeout = TIMEOUT) @ContinuousIntegrationTest(estimatedDuration = 10.0) public void testFlatGroundWithWall() { Point3D startPose = new Point3D(-4.805, 0.001, 0.0); Point3D goalPose = new Point3D(4.805, 0.001, 0.0); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(10.0, 5.0); generator.translate(0.0, 0.0, 1.0); generator.rotate(Math.PI / 2.0, Axis.Y); generator.rotate(Math.PI / 2.0, Axis.Z); generator.addRectangle(3.0, 2.0); PlanarRegionsList regions = generator.getPlanarRegionsList(); runTest(startPose, goalPose, regions, OcclusionMethod.OCCLUSION, defaultMaxAllowedSolveTime); }
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; }
private PlanarRegionsList createSimpleOcclusionField(Point3D startPoseToPack, Point3D goalPoseToPack) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); // generator.rotate(Math.toRadians(10.0), Axis.X); generator.addRectangle(6.0, 6.0); generator.translate(-1.0, -1.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 4.0); generator.identity(); // generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(1.0, 1.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 4.0); startPoseToPack.set(-2.0, -2.0, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), startPoseToPack, startPoseToPack); goalPoseToPack.set(2.0, 2.0, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), goalPoseToPack, goalPoseToPack); return generator.getPlanarRegionsList(); }
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 OverCinderBlockFieldTestData() { super(overCinderBlockField); double startX = 0.0; double startY = 0.0; double cinderBlockHeight = 0.15; double cinderBlockSize = 0.4; int courseWidthXInNumberOfBlocks = 21; int courseLengthYInNumberOfBlocks = 6; double heightVariation = 0.1; PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(startX, startY, -0.001); PlanarRegionsListExamples .generateCinderBlockField(generator, cinderBlockSize, cinderBlockHeight, courseWidthXInNumberOfBlocks, courseLengthYInNumberOfBlocks, heightVariation); PlanarRegionsList cinderBlockField = generator.getPlanarRegionsList(); FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(9.0, 0.0, 0.0); FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); initialStanceFootPose.setPosition(0.0, -0.7, 0.0); RobotSide initialStanceSide = RobotSide.RIGHT; setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(cinderBlockField); 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); } }
planarRegionTransform.setRotationEulerAndZeroTranslation(0.1, 0.2, 0.3); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.setTransform(planarRegionTransform);
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); } }
public WallTestData() { super(wall); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(5.0, 5.0); generator.translate(-0.5, 0.5, 0.5); generator.rotate(Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 1.5); generator.identity(); generator.translate(0.5, -0.5, 0.5); generator.rotate(Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 1.5); PlanarRegionsList regions = generator.getPlanarRegionsList(); FootstepPlannerParameters plannerParameters = new DefaultFootstepPlanningParameters(); // define start and goal conditions FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); RobotSide initialStanceSide = RobotSide.LEFT; initialStanceFootPose.setY(initialStanceSide.negateIfRightSide(plannerParameters.getIdealFootstepWidth() / 2.0)); initialStanceFootPose.setX(-2.0); FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(2.0, 0.0, 0.0); setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(regions); setStartSide(initialStanceSide); } }
PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator();