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