public YoGraphicCoordinateSystem(String namePrefix, String nameSuffix, YoVariableRegistry registry, double scale) { this(namePrefix, nameSuffix, registry, scale, YoAppearance.Gray()); }
public YoGraphicReferenceFrame(ReferenceFrame referenceFrame, YoVariableRegistry registry, double scale) { this(referenceFrame, registry, scale, YoAppearance.Gray()); }
public PlanarRegionTerrainObject(PlanarRegion planarRegion, double allowablePenetrationThickness) { this(planarRegion, allowablePenetrationThickness, YoAppearance.Gray()); }
public void addCoordinateSystem(double length) { addCoordinateSystem(length, YoAppearance.Gray()); }
public void addCoordinateSystem(double length) { addCoordinateSystem(length, YoAppearance.Gray()); }
private CombinedTerrainObject3D setUpGround(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.Gray()); return combinedTerrainObject; }
private ImmutablePair<CombinedTerrainObject3D, Double> createRamp() { double rampSlopeUp = 0.1; double rampSlopeDown = 0.08; double rampXStart0 = 0.5; double rampXLength0 = 6.0; //2.0; double landingHeight = rampSlopeUp * rampXLength0; double landingLength = 1.0; double rampXLength1 = landingHeight / rampSlopeDown; double rampYStart = -2.0; double rampYEnd = 6.0; double landingStartX = rampXStart0 + rampXLength0; double landingEndX = landingStartX + landingLength; double rampEndX = landingEndX + rampXLength1; CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D("JustARamp"); AppearanceDefinition appearance = YoAppearance.Green(); combinedTerrainObject.addRamp(rampXStart0, rampYStart, landingStartX, rampYEnd, landingHeight, appearance); combinedTerrainObject.addBox(landingStartX, rampYStart, landingEndX, rampYEnd, 0.0, landingHeight, YoAppearance.Gray()); combinedTerrainObject.addRamp(rampEndX, rampYStart, landingEndX, rampYEnd, landingHeight, appearance); combinedTerrainObject.addBox(rampXStart0 - 2.0, rampYStart, rampEndX + 6.0, rampYEnd, -0.05, 0.0); return new ImmutablePair<CombinedTerrainObject3D, Double>(combinedTerrainObject, rampEndX); }
private CombinedTerrainObject3D setUpGround(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.Gray()); combinedTerrainObject.addBox(FORWARD_OFFSET,HATCH_OPENING_WIDTH/2, FORWARD_OFFSET+HATCH_THICKNESS,(HATCH_OPENING_WIDTH/2)+0.5f, 0, HATCH_UPENING_HEIGHT+HATCH_UPENING_HEIGHT_OFF_GROUND, YoAppearance.DarkGray()); combinedTerrainObject.addBox(FORWARD_OFFSET,-HATCH_OPENING_WIDTH/2, FORWARD_OFFSET+HATCH_THICKNESS,-(HATCH_OPENING_WIDTH/2)-0.5f, 0, HATCH_UPENING_HEIGHT+HATCH_UPENING_HEIGHT_OFF_GROUND, YoAppearance.DarkGray()); combinedTerrainObject.addBox(FORWARD_OFFSET,(HATCH_OPENING_WIDTH/2)+0.5f, FORWARD_OFFSET+HATCH_THICKNESS,-(HATCH_OPENING_WIDTH/2)-0.5f, 0, HATCH_UPENING_HEIGHT_OFF_GROUND, YoAppearance.DarkGray()); combinedTerrainObject.addBox(FORWARD_OFFSET,(HATCH_OPENING_WIDTH/2)+0.5f, FORWARD_OFFSET+HATCH_THICKNESS,-(HATCH_OPENING_WIDTH/2)-0.5f, HATCH_UPENING_HEIGHT+ HATCH_UPENING_HEIGHT_OFF_GROUND, HATCH_UPENING_HEIGHT+ HATCH_UPENING_HEIGHT_OFF_GROUND+HATCH_THICKNESS, YoAppearance.DarkGray()); // combinedTerrainObject.addBox(2.0, -0.05, 3.0, 0.05, 2.0, YoAppearance.Beige()); // combinedTerrainObject.addBox(3.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.getX(), -0.05, 4.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.getX(), 0.05, 2.0, YoAppearance.Beige()); return combinedTerrainObject; }
private static CombinedTerrainObject3D setUpPath4DRCTrialsTrainingWalkingCourse(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); double courseAngleDeg = 45.0; double startDistance = 4.0; AppearanceDefinition color = YoAppearance.Gray(); final double sectionLength = 2.4384; // 8 ft // 1. Ramp and Zigzag Hurdle (Pitch Ramp 15degrees) int numberOfRamps = 1; combinedTerrainObject.addTerrainObject(setUpMultipleUpDownRamps("upDownRamp1", courseAngleDeg, startDistance, numberOfRamps, sectionLength / 2, color)); startDistance += sectionLength; combinedTerrainObject.addTerrainObject(setUpZigZagHurdles("zigZagHurdles", courseAngleDeg, startDistance, new int[] {9}, -45.0)); startDistance += sectionLength / 4; // 2a. Ascend Flat Top Steps // 2b. Descend Flat Top Steps // 3a. Ascend Pitch/Roll 15 deg Top Steps // 3b. Descend Pitch/Roll 15 deg Top Steps combinedTerrainObject.addTerrainObject(setUpCinderBlockFieldActual("cinderBlockField", courseAngleDeg, startDistance)); startDistance += sectionLength * 5; // 4. Two cinder block high hurdle for testcase purposes only, not part of actual trial obstacle course combinedTerrainObject.addTerrainObject(setUpZigZagHurdles("zigZagHurdlesTwoHigh", courseAngleDeg, startDistance, new int[] {8, 7}, 45.0)); return combinedTerrainObject; }
public static CombinedTerrainObject3D setUpPath4DRCTrialsTrainingWalkingCourse(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); double courseAngleDeg = 45.0; double startDistance = 4.0; AppearanceDefinition color = YoAppearance.Gray(); final double sectionLength = 2.4384; // 8 ft // 1. Ramp and Zigzag Hurdle (Pitch Ramp 15degrees) int numberOfRamps = 1; combinedTerrainObject.addTerrainObject(setUpMultipleUpDownRamps("upDownRamp1", courseAngleDeg, startDistance, numberOfRamps, sectionLength / 2, color)); startDistance += sectionLength; combinedTerrainObject.addTerrainObject(setUpZigZagHurdles("zigZagHurdles", courseAngleDeg, startDistance, new int[] {9}, -45.0)); startDistance += sectionLength / 4; // 2a. Ascend Flat Top Steps // 2b. Descend Flat Top Steps // 3a. Ascend Pitch/Roll 15 deg Top Steps // 3b. Descend Pitch/Roll 15 deg Top Steps combinedTerrainObject.addTerrainObject(setUpCinderBlockFieldActual("cinderBlockField", courseAngleDeg, startDistance)); startDistance += sectionLength * 5; // 4. Two cinder block high hurdle for testcase purposes only, not part of actual trial obstacle course combinedTerrainObject.addTerrainObject(setUpZigZagHurdles("zigZagHurdlesTwoHigh", courseAngleDeg, startDistance, new int[] {8, 7}, 45.0)); return combinedTerrainObject; }
AppearanceDefinition color = YoAppearance.Gray();
AppearanceDefinition color = YoAppearance.Gray();
Graphics3DObject handGraphics = new Graphics3DObject(); handGraphics.transform(endEffectorTransformToWrist); handGraphics.addEllipsoid(0.08, 0.03, 0.06, YoAppearance.Gray()); handGraphics.addCoordinateSystem(0.25); return handGraphics;
private CombinedTerrainObject3D setUpVan(String name, Point2D doorLocation) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addBox(doorLocation.getX() - dummyVanLength / 2, doorLocation.getY(), doorLocation.getX() + dummyVanLength / 2, doorLocation.getY() + dummyVanWidth, dummyVanWheelRadius, dummyVanWheelRadius + dummyVanHeight, YoAppearance.Gray()); RigidBodyTransform frontWheel = new RigidBodyTransform(); RigidBodyTransform rearWheel = new RigidBodyTransform(); frontWheel.setTranslation(doorLocation.getX() + dummyVanWheelDistance / 2, doorLocation.getY() + dummyVanWidth / 2, dummyVanWheelRadius); rearWheel.setTranslation(doorLocation.getX() - dummyVanWheelDistance / 2, doorLocation.getY() + dummyVanWidth / 2, dummyVanWheelRadius); frontWheel.appendRollRotation(Math.PI / 2); rearWheel.appendRollRotation(Math.PI / 2); double offset = 0.05; combinedTerrainObject.addCylinder(frontWheel, dummyVanWidth - offset, dummyVanWheelRadius, YoAppearance.DarkGray()); combinedTerrainObject.addCylinder(rearWheel, dummyVanWidth - offset, dummyVanWheelRadius, YoAppearance.DarkGray()); combinedTerrainObject.addBox(doorLocation.getX(), doorLocation.getY() - dummyDoorThickness, doorLocation.getX() + dummyDoorWidth, doorLocation.getY(), dummyDoorHeightOffset, dummyDoorHeightOffset + dummyDoorHeight, YoAppearance.DarkGray()); knob.appendTranslation(doorLocation.getX(), doorLocation.getY(), dummyDoorHeightOffset + dummyDoorHeight / 2); knob.appendTranslation(dummyDoorKnobLength / 2 + dummyDoorKnobDiameter, -dummyDoorThickness - dummyDoorKnobDiameter, 0); knob.appendPitchRotation(Math.PI / 2); combinedTerrainObject.addCylinder(knob, dummyDoorKnobLength, dummyDoorKnobDiameter / 2, YoAppearance.Gray()); return combinedTerrainObject; }
polygonSnapperVisualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.Gray()); polygonSnapperVisualizer.setSnappedPolygon(nonSnappedTransform, snapTransform);
@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(); } }
private CombinedTerrainObject3D setUpVessel(String name, Point2D doorLocation) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addSphere(doorLocation.getX(), doorLocation.getY(), vesselCartWheelRadius + vesselCartBodyHeight + vesselRadius, vesselRadius, YoAppearance.Blue()); combinedTerrainObject.addBox(doorLocation.getX() - vesselCartLength / 2, doorLocation.getY() - vesselCartLength / 2, doorLocation.getX() + vesselCartLength / 2, doorLocation.getY() + vesselCartLength / 2, vesselCartWheelRadius, vesselCartWheelRadius + vesselCartBodyHeight, YoAppearance.Gray()); RigidBodyTransform frontWheel = new RigidBodyTransform(); RigidBodyTransform rearWheel = new RigidBodyTransform(); frontWheel.setTranslation(doorLocation.getX() + vesselCartLength * 0.25, doorLocation.getY(), vesselCartWheelRadius); rearWheel.setTranslation(doorLocation.getX() - vesselCartLength * 0.25, doorLocation.getY(), vesselCartWheelRadius); frontWheel.appendRollRotation(Math.PI / 2); rearWheel.appendRollRotation(Math.PI / 2); double offset = 0.05; combinedTerrainObject.addCylinder(frontWheel, vesselCartLength - offset, vesselCartWheelRadius, YoAppearance.DarkGray()); combinedTerrainObject.addCylinder(rearWheel, vesselCartLength - offset, vesselCartWheelRadius, YoAppearance.DarkGray()); return combinedTerrainObject; }
color = YoAppearance.Gray(); float rampHeight = 0.625f;
color = YoAppearance.Gray(); float rampHeight = 0.625f;
yoGraphicsListRegistry.registerYoGraphic("yoIterativeClosestPointPoseInWorldFrameGraphic", yoIterativeClosestPointPoseInWorldFrameGraphic); YoGraphicCoordinateSystem yoStateEstimatorInWorldFramePoseGraphic = new YoGraphicCoordinateSystem("yoStateEstimatorInWorldFramePoseGraphic", yoStateEstimatorInWorldFramePose, 0.5, YoAppearance.Gray()); yoGraphicsListRegistry.registerYoGraphic("yoCorrectedPelvisPoseInWorldFrame", yoStateEstimatorInWorldFramePoseGraphic);