@Override public AppearanceDefinition getAppearance() { // Does not matter as the appearance is generated internally return YoAppearance.AliceBlue(); }
@Override public AppearanceDefinition getAppearance() { // Does not matter as the appearance is generated internally return YoAppearance.AliceBlue(); }
@Override public AppearanceDefinition getAppearance() { // Does not matter as the appearance is generated internally return YoAppearance.AliceBlue(); }
public CartRobotRacingEnvironment(boolean useRampTerrain) { combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); for (int i = 0; i < numberOfPlates; i++) { combinedTerrainObject.addBox(-0.5 * plateSize + plateSize * i, -0.5 * plateSize, 0.5 * plateSize + plateSize * i, 0.5 * plateSize, -plateThickness - plateHeightGap * i, -plateHeightGap * i); } combinedTerrainObject.addSphere(0.5, 0.0, 0.0, 0.05, YoAppearance.AliceBlue()); RigidBodyTransform cylinderObstacleTransform = new RigidBodyTransform(); cylinderObstacleTransform.appendRollRotation(0.5 * Math.PI); combinedTerrainObject.addCylinder(cylinderObstacleTransform, 0.3, 0.05, YoAppearance.AliceBlue()); if (useRampTerrain) combinedTerrainObject.addRamp(0.8, -0.7, 2.0, 0.7, 0.5, YoAppearance.AliceBlue()); }
meshBuffer.add(graphics3dObject.addMeshData(null, YoAppearance.AliceBlue()));
meshBuffer.add(graphics3dObject.addMeshData(null, YoAppearance.AliceBlue()));
meshBuffer.add(graphics3dObject.addMeshData(null, YoAppearance.AliceBlue()));
private void visualizeSpline() { double t0; double tf; double t; for (int i = 0; i < numberOfVisualizationMarkers; i++) { t0 = concatenatedSplinesWithArcLengthCalculatedIteratively.getT0(); tf = concatenatedSplinesWithArcLengthCalculatedIteratively.getTf(); t = t0 + (double) i / (double) (numberOfVisualizationMarkers) * (tf - t0); compute(t); trajectoryBagOfBalls.setBall(desiredPosition.getFramePointCopy(), i); } for (int i = 0; i < nonAccelerationEndpointIndices.length; i++) { fixedPointBagOfBalls.setBall(allPositions[nonAccelerationEndpointIndices[i]].getFramePointCopy(), YoAppearance.AliceBlue(), i); } }
public void addVerticalDebrisLeaningAgainstAWall(double xRelativeToRobot, double yRelativeToRobot, double debrisYaw, double debrisPitch) { Point3D tempPosition = new Point3D(xRelativeToRobot, yRelativeToRobot, debrisLength / 2.0 * Math.cos(debrisPitch)); FramePose3D debrisPose = generateDebrisPose(tempPosition, debrisYaw, debrisPitch, 0.0); debrisRobots.add(createDebrisRobot(debrisPose)); double supportWidth = 0.1; double supportLength = 0.2; double supportHeight = 1.05*debrisLength; RigidBodyTransform debrisTransform = new RigidBodyTransform(); debrisPose.get(debrisTransform ); TransformTools.appendRotation(debrisTransform, -debrisPitch, Axis.Y); debrisPose.set(debrisTransform); debrisPose.setZ(0.0); PoseReferenceFrame debrisReferenceFrame = new PoseReferenceFrame("debrisReferenceFrame", debrisPose); FramePose3D supportPose = new FramePose3D(debrisReferenceFrame); double x = supportWidth / 2.0 + debrisLength/2.0 * Math.sin(debrisPitch); double y = 0.0; double z = supportHeight / 2.0; supportPose.setPosition(x, y, z); supportPose.changeFrame(constructionWorldFrame); RigidBodyTransform supportTransform = new RigidBodyTransform(); supportPose.get(supportTransform); combinedTerrainObject.addRotatableBox(supportTransform, supportWidth, supportLength, supportHeight, YoAppearance.AliceBlue()); }
RigidBodyTransform firstSupportTransform = new RigidBodyTransform(); firstSupportPose.get(firstSupportTransform); combinedTerrainObject.addRotatableBox(firstSupportTransform, supportLength, supportWidth, supportHeight, YoAppearance.AliceBlue()); RigidBodyTransform secondSupportTransform = new RigidBodyTransform(); secondSupportPose.get(secondSupportTransform); combinedTerrainObject.addRotatableBox(secondSupportTransform, supportLength, supportWidth, supportHeight, YoAppearance.AliceBlue());
meshBuffer.add(graphics3dObject.addMeshData(null, YoAppearance.AliceBlue()));
scs.addStaticLinkGraphics(createTrajectoryMessageVisualization(manifold, 0.01, YoAppearance.AliceBlue()));
YoAppearance.AliceBlue()));
scs.addStaticLinkGraphics(createFunctionTrajectoryVisualization(handFunction, 0.0, trajectoryTime, timeResolution, 0.01, YoAppearance.AliceBlue()));
scs.addStaticLinkGraphics(createTrajectoryMessageVisualization(trajectoryMessage, 0.01, YoAppearance.AliceBlue()));
graphics3DAddMeshDataInstructions[i] = graphics3dObject.addMeshData(segmentedLine3DMeshGenerator.getMeshDataHolders()[i], YoAppearance.AliceBlue());
for (MeshDataHolder mesh : segmentedLine3DMeshGenerator.getMeshDataHolders()) graphics.addMeshData(mesh, YoAppearance.AliceBlue());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimplePartialFoothold() { FootstepNode nodeToSnap = new FootstepNode(1.0, 0.0); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setRotation(new AxisAngle(0.0, 1.0, 0.0, 0.25 * Math.PI)); transformToWorld.setTranslationZ(-1.0); ConvexPolygon2D partialFootholdPolygon = new ConvexPolygon2D(footPolygons.get(RobotSide.LEFT)); partialFootholdPolygon.scale(0.5); partialFootholdPolygon.translate(Math.sqrt(2.0), 0.0); PlanarRegion planarRegion = new PlanarRegion(transformToWorld, partialFootholdPolygon); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); snapper.setPlanarRegions(planarRegionsList); FootstepNodeSnapData snapData = snapper.snapFootstepNode(nodeToSnap); if(visualize) { visualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.AliceBlue()); visualizer.setSnappedPolygon(nodeTransform, snapData.getSnapTransform(), snapData.getCroppedFoothold()); ThreadTools.sleepForever(); } assertEquals(snapData.getCroppedFoothold().getArea(), partialFootholdPolygon.getArea(), epsilon); PlanarRegionPolygonSnapperTest.assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(snapData.getSnapTransform(), transformToWorld); }
private void doAFullFootholdTest(RigidBodyTransform regionToWorldFrameTransform, FootstepNode nodeToSnap) { RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); ConvexPolygon2D footholdPolygon = new ConvexPolygon2D(unitSquare); footholdPolygon.applyTransform(nodeTransform, false); PlanarRegion planarRegion = createPlanarRegion(regionToWorldFrameTransform, footholdPolygon); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); snapper.setPlanarRegions(planarRegionsList); FootstepNodeSnapData snapData = snapper.snapFootstepNode(nodeToSnap); if(visualize) { RigidBodyTransform snappedNodeTransform = new RigidBodyTransform(); FootstepNodeTools.getSnappedNodeTransform(nodeToSnap, snapData.getSnapTransform(), snappedNodeTransform); visualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.AliceBlue()); visualizer.setSnappedPolygon(nodeTransform, snapData.getSnapTransform()); ThreadTools.sleepForever(); } assertEquals(snapData.getCroppedFoothold().getArea(), footPolygons.get(RobotSide.LEFT).getArea(), epsilon); PlanarRegionPolygonSnapperTest.assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(snapData.getSnapTransform(), regionToWorldFrameTransform); }
graphics3DAddMeshDataInstructions[i] = graphics3dObject.addMeshData(segmentedLine3DMeshGenerator.getMeshDataHolders()[i], YoAppearance.AliceBlue());