private double getExpectedFinalTime(SimulationConstructionSet scs) { double initialTime = scs.getRobots()[0].getTime(); double recordFreq = scs.getRecordFreq(); double DT = scs.getDT(); return initialTime + recordFreq * DT; }
public void simulateAndBlock(int simulationTicks) throws SimulationExceededMaximumTimeException, ControllerFailureException { double startTime = scs.getTime(); scs.simulate(simulationTicks); BlockingSimulationRunner.waitForSimulationToFinish(scs, 600, true); if (hasControllerFailed.get()) { throw new ControllerFailureException("Controller failure has been detected."); } double endTime = scs.getTime(); double elapsedTime = endTime - startTime; if (Math.abs(elapsedTime - scs.getDT() * simulationTicks) > 0.01) { throw new SimulationExceededMaximumTimeException("Elapsed time didn't equal requested. Sim probably crashed"); } }
AngularMomentumRecorder(DRCSimulationTestHelper helper) { this.recordFrequency = (int) Math.round(angularMomentumRecordDT / helper.getSimulationConstructionSet().getDT()); this.angularMomentumX = (YoDouble) helper.getYoVariable("AngularMomentumX"); this.angularMomentumY = (YoDouble) helper.getYoVariable("AngularMomentumY"); this.angularMomentumZ = (YoDouble) helper.getYoVariable("AngularMomentumZ"); this.time = (YoDouble) helper.getYoVariable("t"); drcSimulationTestHelper.getSimulationConstructionSet().addScript(this); recordCounter = recordFrequency - 1; }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, FloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector .createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED, 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
private OscillateFeetPerturber generateFeetPertuber(final SimulationConstructionSet simulationConstructionSet, HumanoidFloatingRootJointRobot robot, int ticksPerPerturbation) { OscillateFeetPerturber oscillateFeetPerturber = new OscillateFeetPerturber(robot, simulationConstructionSet.getDT() * (ticksPerPerturbation)); oscillateFeetPerturber.setTranslationMagnitude(new double[] { 0.008, 0.012, 0.005 }); oscillateFeetPerturber.setRotationMagnitudeYawPitchRoll(new double[] { 0.010, 0.06, 0.010 }); oscillateFeetPerturber.setTranslationFrequencyHz(RobotSide.LEFT, new double[] { 1.0, 2.5, 3.3 }); oscillateFeetPerturber.setTranslationFrequencyHz(RobotSide.RIGHT, new double[] { 2.0, 0.5, 1.3 }); oscillateFeetPerturber.setRotationFrequencyHzYawPitchRoll(RobotSide.LEFT, new double[] { 5.0, 0.5, 0.3 }); oscillateFeetPerturber.setRotationFrequencyHzYawPitchRoll(RobotSide.RIGHT, new double[] { 0.2, 3.4, 1.11 }); return oscillateFeetPerturber; }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED , 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED , 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
public void visualizeFootstep(ContactablePlaneBody bipedFoot, Footstep footstep) { bagOfSingleFootstepVisualizers.visualizeFootstep(footstep, bipedFoot); nullRobot.setTime(nullRobot.getTime() + scs.getDT()); FramePoint3D solePositon = new FramePoint3D(footstep.getSoleReferenceFrame()); solePositon.changeFrame(worldFrame); updateFocus(solePositon); scs.setCameraFix(focusX, focusY, 0.0); scs.setCameraPosition(focusX, focusY - 1.5, 6.0); scs.tickAndUpdate(); }
public void visualizeFootstep(ContactablePlaneBody bipedFoot, Footstep footstep) { bagOfSingleFootstepVisualizers.visualizeFootstep(footstep, bipedFoot); nullRobot.setTime(nullRobot.getTime() + scs.getDT()); FramePoint solePositon = new FramePoint(footstep.getSoleReferenceFrame()); solePositon.changeFrame(worldFrame); updateFocus(solePositon); scs.setCameraFix(focusX, focusY, 0.0); scs.setCameraPosition(focusX, focusY - 1.5, 6.0); scs.tickAndUpdate(); }
@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); }
int simulationTicks = (int) (getRobotModel().getControllerDT() / scs.getDT()); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(simulationTicks);
public void testStandingWithStateEstimatorDrift() throws SimulationExceededMaximumTimeException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(flatGround); drcSimulationTestHelper.createSimulation("DRCSimpleFlatGroundScriptTest"); SimulationConstructionSet simulationConstructionSet = drcSimulationTestHelper.getSimulationConstructionSet(); HumanoidFloatingRootJointRobot robot = drcSimulationTestHelper.getRobot(); FullHumanoidRobotModel fullRobotModel = drcSimulationTestHelper.getControllerFullRobotModel(); setupCameraForWalkingUpToRamp(); Vector3DReadOnly slidingVelocity = new Vector3D(0.10, 0.0, 0.0); double simDT = simulationConstructionSet.getDT(); Script stateEstimatorDriftator = createStateEstimatorDriftator(simulationConstructionSet, fullRobotModel, slidingVelocity, simDT); simulationConstructionSet.addScript(stateEstimatorDriftator); boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10.0); drcSimulationTestHelper.createVideo(getSimpleRobotName(), 1); drcSimulationTestHelper.checkNothingChanged(); assertTrue(success); Point3D center = new Point3D(0.0, 0.0, 0.86); Vector3D plusMinusVector = new Vector3D(0.06, 0.06, 0.05); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }
OscillateFeetPerturber oscillateFeetPerturber = new OscillateFeetPerturber(robot, simulationConstructionSet.getDT() * (ticksPerPerturbation)); oscillateFeetPerturber.setTranslationMagnitude(new double[] {0.01, 0.015, 0.005}); oscillateFeetPerturber.setRotationMagnitudeYawPitchRoll(new double[] {0.017, 0.012, 0.011});
OscillateFeetPerturber oscillateFeetPerturber = new OscillateFeetPerturber(robot, simulationConstructionSet.getDT() * (ticksPerPerturbation)); oscillateFeetPerturber.setTranslationMagnitude(new double[] {0.008, 0.011, 0.004}); oscillateFeetPerturber.setRotationMagnitudeYawPitchRoll(new double[] {0.012, 0.047, 0.009});
double DT = scs.getDT(); callSCSMethodSimulateOneTimeStep(scs); double finalTime = scs.getRobots()[0].getTime();