@Override public void notifyOfVariableChange(YoVariable<?> v) { if (groundGraphicsNode != null) scs.removeGraphics3dNode(groundGraphicsNode); groundGraphicsNode = scs.addStaticLinkGraphics(environmentsGraphics.get(groundType.getEnumValue())); } };
public static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { // JaxbSDFLoader loader = new JaxbSDFLoader(new File("Models/unitBox.sdf"), "Models/"); // Robot robot = loader.createRobot("unit_box_1"); SDFWorldLoader loader = new SDFWorldLoader(new File("Models/unitBox.sdf"), "Models/"); SimulationConstructionSet scs = new SimulationConstructionSet(); scs.addStaticLinkGraphics(loader.createGraphics3dObject()); Thread thread = new Thread(scs); thread.start(); } }
public static Graphics3DNode drawNodeBoundingBoxes(QuadTreeForGroundHeightMap heightMap, SimulationConstructionSet scs, double heightToDrawAt) { QuadTreeForGroundNode rootNode = heightMap.getRootNode(); Graphics3DObject nodeBoundsGraphic = new Graphics3DObject(); drawNodeBoundingBoxesRecursively(rootNode, nodeBoundsGraphic, 0, heightToDrawAt); Graphics3DNode graphics3DNodeHandle = scs.addStaticLinkGraphics(nodeBoundsGraphic); return graphics3DNodeHandle; }
public static Graphics3DNode drawNodeBoundingBoxes(QuadTreeForGroundHeightMap heightMap, SimulationConstructionSet scs, double heightToDrawAt) { QuadTreeForGroundNode rootNode = heightMap.getRootNode(); Graphics3DObject nodeBoundsGraphic = new Graphics3DObject(); drawNodeBoundingBoxesRecursively(rootNode, nodeBoundsGraphic, 0, heightToDrawAt); Graphics3DNode graphics3DNodeHandle = scs.addStaticLinkGraphics(nodeBoundsGraphic); return graphics3DNodeHandle; }
public void testSetupInEnvironment() { // Not an actual test, could be given @Test(timeout=300000) for visual confirmation though SimulationConstructionSet scs = new SimulationConstructionSet(); scs.addStaticLinkGraphics(inclinedTopFaceOctagon3d.getLinkGraphics()); scs.setGroundVisible(false); scs.startOnAThread(); while (true) { try { Thread.sleep(1000); } catch (InterruptedException e) { } } }
public void updateEnvironment(CommonAvatarEnvironmentInterface environment) { setupEnvironmentAndListSimulatedRobots(simulatedRobot, environment); if (environment != null && environment.getTerrainObject3D() != null) { scs.addStaticLinkGraphics(environment.getTerrainObject3D().getLinkGraphics()); } }
public static Graphics3DNode drawPoints(SimulationConstructionSet scs, ArrayList<Point3D> points, double resolution, AppearanceDefinition appearance) { Graphics3DObject pointsInQuadTreeGraphic = new Graphics3DObject(); for (Point3D point : points) { pointsInQuadTreeGraphic.identity(); pointsInQuadTreeGraphic.translate(point); pointsInQuadTreeGraphic.addCube(resolution, resolution, resolution/4.0, appearance); } Graphics3DNode graphics3DNodeHandle = scs.addStaticLinkGraphics(pointsInQuadTreeGraphic); return graphics3DNodeHandle; }
public static Graphics3DNode drawPoints(SimulationConstructionSet scs, ArrayList<Point3d> points, double resolution, AppearanceDefinition appearance) { Graphics3DObject pointsInQuadTreeGraphic = new Graphics3DObject(); for (Point3d point : points) { pointsInQuadTreeGraphic.identity(); pointsInQuadTreeGraphic.translate(point); pointsInQuadTreeGraphic.addCube(resolution, resolution, resolution/4.0, appearance); } Graphics3DNode graphics3DNodeHandle = scs.addStaticLinkGraphics(pointsInQuadTreeGraphic); return graphics3DNodeHandle; }
public void addPlanarRegionsList(PlanarRegionsList planarRegions, AppearanceDefinition... appearances) { Graphics3DObject graphics3DObject = new Graphics3DObject(); Graphics3DObjectTools.addPlanarRegionsList(graphics3DObject, planarRegions, appearances); scs.addStaticLinkGraphics(graphics3DObject); scs.setTime(scs.getTime() + 1.0); scs.tickAndUpdate(); }
public static void main(String[] args) { SimulationConstructionSet scs = new SimulationConstructionSet(); scs.setGroundVisible(false); scs.addStaticLinkGraphics(new CinderBlockFieldPlanarRegionEnvironment().getTerrainObject3D().getLinkGraphics()); scs.startOnAThread(); }
public void addPolygon(RigidBodyTransform transform, ConvexPolygon2D polygon, AppearanceDefinition appearance) { Graphics3DObject graphics3DObject = new Graphics3DObject(); graphics3DObject.transform(transform); graphics3DObject.addPolygon(polygon, appearance); scs.addStaticLinkGraphics(graphics3DObject); scs.setTime(scs.getTime() + 1.0); scs.tickAndUpdate(); }
public void updateEnvironment(CommonAvatarEnvironmentInterface commonAvatarEnvironment) { if (commonAvatarEnvironment != null && commonAvatarEnvironment.getEnvironmentRobots() != null) { commonAvatarEnvironment.addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints()); commonAvatarEnvironment.createAndSetContactControllerToARobot(); } if (commonAvatarEnvironment != null && commonAvatarEnvironment.getTerrainObject3D() != null) { simulationConstructionSet.addStaticLinkGraphics(commonAvatarEnvironment.getTerrainObject3D().getLinkGraphics()); } }
public void updateEnvironment(CommonAvatarEnvironmentInterface commonAvatarEnvironment) { if (commonAvatarEnvironment != null && commonAvatarEnvironment.getEnvironmentRobots() != null) { commonAvatarEnvironment.addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints()); commonAvatarEnvironment.createAndSetContactControllerToARobot(); } if (commonAvatarEnvironment != null && commonAvatarEnvironment.getTerrainObject3D() != null) { simulationConstructionSet.addStaticLinkGraphics(commonAvatarEnvironment.getTerrainObject3D().getLinkGraphics()); } }
public static void addCube(double cubeX, double cubeY, double cubeSize, Color color, SimulationConstructionSet scs) { final double CUBE_HEIGHT = 0.001; // Make cubes a tiny bit smaller so they don't blend together. cubeSize *= 0.9; Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.translate(new Vector3D(cubeX, cubeY ,CUBE_HEIGHT)); linkGraphics.addCube(cubeSize, cubeSize, cubeSize, new YoAppearanceRGBColor(new MutableColor(color), 0.0)); scs.addStaticLinkGraphics(linkGraphics); }
private void vidualizeCorruptedFootsteps(FootstepDataListMessage corruptedFootstepDataList, SimulationConstructionSet scs) { if (!VISUALIZE) return; List<FootstepDataMessage> dataList = corruptedFootstepDataList.getFootstepDataList(); for (int i = 0; i < dataList.size(); i++) { FootstepDataMessage footstepData = dataList.get(i); Graphics3DObject staticLinkGraphics = new Graphics3DObject(); staticLinkGraphics.translate(new Vector3D(footstepData.getLocation())); RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(footstepData.getOrientation()); staticLinkGraphics.rotate(rotationMatrix); staticLinkGraphics.addCoordinateSystem(0.15, YoAppearance.Red()); scs.addStaticLinkGraphics(staticLinkGraphics); } }
public static void main(String[] args) { SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("robot")); scs.addStaticLinkGraphics(new TwoBollardEnvironment(0.75).getTerrainObject3D().getLinkGraphics()); scs.setGroundVisible(false); scs.startOnAThread(); } }
private void initialize() { sphereVoxelShape = new SphereVoxelShape(gridFrame, voxelSize, numberOfRays, numberOfRotationsAroundRay, SphereVoxelType.graspOrigin); voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, gridSizeInNumberOfVoxels, voxelSize); if (reachabilityMapFileWriter != null) reachabilityMapFileWriter.initialize(solver.getRobotArmJoints(), voxel3dGrid); gridFrameViz.update(); scs.addStaticLinkGraphics(ReachabilityMapTools.createBoundingBoxGraphics(voxel3dGrid.getMinPoint(), voxel3dGrid.getMaxPoint())); }
public void addPoints(List<Point3D> points) { if(!disableGraphics) { for(Point3D point : points) { Graphics3DObject testCubeGraphicObject = new Graphics3DObject(); testCubeGraphicObject.addCube(VOXEL_SIZE, VOXEL_SIZE, VOXEL_SIZE, YoAppearance.Aqua()); Graphics3DNode cubeNode = scs.addStaticLinkGraphics(testCubeGraphicObject, Graphics3DNodeType.VISUALIZATION); cubeNode.translate(point.getX(), point.getY(), point.getZ()); if(rootNode == null) rootNode = cubeNode; else rootNode.addChild(cubeNode); } } }
private void startSCS() { scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(graphicsListRegistry); scs.setPlaybackRealTimeRate(0.025); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(0.3); scs.addStaticLinkGraphics(linkGraphics); scs.setCameraFix(0.0, 0.0, 0.5); scs.setCameraPosition(-0.5, 0.0, 1.0); SimulationOverheadPlotterFactory simulationOverheadPlotterFactory = scs.createSimulationOverheadPlotterFactory(); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(graphicsListRegistry); simulationOverheadPlotterFactory.createOverheadPlotter(); scs.startOnAThread(); }
public ReachabilitySphereMapCalculator(OneDoFJointBasics[] robotArmJoints, SimulationConstructionSet scs) { this.scs = scs; solver = new ReachabilityMapSolver(robotArmJoints, null, registry); FramePose3D gridFramePose = new FramePose3D(ReferenceFrame.getWorldFrame(), robotArmJoints[0].getFrameBeforeJoint().getTransformToWorldFrame()); gridFramePose.appendTranslation(getGridSizeInMeters() / 2.5, 0.0, 0.0); setGridFramePose(gridFramePose); scs.addStaticLinkGraphics(ReachabilityMapTools.createReachibilityColorScale()); scs.addYoGraphic(gridFrameViz); scs.addYoGraphic(currentEvaluationPose); scs.addYoGraphic(currentEvaluationPosition); scs.addYoVariableRegistry(registry); }