public void setRobotInitialSetup(double groundHeight, double initialYaw) { robotInitialSetup = robotModel.getDefaultRobotInitialSetup(groundHeight, initialYaw); }
public void setRobotInitialSetup(double groundHeight, double initialYaw) { robotInitialSetup = robotModel.getDefaultRobotInitialSetup(groundHeight, initialYaw); }
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); // scsInitialSetup.setInitializeEstimatorToActual(true); scsInitialSetup.setDrawGroundProfile(true); scsInitialSetup.setRunMultiThreaded(runMultiThreaded); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel); drcFlatGroundWalkingTrack.getAvatarSimulation(); }
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(true); scsInitialSetup.setDrawGroundProfile(true); scsInitialSetup.setRunMultiThreaded(runMultiThreaded); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel); }
private DRCFlatGroundWalkingTrack setupFlatGroundSimulationTrackForSameWayTwiceVerifier(DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup(); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setDrawGroundProfile(drawGroundProfile); if (cheatWithGroundHeightAtForFootstep) scsInitialSetup.setInitializeEstimatorToActual(true); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); setupCameraForUnitTest(scs); return drcFlatGroundWalkingTrack; }
private FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
private SimulationConstructionSet setupScs() { boolean useVelocityAndHeadingScript = true; boolean cheatWithGroundHeightAtForFootstep = false; GroundProfile3D groundProfile = new FlatGroundProfile(); DRCRobotModel robotModel = getRobotModel(); DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup(); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setRunMultiThreaded(false); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); setupCameraForUnitTest(scs); return scs; }
private FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
protected FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(groundHeight, initialYaw);
private DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment, GroundProfile3D groundProfile3D) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; this.scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT()); this.scsInitialSetup.setDrawGroundProfile(environment == null); this.scsInitialSetup.setInitializeEstimatorToActual(false); this.scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); this.scsInitialSetup.setRunMultiThreaded(true); this.highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
public DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; scsInitialSetup = new DRCSCSInitialSetup(environment, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(false); scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); scsInitialSetup.setRunMultiThreaded(true); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.armControllerParameters = robotModel.getArmControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.icpOptimizationParameters = robotModel.getICPOptimizationParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
private DRCFlatGroundWalkingTrack setupWalkingSim() { DRCRobotModel robotModel = getRobotModel(); DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setDrawGroundProfile(false); externalPelvisPosePublisher = new ExternalPelvisPoseCreator(); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotModel.getDefaultRobotInitialSetup(0.0, 0.0), guiInitialSetup, scsInitialSetup, true, false, getRobotModel(), externalPelvisPosePublisher); simulationConstructionSet = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); registry = robot.getRobotsYoVariableRegistry(); YoBoolean walk = (YoBoolean) simulationConstructionSet.getVariable("walkCSG"); walk.set(true); return drcFlatGroundWalkingTrack; }
public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive) { simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false); DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName()); if (robotInitialSetup == null) robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap()); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); DoubleYoVariable yoTime = simulatedRobot.getYoTime(); double dt = robotModel.getEstimatorDT(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); if (diagnosticParameters == null) diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false); DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters); SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration); DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt, sensorProcessingConfiguration, simulationRegistry); automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream, simulationRegistry); automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive); automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController); outputWriter = new DRCSimulationOutputWriter(simulatedRobot); outputWriter.setFullRobotModel(fullRobotModel, null); int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT()); simulatedRobot.setController(this, simulationTicksPerControlTick); return automatedDiagnosticConfiguration; }
public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive) { simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false); DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName()); if (robotInitialSetup == null) robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap()); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); YoDouble yoTime = simulatedRobot.getYoTime(); double dt = robotModel.getEstimatorDT(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); if (diagnosticParameters == null) diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false); JointDesiredOutputList lowLevelOutput = new JointDesiredOutputList(fullRobotModel.getOneDoFJoints()); DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters, lowLevelOutput); SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration); DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, lowLevelOutput, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt, sensorProcessingConfiguration, simulationRegistry); automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream, simulationRegistry); automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive); automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController); lowLevelOutputWriter = new SimulatedLowLevelOutputWriter(simulatedRobot, false); lowLevelOutputWriter.setJointDesiredOutputList(lowLevelOutput); int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT()); simulatedRobot.setController(this, simulationTicksPerControlTick); return automatedDiagnosticConfiguration; }
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0);
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0);
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(groundHeight, initialYaw);
DRCRobotJointMap jointMap = drcRobotModel.getJointMap(); HumanoidFloatingRootJointRobot humanoidFloatingRobotModel = drcRobotModel.createHumanoidFloatingRootJointRobot(false); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = drcRobotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(humanoidFloatingRobotModel, jointMap); SDFPerfectSimulatedSensorReader sdfPerfectReader = new SDFPerfectSimulatedSensorReader(humanoidFloatingRobotModel, humanoidFullRobotModel, null);
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = model.getDefaultRobotInitialSetup(groundHeight, initialYaw);