protected void setupTest(DRCRobotModel robotModel)
throws SimulationExceededMaximumTimeException, InterruptedException {
boolean runMultiThreaded = false;
setupTrack(runMultiThreaded, robotModel);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
pushRobotController =
new PushRobotController(
drcFlatGroundWalkingTrack.getDrcSimulation().getRobot(), fullRobotModel);
if (VISUALIZE_FORCE) {
drcFlatGroundWalkingTrack
.getSimulationConstructionSet()
.addYoGraphic(pushRobotController.getForceVisualizer());
}
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
BooleanYoVariable enable =
(BooleanYoVariable) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery");
// enable push recovery
enable.set(true);
for (RobotSide robotSide : RobotSide.values) {
String prefix = fullRobotModel.getFoot(robotSide).getName();
scs.getVariable(prefix + "FootControlModule", prefix + "State");
@SuppressWarnings("unchecked")
final EnumYoVariable<WalkingStateEnum> walkingState =
(EnumYoVariable<WalkingStateEnum>)
scs.getVariable("WalkingHighLevelHumanoidController", "walkingState");
doubleSupportStartConditions.put(
robotSide, new DoubleSupportStartCondition(walkingState, robotSide));
}
}
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.getDrcSimulation();
}
protected void setupSubscribers(ConnectedNode connectedNode) {
DRCRobotSensorInformation sensorInforamtion = robotModel.getSensorInformation();
DRCRobotLidarParameters lidarParameters = sensorInforamtion.getLidarParameters(0);
if (lidarParameters != null && lidarParameters.getLidarSpindleJointTopic() != null) {
jointSubscriber =
connectedNode.newSubscriber(
lidarParameters.getLidarSpindleJointTopic(), sensor_msgs.JointState._TYPE);
}
}
public ValkyrieSliderBoard(
SimulationConstructionSet scs,
YoVariableRegistry registry,
DRCRobotModel drcRobotModel,
ValkyrieSliderBoardType sliderBoardType) {
selectedJoint =
new EnumYoVariable<>("selectedJoint", registry, ValkyrieSliderBoardSelectableJoints.class);
selectedJoint.set(ValkyrieSliderBoardSelectableJoints.RightKneeExtensor);
final SliderBoardConfigurationManager sliderBoardConfigurationManager =
new SliderBoardConfigurationManager(scs);
switch (sliderBoardType) {
case ON_BOARD_POSITION:
setupSliderBoardForOnBoardPositionControl(
registry, drcRobotModel.getGeneralizedRobotModel(), sliderBoardConfigurationManager);
break;
case TORQUE_PD_CONTROL:
setupSliderBoardForForceControl(
registry, drcRobotModel.getGeneralizedRobotModel(), sliderBoardConfigurationManager);
break;
case WALKING:
new WalkControllerSliderBoard(scs, registry, drcRobotModel);
setupJoyStickAndTreadmill(registry);
break;
case GRAVITY_COMPENSATION:
new InverseDynamicsJointController.GravityCompensationSliderBoard(
scs,
drcRobotModel.createFullRobotModel(),
registry,
CommonNames.doIHMCControlRatio.toString(),
0.0,
1.0);
break;
}
sliderBoardConfigurationManager.loadConfiguration(selectedJoint.getEnumValue().toString());
}
public RosConnectedZeroPoseRobotConfigurationDataProducer(
URI rosMasterURI, PacketCommunicator objectCommunicator, final DRCRobotModel robotModel) {
this.robotModel = robotModel;
this.packetCommunicator = objectCommunicator;
fullRobotModel = robotModel.createFullRobotModel();
referenceFrames = new HumanoidReferenceFrames(fullRobotModel);
pelvisFrame = referenceFrames.getPelvisFrame();
headFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH);
forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions();
updateRobotLocationBasedOnMultisensePose(new RigidBodyTransform());
if (rosMasterURI != null) {
NodeConfiguration nodeConfiguration = RosTools.createNodeConfiguration(rosMasterURI);
NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault();
nodeMainExecutor.execute(this, nodeConfiguration);
}
}
public GazeboOutputWriter(DRCRobotModel robotModel) {
estimatorTicksPerControlTick =
(int) Math.round(robotModel.getControllerDT() / robotModel.getEstimatorDT());
estimatorFrequencyInHz = (int) (1.0 / robotModel.getEstimatorDT());
}
public ROSAPISimulator(
DRCRobotModel robotModel,
DRCStartingLocation startingLocation,
String nameSpace,
String tfPrefix,
boolean runAutomaticDiagnosticRoutine,
boolean disableViz)
throws IOException {
this.robotModel = robotModel;
DRCSimulationStarter simulationStarter =
new DRCSimulationStarter(robotModel, createEnvironment());
simulationStarter.setRunMultiThreaded(true);
DRCNetworkModuleParameters networkProcessorParameters = new DRCNetworkModuleParameters();
URI rosUri = NetworkParameters.getROSURI();
networkProcessorParameters.setRosUri(rosUri);
PacketCommunicator gfe_communicator =
PacketCommunicator.createIntraprocessPacketCommunicator(
NetworkPorts.GFE_COMMUNICATOR, new IHMCCommunicationKryoNetClassList());
networkProcessorParameters.enableGFECommunicator(true);
if (runAutomaticDiagnosticRoutine) {
networkProcessorParameters.enableBehaviorModule(true);
networkProcessorParameters.enableBehaviorVisualizer(true);
networkProcessorParameters.enableAutomaticDiagnostic(true, 5);
}
if (START_UI) {
networkProcessorParameters.enableUiModule(true);
System.err.println(
"Cannot start UI automatically from open source projects. Start UI Manually");
}
if (disableViz) {
DRCGuiInitialSetup guiSetup = new DRCGuiInitialSetup(false, false, false);
simulationStarter.setGuiInitialSetup(guiSetup);
}
simulationStarter.registerHighLevelController(new JointPositionControllerFactory(false));
simulationStarter.setStartingLocation(startingLocation);
simulationStarter.setInitializeEstimatorToActual(true);
simulationStarter.startSimulation(networkProcessorParameters, true);
if (REDIRECT_UI_PACKETS_TO_ROS) {
PacketRouter<PacketDestination> packetRouter = simulationStarter.getPacketRouter();
new UiPacketToRosMsgRedirector(
robotModel,
rosUri,
gfe_communicator,
packetRouter,
DEFAULT_PREFIX + "/" + robotModel.getSimpleRobotName().toLowerCase());
}
LocalObjectCommunicator sensorCommunicator =
simulationStarter.getSimulatedSensorsPacketCommunicator();
SimulationRosClockPPSTimestampOffsetProvider ppsOffsetProvider =
new SimulationRosClockPPSTimestampOffsetProvider();
new ThePeoplesGloriousNetworkProcessor(
rosUri,
gfe_communicator,
sensorCommunicator,
ppsOffsetProvider,
robotModel,
nameSpace,
tfPrefix,
createCustomSubscribers(nameSpace, gfe_communicator),
createCustomPublishers(nameSpace, gfe_communicator));
}
