private SimulationConstructionSet constructSimulationConstructionSet(Robot robot, RobotController controller) { SimulationConstructionSetParameters parameters = SimulationConstructionSetParameters.createFromSystemProperties();; parameters.setCreateGUI(SHOW_GUI); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); scs.setDT(DT, 1); Thread thread = new Thread(scs); thread.start(); try { Thread.sleep(2000); } catch (InterruptedException e) { } return scs; }
private void setupSim() { SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(); parameters.setCreateGUI(simulationTestingParameters.getCreateGUI()); simulationConstructionSet = new SimulationConstructionSet(robot, parameters); simulationConstructionSet.setDT(0.001, 1); bsr = new BlockingSimulationRunner(simulationConstructionSet, 60.0 * 10.0); Thread simThread = new Thread(simulationConstructionSet); simThread.start(); }
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(); }
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(); }
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(); }
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 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(); }
public void initializeSimulation(SimulationConstructionSet scs) { scs.setDT(simulateDT, recordFrequency); if (drawGroundProfile) { // boolean drawGroundBelow = false; // ArrayList<Graphics3DObject> groundLinkGraphics = commonTerrain.createLinkGraphics(drawGroundBelow); ArrayList<Graphics3DObject> groundLinkGraphics = createGroundLinkGraphicsFromGroundProfile(groundProfile3D); scs.addStaticLinkGraphics(groundLinkGraphics); } 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 testAgainstLagrangianCalculation(RobotWithClosedFormDynamics robotWithClosedFormDynamics, double epsilon) { robotWithClosedFormDynamics.setController(new DynamicsChecker(robotWithClosedFormDynamics, epsilon)); SimulationConstructionSet scs = new SimulationConstructionSet(robotWithClosedFormDynamics, simulationTestingParameters); scs.setDT(1e-5, 20); scs.startOnAThread(); BlockingSimulationRunner blockingSimulationRunner = new BlockingSimulationRunner(scs, 45.0); try { blockingSimulationRunner.simulateAndBlock(15.0); } catch(Exception e) { fail(); } }
private void setupSimulationConstructionSet() { SimulationConstructionSetParameters simulationConstructionSetParameters = guiInitialSetup.get().getSimulationConstructionSetParameters(); simulationConstructionSetParameters.setDataBufferSize(scsInitialSetup.get().getSimulationDataBufferSize()); List<Robot> allSimulatedRobotList = new ArrayList<Robot>(); allSimulatedRobotList.add(humanoidFloatingRootJointRobot); if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getEnvironmentRobots() != null) { allSimulatedRobotList.addAll(commonAvatarEnvironment.get().getEnvironmentRobots()); commonAvatarEnvironment.get().addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints()); commonAvatarEnvironment.get().createAndSetContactControllerToARobot(); } simulationConstructionSet = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), guiInitialSetup.get().getGraphics3DAdapter(), simulationConstructionSetParameters); simulationConstructionSet.setDT(robotModel.get().getSimulateDT(), 1); }
private void setupSimulationConstructionSet() { SimulationConstructionSetParameters simulationConstructionSetParameters = guiInitialSetup.get().getSimulationConstructionSetParameters(); simulationConstructionSetParameters.setDataBufferSize(scsInitialSetup.get().getSimulationDataBufferSize()); List<Robot> allSimulatedRobotList = new ArrayList<Robot>(); allSimulatedRobotList.add(humanoidFloatingRootJointRobot); if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getEnvironmentRobots() != null) { allSimulatedRobotList.addAll(commonAvatarEnvironment.get().getEnvironmentRobots()); commonAvatarEnvironment.get().addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints()); commonAvatarEnvironment.get().createAndSetContactControllerToARobot(); } simulationConstructionSet = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), guiInitialSetup.get().getGraphics3DAdapter(), simulationConstructionSetParameters); simulationConstructionSet.setDT(robotModel.get().getSimulateDT(), 1); }
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 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 void startSimulation() { SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters); scs.setDT(robotModel.getSimulateDT(), 10); scs.setCameraPosition(scsCameraPosition.getX(), scsCameraPosition.getY(), scsCameraPosition.getZ()); scs.setCameraFix(scsCameraFix.getX(), scsCameraFix.getY(), scsCameraFix.getZ()); scs.startOnAThread(); }
@ContinuousIntegrationTest(estimatedDuration = 7.2) @Test(timeout = 36000) public void test() { TwoLinkRobotForTesting twoLinkRobotForTesting = new TwoLinkRobotForTesting(); SimulationConstructionSet scs = new SimulationConstructionSet(twoLinkRobotForTesting, simulationTestingParameters); scs.setDT(0.00001, 100); scs.startOnAThread(); twoLinkRobotForTesting.setElbowPosition(0.0); twoLinkRobotForTesting.setUpperPosition(3.0); twoLinkRobotForTesting.setElbowVelocity(-2.0); twoLinkRobotForTesting.setUpperVelocity(-3.0); scs.simulate(6.0); while(scs.isSimulating()) { Thread.yield(); } RobotAllJointsDataChecker robotAllJointsDataChecker = new RobotAllJointsDataChecker(scs, twoLinkRobotForTesting); robotAllJointsDataChecker.cropFirstPoint(); scs.applyDataProcessingFunction(robotAllJointsDataChecker); }
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 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 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(); }
@Disabled //org.junit.runners.model.TestTimedOutException: test timed out after 300000 milliseconds @Test// timeout=300000 public void testSimulationConstructionSetNewViewportWindowUsingGUITestFixture() throws AWTException { Assume.assumeTrue(!isGradleBuild()); FallingBrickRobot robot = new FallingBrickRobot(); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); scs.setDT(0.0001, 100); scs.setFrameMaximized(); scs.startOnAThread(); scs.setSimulateDuration(2.0); ThreadTools.sleep(2000); SimulationGUITestFixture testFixture = new SimulationGUITestFixture(scs); testFixture.closeAllViewportWindows(); testFixture.selectNewViewportWindowMenu(); testFixture.focusNthViewportWindow(0); ThreadTools.sleepForever(); testFixture.closeAndDispose(); scs.closeAndDispose(); scs = null; testFixture = null; }
@Test// timeout = 30000 public void testTimingMethods() throws AWTException { scs.setDT(simulateDT, recordFrequency); double simulateDTFromSCS = scs.getDT(); assertEquals(simulateDT, simulateDTFromSCS, epsilon); scs.setRecordDT(recordDT); double recordFreqFromSCS = scs.getRecordFreq(); assertEquals(recordFreq, recordFreqFromSCS, epsilon); scs.setPlaybackRealTimeRate(realTimeRate); double realTimeRateFromSCS = scs.getPlaybackRealTimeRate(); assertEquals(realTimeRate, realTimeRateFromSCS, epsilon); scs.setPlaybackDesiredFrameRate(frameRate); double frameRateFromSCS = scs.getPlaybackFrameRate(); assertEquals(recomputedSecondsPerFrameRate, frameRateFromSCS, epsilon); int ticksPerCycle = computeTicksPerPlayCycle(simulateDT, recordFreq, realTimeRate, frameRate); double ticksPerCycleFromSCS = scs.getTicksPerPlayCycle(); assertEquals(ticksPerCycle, ticksPerCycleFromSCS, epsilon); scs.setTime(Math.PI); double timeFromSCS = scs.getTime(); assertEquals(Math.PI, timeFromSCS, epsilon); }