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