How to use

getDefaultRobotInitialSetup

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

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);