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 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()); } }
private PlanarRegionsList createMazeOcclusionField(Point3D startPoseToPack, Point3D goalPoseToPack) generator.rotate(Math.toRadians(10.0), Axis.X); generator.addRectangle(6.0, 12.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(-1.0, -2.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 8.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(1.0, 0.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 8.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(0.0, -4.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.X); generator.addRectangle(2.0, 1.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(0.0, 4.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.X); generator.addRectangle(2.0, 1.0);
private PlanarRegionsList createMazeOcclusionField(FramePose3D startPoseToPack, FramePose3D goalPoseToPack) generator.rotate(Math.toRadians(10.0), Axis.X); generator.addRectangle(6.0, 12.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(-1.0, -2.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 8.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(1.0, 0.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 8.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(0.0, -4.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.X); generator.addRectangle(2.0, 1.0); generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(0.0, 4.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.X); generator.addRectangle(2.0, 1.0);
public ZigZagSlopeEnvironment(double rampSlope, double rampLength, int numberOfRamps, double heightAtRampMiddle) { double rampLengthX = rampLength * Math.cos(rampSlope); double startX = -0.5 * rampLengthX * (numberOfRamps - 1); double slopeSign = -1.0; for (int i = 0; i < numberOfRamps; i++) { generator.identity(); generator.translate(startX + i * rampLengthX, 0.0, heightAtRampMiddle); generator.rotate(rampSlope * slopeSign, Axis.Y); generator.addRectangle(rampLength, 1.0); slopeSign *= -1.0; } addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
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(); }
@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); }
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(); }
generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(2.0 + extrusionDistance, 6.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(2.0 + extrusionDistance, 4.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(2.0 + extrusionDistance, 4.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(2.0 + extrusionDistance, 4.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(2.0 + extrusionDistance, 2.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.X); generator.addRectangle(4.0 + extrusionDistance, 2.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.X); generator.addRectangle(2.0 + extrusionDistance, 2.0 + extrusionDistance); generator.identity(); generator.rotate(0.5 * Math.PI, Axis.X);
generator.rotate(- angle, Axis.Z);
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 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()); } }
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; }
break; generator.rotate(quaternion);
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.rotate(0.25 * Math.PI, Axis.X); planarRegionsListGenerator.addRectangle(2.0, 2.0); PlanarRegionsList planarRegionsList = planarRegionsListGenerator.getPlanarRegionsList();
double boxSize = 1.0; PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.rotate(Math.PI / 4.0, Axis.Z); generator.translate(-6.0 + 1.0 + boxSize / 2.0, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(boxSize, boxSize, stepHeight);