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 startVisualizer() { scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.addYoVariableRegistry(registry); scs.setupGraphGroup("step times", new String[][] { {"t"} }); scs.startOnAThread(); scs.tickAndUpdate(); }
public void initializeSimulation(SimulationConstructionSet scs) { scs.setDT(simulateDT, recordFrequency); if (drawGroundProfile) { scs.addStaticLinkGraphics(createGroundLinkGraphicsFromGroundProfile(groundProfile3D)); } if (SHOW_WORLD_COORDINATE_FRAME) { Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(0.3); scs.addStaticLinkGraphics(linkGraphics); } /* * This makes sure that the initial values of all YoVariables that are added to the scs (i.e. * at index 0 of the data buffer) are properly stored in the data buffer */ scs.getDataBuffer().copyValuesThrough(); }
private void startSimAndDisplayLink(Link linkToDisplay) { // Robot nullRobot = new Robot("Null"); sim = new SimulationConstructionSet(parameters); sim.addStaticLink(linkToDisplay); // position the camera to view links sim.setCameraPosition(6.0, 6.0, 3.0); sim.setCameraFix(0.5, 0.5, 0.0); sim.setGroundVisible(false); sim.startOnAThread(); ThreadTools.sleep(3000); sim.closeAndDispose(); }
public LogDataProcessorWrapper(SimulationConstructionSet scs) { scs.addYoVariableRegistry(logDataProcessorRegistry); scs.addScript(this); controllerTimerCount = (YoLong) scs.getVariable(DRCControllerThread.class.getSimpleName(), "controllerTimerCount"); haveFoundControllerTimerVariable = controllerTimerCount != null; if (!haveFoundControllerTimerVariable) { System.err.println("Could not find controller timer variable, running processors at log data rate"); } }
public void startSimulation() throws IOException { SimpleLidarRobot robot = new SimpleLidarRobot(); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(true, RealtimeTools.nextPowerOfTwo(200000)); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); double simDT = 0.0001; double controlDT = 0.01; scs.setDT(simDT, 10); scs.setSimulateDoneCriterion(() -> false); Graphics3DAdapter graphics3dAdapter = scs.getGraphics3dAdapter(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); SimpleLidarRobotController controller = new SimpleLidarRobotController(robot, controlDT, ros2Node, graphics3dAdapter, yoGraphicsListRegistry); robot.setController(controller, (int) (controlDT / simDT)); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); createGroundTypeListener(scs); scs.setGroundVisible(false); scs.startOnAThread(); scs.simulate(); }
public static PlanarRegionBipedalFootstepPlannerVisualizer createWithSimulationConstructionSet(double dtForViz, SideDependentList<ConvexPolygon2d> footPolygonsInSoleFrame) { YoVariableRegistry registry = new YoVariableRegistry(PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName()); YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); PlanarRegionBipedalFootstepPlannerVisualizer footstepPlannerVisualizer = new PlanarRegionBipedalFootstepPlannerVisualizer(10, footPolygonsInSoleFrame, registry, graphicsListRegistry); SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("Test")); footstepPlannerVisualizer.setTickAndUpdatable(scs); scs.changeBufferSize(32000); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(graphicsListRegistry); scs.setDT(dtForViz, 1); scs.setGroundVisible(false); scs.startOnAThread(); return footstepPlannerVisualizer; }
public static void main(String[] args) { YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); DRCDemoEnvironmentSimpleSteeringWheelContact env = new DRCDemoEnvironmentSimpleSteeringWheelContact(yoGraphicsListRegistry); List<Robot> robots = env.getEnvironmentRobots(); Robot[] robotArray = new Robot[robots.size()]; robots.toArray(robotArray); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(); parameters.setDataBufferSize(36000); SimulationConstructionSet scs = new SimulationConstructionSet(robotArray, parameters); scs.setDT(1e-4, 20); TerrainObject3D terrainObject = env.getTerrainObject3D(); scs.addStaticLinkGraphics(terrainObject.getLinkGraphics()); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.setGroundVisible(false); scs.startOnAThread(); }
public void startSimulation() { SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters); scs.setDT(robotModel.getSimulateDT(), 10); scs.setCameraPosition(scsCameraPosition.x, scsCameraPosition.y, scsCameraPosition.z); scs.setCameraFix(scsCameraFix.x, scsCameraFix.y, scsCameraFix.z); scs.startOnAThread(); }
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 static void visualizeFootsteps(Robot nullRobot, List<Footstep> footsteps, List<ContactablePlaneBody> contactablePlaneBodies) { SimulationConstructionSet scs = new SimulationConstructionSet(nullRobot); scs.setDT(0.25, 1); YoVariableRegistry rootRegistry = scs.getRootRegistry(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); int maxNumberOfContacts = 2; int maxPointsPerContact = 4; FootstepGeneratorVisualizer footstepGeneratorVisualizer = new FootstepGeneratorVisualizer(maxNumberOfContacts, maxPointsPerContact, rootRegistry, yoGraphicsListRegistry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); footstepGeneratorVisualizer.addFootstepsAndTickAndUpdate(scs, footsteps, contactablePlaneBodies); scs.startOnAThread(); deleteFirstDataPointAndCropData(scs); }
public PolygonSnapperVisualizer(ConvexPolygon2D snappingPolygonShape) { Robot robot = new Robot("Robot"); scs = new SimulationConstructionSet(robot); scs.setDT(0.1, 1); polygonToSnap = new YoFrameConvexPolygon2D("polygonToSnap", ReferenceFrame.getWorldFrame(), 4, registry); snappedPolygon = new YoFrameConvexPolygon2D("snappedPolygon", ReferenceFrame.getWorldFrame(), 4, registry); polygonToSnap.set(snappingPolygonShape); snappedPolygon.set(snappingPolygonShape); polygonToSnapPose = new YoFramePoseUsingYawPitchRoll("polygonToSnapPose", ReferenceFrame.getWorldFrame(), registry); snappedPolygonPose = new YoFramePoseUsingYawPitchRoll("snappedPolygonPose", ReferenceFrame.getWorldFrame(), registry); polygonToSnapPose.setToNaN(); snappedPolygonPose.setToNaN(); polygonToSnapViz = new YoGraphicPolygon("polygonToSnapViz", polygonToSnap, polygonToSnapPose, 1.0, YoAppearance.Green()); snappedPolygonViz = new YoGraphicPolygon("snappedPolygonViz", polygonToSnap, snappedPolygonPose, 1.0, YoAppearance.Red()); polygonToSnapViz.update(); snappedPolygonViz.update(); scs.addYoGraphic(polygonToSnapViz); scs.addYoGraphic(snappedPolygonViz); scs.addYoVariableRegistry(registry); scs.setGroundVisible(false); scs.startOnAThread(); }
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description) { nullRobot = new Robot("FootstepVisualizerRobot"); if (groundProfile != null) { GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry()); gcModel.setGroundProfile3D(groundProfile); nullRobot.setGroundContactModel(gcModel); } scs = new SimulationConstructionSet(nullRobot); if (linkGraphics != null) { scs.setGroundVisible(false); scs.addStaticLinkGraphics(linkGraphics); } printifdebug("Attaching exit listener"); scs.setDT(1, 1); yoGraphicsListRegistry = new YoGraphicsListRegistry(); bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry); addText(scs, yoGraphicsListRegistry, description); }
public ValkyrieSDFLoadingDemo() { ValkyrieRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.SCS, false); FloatingRootJointRobot valkyrieRobot = robotModel.createHumanoidFloatingRootJointRobot(false); valkyrieRobot.setPositionInWorld(new Vector3D()); if (SHOW_ELLIPSOIDS) { addIntertialEllipsoidsToVisualizer(valkyrieRobot); } if (SHOW_COORDINATES_AT_JOINT_ORIGIN) addJointAxis(valkyrieRobot); FullRobotModel fullRobotModel = robotModel.createFullRobotModel(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); CommonInertiaEllipsoidsVisualizer inertiaVis = new CommonInertiaEllipsoidsVisualizer(fullRobotModel.getElevator(), yoGraphicsListRegistry); inertiaVis.update(); scs = new SimulationConstructionSet(valkyrieRobot); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.setGroundVisible(false); scs.startOnAThread(); }
public ValkyrieDataFileNamespaceRenamer() { SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("null")); YoVariableRegistry rootRegistry = scs.getRootRegistry(); NameSpaceRenamer valkyrieNameSpaceRenamer = new ValkyrieNameSpaceRenamer(); ChangeNamespacesToMatchSimButton changeNamespacesToMatchSimButton = new ChangeNamespacesToMatchSimButton("ChangeValkyrieNamespaces", rootRegistry, valkyrieNameSpaceRenamer); scs.addButton(changeNamespacesToMatchSimButton); NameSpaceRenamer stepprNameSpaceRenamer = new StepprNameSpaceRenamer(); ChangeNamespacesToMatchSimButton changeStepprNamespacesToMatchSimButton = new ChangeNamespacesToMatchSimButton("ChangeStepprNamespaces", rootRegistry, stepprNameSpaceRenamer); scs.addButton(changeStepprNamespacesToMatchSimButton); scs.startOnAThread(); }
public static void main(String[] args) { final SimulationConstructionSet scs; // initialize SCS DRCRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.REAL_ROBOT, false); final FloatingRootJointRobot robot =robotModel.createHumanoidFloatingRootJointRobot(false); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(); parameters.setDataBufferSize(65536); scs = new SimulationConstructionSet(robot, parameters); // add sysid button JButton button = new JButton("findCOM"); button.addActionListener(new LinkComIDActionListener(scs.getDataBuffer(), robot)); scs.addButton(button); scs.startOnAThread(); } }
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 static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { Robot robot = new Robot("robot"); SimulationConstructionSet scs = new SimulationConstructionSet(robot); DRCVehicleSDFLoader drcVehicleSDFLoader = new DRCVehicleSDFLoader(); scs.addStaticLinkGraphics(drcVehicleSDFLoader.loadDRCVehicle(false)); RigidBodyTransform vehicleToWorldTransform = new RigidBodyTransform(); ReferenceFrame vehicleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("vehicle", ReferenceFrame.getWorldFrame(), vehicleToWorldTransform, false, true, true); VehicleModelObjects vehicleModelObjects = new VehicleModelObjects(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoVariableRegistry registry = scs.getRootRegistry(); VehicleModelObjectVisualizer vehicleModelObjectVisualizer = new VehicleModelObjectVisualizer(vehicleFrame, vehicleModelObjects, yoGraphicsListRegistry, registry); vehicleModelObjectVisualizer.setVisible(true); vehicleModelObjectVisualizer.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); Thread thread = new Thread(scs); thread.start(); }
public TestMultiThreadedSimulation() { DoublePendulum doublePendulum = new DoublePendulum(); SimulationConstructionSet scs = new SimulationConstructionSet(doublePendulum); DoublePendulumController controller = new DoublePendulumController(doublePendulum); MultiThreadedRobotController multiThreadedRobotController = new MultiThreadedRobotController("threadedController", doublePendulum, scs); multiThreadedRobotController.addController(controller, 10, false); multiThreadedRobotController.initialize(); doublePendulum.setController(multiThreadedRobotController); scs.setDT(0.0001, 1); scs.startOnAThread(); }