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()); }
private static LinkGraphicsDescription createArmGraphic(double length, double radius, AppearanceDefinition appearance) { LinkGraphicsDescription graphics = new LinkGraphicsDescription(); graphics.addSphere(1.2 * radius, YoAppearance.Grey()); graphics.addCylinder(length, radius, appearance); return graphics; }
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()); } }
private static LinkGraphicsDescription createHandGraphics() { LinkGraphicsDescription graphics = new LinkGraphicsDescription(); graphics.addSphere(0.025, YoAppearance.Grey()); graphics.translate(0.0, 0.0, 0.05); graphics.addEllipsoid(0.04, 0.01, 0.1); return graphics; }
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()); } }
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()); } }
public TestingEnvironment() { terrain = new CombinedTerrainObject3D(getClass().getSimpleName()); terrain.addBox(-0.2, -0.225, 3.2, 0.225, -0.1, 0.0); terrain.addBox(0.15, 0.05, 0.45, 0.25, 0.15); terrain.addBox(0.6, -0.05, 0.775, -0.25, 0.08); terrain.addCylinder(new RigidBodyTransform(new Quaternion(0.0, 0.0, 0.0, 1.0), new Point3D(1.5, 0.15, 0.1)), 0.2, 0.15, YoAppearance.Grey()); terrain.addCylinder(new RigidBodyTransform(new Quaternion(0.0, 0.0, 0.0, 1.0), new Point3D(1.8, -0.15, 0.1)), 0.2, 0.025, YoAppearance.Grey()); terrain.addBox(1.96, 0.125, 1.99, 0.0, 0.2); terrain.addBox(2.235, 0.175, 2.265, 0.25, 0.2); }
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 VaryingHeightTiledGroundEnvironment(double tileWidth, int courseLengthInNumberOfTiles, int courseWidthInNumberOfTiles, double nominalGroundHeight, double maxHeightVariation) { generator.identity(); double startCoordinateX = - 0.5 * tileWidth * (courseLengthInNumberOfTiles - 1); double startCoordinateY = - 0.5 * tileWidth * (courseWidthInNumberOfTiles- 1); for (int i = 0; i < courseLengthInNumberOfTiles; i++) { for (int j = 0; j < courseWidthInNumberOfTiles; j++) { double tilePositionX = startCoordinateX + tileWidth * i; double tilePositionY = startCoordinateY + tileWidth * j; double tilePositionZ = nominalGroundHeight + EuclidCoreRandomTools.nextDouble(random, 0.5 * maxHeightVariation); generator.translate(tilePositionX, tilePositionY, tilePositionZ); generator.addRectangle(tileWidth, tileWidth); generator.identity(); } } 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()); } }
private void createHandleGraphics() { // graphics - handle hinge RotationMatrix rotX90 = new RotationMatrix(); rotX90.setToRollMatrix(Math.PI / 2.0); doorHandleGraphics.rotate(rotX90); doorHandleGraphics.translate(new Vector3D(0.0, 0.0, -0.5 * depthY)); // center graphics for(RobotSide robotSide : RobotSide.values()) { doorHandleGraphics.addCylinder(robotSide.negateIfLeftSide(handleDoorSeparation+0.5*depthY), handleHingeRadius, YoAppearance.Grey()); // graphics - handle double translation = robotSide.negateIfLeftSide(handleDoorSeparation + 0.5*depthY); doorHandleGraphics.translate(new Vector3D(0.0, 0.0, translation)); doorHandleGraphics.rotate(new AxisAngle(0.0, 1.0, 0.0, robotSide.negateIfRightSide(0.5 * Math.PI))); doorHandleGraphics.addCylinder(robotSide.negateIfLeftSide(handleLength), handleRadius, YoAppearance.Grey()); doorHandleGraphics.rotate(new AxisAngle(0.0, 1.0, 0.0, -robotSide.negateIfRightSide(0.5 * Math.PI))); doorHandleGraphics.translate(new Vector3D(0.0, 0.0, -translation)); } handleLink.setLinkGraphics(doorHandleGraphics); }
AppearanceDefinition potholeAppearance = YoAppearance.Grey(); AppearanceDefinition flatAppearance = YoAppearance.DarkGrey();
AppearanceDefinition flatAppearance = YoAppearance.Grey();
Graphics3DObjectTools.addPlanarRegionsList(graphics3DObject, planarRegionsList, YoAppearance.White(), YoAppearance.Grey(), YoAppearance.DarkGray()); scs.setGroundVisible(false);
generator.addRectangle(startingBlockLength, 1.75); addPlanarRegionsToTerrain(YoAppearance.Grey());
private static SimulationConstructionSet setupSCS(String testName, YoVariableRegistry testRegistry, PlanarRegionsList regions, Point3D start, Point3D goal) { Robot robot = new Robot(VisibilityGraphsOcclusionTest.class.getSimpleName()); robot.addYoVariableRegistry(testRegistry); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationTestingParameters); Graphics3DObject graphics3DObject = new Graphics3DObject(); graphics3DObject.addCoordinateSystem(0.8); if (regions != null) { Graphics3DObjectTools.addPlanarRegionsList(graphics3DObject, regions, YoAppearance.White(), YoAppearance.Grey(), YoAppearance.DarkGray()); scs.setGroundVisible(false); } graphics3DObject.identity(); graphics3DObject.translate(start); graphics3DObject.translate(0.0, 0.0, 0.05); graphics3DObject.addCone(0.3, 0.05, YoAppearance.Green()); graphics3DObject.identity(); graphics3DObject.translate(goal); graphics3DObject.translate(0.0, 0.0, 0.05); graphics3DObject.addCone(0.3, 0.05, YoAppearance.Red()); scs.addStaticLinkGraphics(graphics3DObject); scs.setCameraPosition(-7.0, -1.0, 25.0); scs.setCameraFix(0.0, 0.0, 0.0); return scs; }
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 CombinedTerrainObject3D setUpInclinedSurface(int startRow, int endRow, int startColumn, int endColumn) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D("InclinedSurface"); AppearanceDefinition inclinedSurfaceAppearance = YoAppearance.Grey(); AppearanceDefinition wallAppearance = new YoAppearanceTexture("Textures/cinderBlock2.jpeg"); YoAppearance.makeTransparent(wallAppearance, 0.7f); int rampLengthDimension = (endColumn - startColumn + 1); int rampWidthDimension = (endRow - startRow + 1); double wallThickness = 0.05; double wallHeight = gridWidth; double rampWidth = rampWidthDimension * gridLength - wallThickness; double rampLength = rampLengthDimension * gridWidth; double centerX = (getWorldCoordinate(startRow, startColumn).getX() + getWorldCoordinate(endRow, endColumn).getX()) / 2 + wallThickness / 2; double centerY = (getWorldCoordinate(startRow, startColumn).getY() + getWorldCoordinate(endRow, endColumn).getY()) / 2; RotatableRampTerrainObject inclinedSurface = new RotatableRampTerrainObject(centerX, centerY, rampLength, rampWidth, flatGridHeight, -90, inclinedSurfaceAppearance); combinedTerrainObject.addTerrainObject(inclinedSurface); for (int i = 0; i < rampLengthDimension; i++) { RigidBodyTransform wallLocation = new RigidBodyTransform(); wallLocation.appendTranslation(centerX, centerY, 0.0); wallLocation.appendTranslation(-wallThickness / 2 - rampWidth / 2, 0.0, wallHeight / 2); wallLocation.appendTranslation(0.0, rampLength / 2, 0.0); wallLocation.appendTranslation(0.0, -wallHeight * (i + 0.5), 0.0); combinedTerrainObject.addTerrainObject(new RotatableBoxTerrainObject(wallLocation, wallThickness, wallHeight, wallHeight, wallAppearance)); } return combinedTerrainObject; }
if (regions != null) Graphics3DObjectTools.addPlanarRegionsList(graphics3DObject, regions, YoAppearance.White(), YoAppearance.Grey(), YoAppearance.DarkGray()); scs.setGroundVisible(false);
public SimpleMazeEnvironment() { super("SimpleMazeEnvironment"); // ground RotatableBoxTerrainObject ground = new RotatableBoxTerrainObject(new Box3D(new Point3D(2.0, 0.0, -0.5), new Quaternion(), 6, 4, 1), YoAppearance.Grey()); addTerrainObject(ground); // front-left wall RotatableBoxTerrainObject wall1 = new RotatableBoxTerrainObject(new Box3D(new Point3D(1.5, 0.75, 0.5), new Quaternion(), 0.1, 2.5, 1.0), YoAppearance.DarkGrey()); addTerrainObject(wall1); // front-right wall RotatableBoxTerrainObject wall2 = new RotatableBoxTerrainObject(new Box3D(new Point3D(1.5, -1.625, 0.5), new Quaternion(), 0.1, 0.75, 1.0), YoAppearance.DarkGrey()); addTerrainObject(wall2); // back-left wall RotatableBoxTerrainObject wall3 = new RotatableBoxTerrainObject(new Box3D(new Point3D(3.0, 1.625, 0.5), new Quaternion(), 0.1, 0.75, 1.0), YoAppearance.DarkGrey()); addTerrainObject(wall3); // back-right wall RotatableBoxTerrainObject wall4 = new RotatableBoxTerrainObject(new Box3D(new Point3D(3.0, -0.75, 0.5), new Quaternion(), 0.1, 2.5, 1.0), YoAppearance.DarkGrey()); addTerrainObject(wall4); }