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(); }
generator.translate(3.25, -2.4, -0.01); generator.addRectangle(9.5, 11.25); generator.translate(-3.25, 2.4, 0.0); generator.translate(3.25, 6.45, 0.0); generator.addRectangle(9.5, 3.1); generator.translate(-3.25, -6.45, 0.0); generator.translate(-1.0, 4.05, 0.0); generator.addRectangle(1.0, 1.7); generator.translate(1.0, -4.05, 0.0); generator.translate(4.25, 4.05, 0.0); generator.addRectangle(7.5, 1.7); generator.translate(-4.25, -4.05, 0.0); generator.translate(-4.75, -4.6, 0.0); generator.addRectangle(6.5, 6.8); generator.translate(4.75, 4.6, 0.0); generator.translate(-4.75, 4.6, 0.0); generator.addRectangle(6.5, 6.8); generator.translate(4.75, -4.6, 0.0); generator.translate(-6.7, 0.0, 0.0); generator.addRectangle(2.6, 2.4); generator.translate(6.7, 0.0, 0.0); addPlanarRegionsToTerrain(YoAppearance.RGBColor(110 / 256.0, 121 / 256.0, 121 / 256.0));
public void addRectangle(double lengthX, double widthY) { ConvexPolygon2D rectangle = createRectanglePolygon(lengthX, widthY); addPolygon(rectangle); }
public static PlanarRegionsList generateFlatGround(double lengthX, double widthY) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addCubeReferencedAtBottomMiddle(lengthX, widthY, 0.001); PlanarRegionsList flatGround = generator.getPlanarRegionsList(); return flatGround; }
public void addCubeReferencedAtBottomMiddle(double lengthX, double widthY, double heightZ) { translate(0.0, 0.0, heightZ / 2.0); addCubeReferencedAtCenter(lengthX, widthY, heightZ); translate(0.0, 0.0, -heightZ / 2.0); }
public SingleStepEnvironment(double stepUpHeight, double stepLength) { // first ground plane generator.identity(); generator.addRectangle(2.0, 2.0); // step generator.translate(1.0 + 0.5 * stepLength, 0.0, stepUpHeight); generator.addRectangle(stepLength, 2.0); generator.identity(); generator.translate(1.0, 0.0, 0.5 * stepUpHeight); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(stepUpHeight, 2.0); generator.identity(); generator.translate(1.0 + stepLength, 0.0, 0.5 * stepUpHeight); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(stepUpHeight, 2.0); // second ground plane generator.identity(); generator.translate(2.0 + stepLength, 0.0, 0.0); generator.addRectangle(2.0, 2.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public StaircaseEnvironment(int numberOfSteps, double stepHeight, double stepLength, boolean includeDown) generator.identity(); generator.addRectangle(startingBlockLength, 1.75); generator.translate(0.5 * (startingBlockLength - stepLength), 0.0, 0.0); generator.translate(stepLength, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(stepLength, 1.75, (i + 1) * stepHeight); generator.translate(0.5 * (startingBlockLength + stepLength), 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(startingBlockLength, 1.75, numberOfSteps * stepHeight); generator.translate(0.5 * (startingBlockLength - stepLength), 0.0, 0.0); generator.translate(stepLength, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(stepLength, 1.75, (numberOfSteps - i) * stepHeight); generator.translate(0.5 * (startingBlockLength + stepLength), 0.0, 0.0); generator.addRectangle(startingBlockLength, 1.75);
@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)); }
generator.addRectangle(startingBlockLength + extrusionLength, courseWidth + extrusionLength); // standing platform generator.translate(0.2 + 0.5 * startingBlockLength, 0.0, 0.0); // forward to first row generator.translate(0.0, -0.5 * (courseLengthYInNumberOfBlocks - 1) * cinderBlockSize, 0.0); // over to grid origin generator.translate(0.0, cinderBlockSize, 0.0); generator.translate(0.0, 0.0, heightVariation); generator.translate(0.0, 0.0, - heightVariation); generator.translate(cinderBlockSize, -cinderBlockSize * courseLengthYInNumberOfBlocks, 0.0); generator.identity(); generator.translate(startingBlockLength + courseWidthXInNumberOfBlocks * cinderBlockSize, 0.0, 0.001); generator.addRectangle(startingBlockLength + extrusionLength, courseWidth + extrusionLength);
public void addRamp(double length, double deltaZ) { checkHasNotBeenGenerated(); generator.translate(length / 2.0, 0.0, deltaZ / 2.0); generator.rotate(-Math.atan2(deltaZ, length), Axis.Y); generator.addRectangle(Math.sqrt(MathTools.square(length) + MathTools.square(deltaZ)), rampWidth); generator.rotate(Math.atan2(deltaZ, length), Axis.Y); generator.translate(length / 2.0, 0.0, deltaZ / 2.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(0.0, 0.0, 0.0001); generator.addPolygon(circlePolygon); for (ConvexPolygon2D step : steps) generator.addPolygon(step); generator.translate(0.0, 0.0, stepHeight); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList();
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(); }
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(); }
generator.translate(xInWorld, yInWorld, surfaceHeightLowestPoint + zHeightToCenter); break; generator.rotate(quaternion); generator.addCubeReferencedAtBottomMiddle(cinderBlockLength, cinderBlockLength, cinderBlockHeight); break; generator.identity();
public AlternatingSlopesEnvironment(double rampWidth, double landingLength) { super(); this.rampWidth = rampWidth; generator.addRectangle(Math.sqrt(MathTools.square(landingLength)), rampWidth); generator.translate(landingLength / 2.0, 0.0, 0.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); }
planarRegionTransform.setRotationEulerAndZeroTranslation(0.1, 0.2, 0.3); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.setTransform(planarRegionTransform); generator.addCubeReferencedAtBottomMiddle(1.0, 0.5, 0.7); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList();
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 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); }
public void addRampReferencedAtBottomMiddle(double lengthX, double widthY, double heightZ) { RigidBodyTransformGenerator transformGeneratorTwo = new RigidBodyTransformGenerator(transformGenerator); double slope = Math.atan2(heightZ, lengthX); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(lengthX / 2.0, 0.0, heightZ / 2.0); transformGeneratorTwo.rotate(-slope, Axis.Y); addRectangle(transformGeneratorTwo, EuclidGeometryTools.pythagorasGetHypotenuse(lengthX, heightZ), widthY); ConvexPolygon2D leftSide = new ConvexPolygon2D(); leftSide.addVertex(0.0, 0.0); leftSide.addVertex(- lengthX, 0.0); leftSide.addVertex(- lengthX, heightZ); leftSide.update(); ConvexPolygon2D rightSide = new ConvexPolygon2D(); rightSide.addVertex(0.0, 0.0); rightSide.addVertex(lengthX, 0.0); rightSide.addVertex(lengthX, heightZ); rightSide.update(); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(0.0, 0.5 * widthY, 0.0); transformGeneratorTwo.rotate(0.5 * Math.PI, Axis.X); transformGeneratorTwo.rotate(Math.PI, Axis.Y); addPolygon(transformGeneratorTwo, leftSide); transformGeneratorTwo.set(transformGenerator); transformGeneratorTwo.translate(0.0, -0.5 * widthY, 0.0); transformGeneratorTwo.rotate(0.5 * Math.PI, Axis.X); addPolygon(transformGeneratorTwo, rightSide); }
public VaryingStairsPlanarRegionEnvironment(double startX, double startZ, double[] stepTreads, double[] stepRises) { generator.translate(startX, 0.0, startZ); for (int i = 0; i < stepTreads.length; i++) { generator.addCubeReferencedAtBottomNegativeXEdgeCenter(stepTreads[i], 1.0, -0.1); generator.translate(stepTreads[i] + 0.01, 0.0, stepRises[i]); } addPlanarRegionsToTerrain(YoAppearance.Grey()); } }