@Override public ArrayList<FootstepData> getStepsAlongPath(RobotSide firstStepSide) { if (overheadPathList.isEmpty()) { throw new RuntimeException(this.getClass().getSimpleName() + ": No Path for generator"); } // if (!hasHeightMap()) // { // TODO: @Agraber do simple planning assuming flat ground at the initial footstep height // throw new RuntimeException(this.getClass().getSimpleName() + ": No HeightMap"); // } ArrayList<FootstepData> generatedSteps = new ArrayList<>(); RobotSide lastStepSide = firstStepSide.getOppositeSide(); SideDependentList<FootstepData> lastFootsteps = initialFeet; for (FootstepOverheadPath overheadPath : overheadPathList) { // for each segment, generate a path to the end, then use that as the initial for the next super.initialize(lastFootsteps, overheadPath); List<FootstepData> segmentFootsteps = super.getStepsAlongPath(lastStepSide.getOppositeSide()); for (FootstepData footstep : segmentFootsteps) { generatedSteps.add(footstep); lastStepSide = footstep.getRobotSide(); lastFootsteps.put(lastStepSide, footstep); } // check that the segment reached the goal FramePose2d segmentGoal = overheadPath.getPoseAtDistance(overheadPath.getTotalDistance()); if (!isGoalFootstep(lastFootsteps.get(lastStepSide), segmentGoal, horizontalDistance)) { break; } } return generatedSteps; }
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 SideDependentList<ForceSensorDataReadOnly> createWristForceSensors( ForceSensorDataHolderReadOnly forceSensorDataHolder) { if (wristSensorNames == null) return null; SideDependentList<ForceSensorDataReadOnly> wristForceSensors = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { if (wristSensorNames.get(robotSide) == null) { return null; } ForceSensorDataReadOnly wristForceSensor = forceSensorDataHolder.getByName(wristSensorNames.get(robotSide)); wristForceSensors.put(robotSide, wristForceSensor); } return wristForceSensors; }
@Override public void initializePhysicalSensors(URI rosCoreURI) { if (rosCoreURI == null) throw new RuntimeException( getClass().getSimpleName() + " Physical sensor requires rosURI to be set in " + NetworkParameters.defaultParameterFile); sensorSuitePacketCommunicator.attachListener( RobotConfigurationData.class, robotConfigurationDataBuffer); ForceSensorNoiseEstimator forceSensorNoiseEstimator = new ForceSensorNoiseEstimator(sensorSuitePacketCommunicator); sensorSuitePacketCommunicator.attachListener( RobotConfigurationData.class, forceSensorNoiseEstimator); RosMainNode rosMainNode = new RosMainNode(rosCoreURI, "atlas/sensorSuiteManager", true); DRCRobotCameraParameters multisenseLeftEyeCameraParameters = sensorInformation.getCameraParameters(AtlasSensorInformation.MULTISENSE_SL_LEFT_CAMERA_ID); DRCRobotLidarParameters multisenseLidarParameters = sensorInformation.getLidarParameters(AtlasSensorInformation.MULTISENSE_LIDAR_ID); DRCRobotPointCloudParameters multisenseStereoParameters = sensorInformation.getPointCloudParameters(AtlasSensorInformation.MULTISENSE_STEREO_ID); MultiSenseSensorManager multiSenseSensorManager = new MultiSenseSensorManager( modelFactory, pointCloudDataReceiver, robotConfigurationDataBuffer, rosMainNode, sensorSuitePacketCommunicator, ppsTimestampOffsetProvider, rosCoreURI, multisenseLeftEyeCameraParameters, multisenseLidarParameters, multisenseStereoParameters, sensorInformation.setupROSParameterSetters()); DRCRobotCameraParameters leftFishEyeCameraParameters = sensorInformation.getCameraParameters(AtlasSensorInformation.BLACKFLY_LEFT_CAMERA_ID); DRCRobotCameraParameters rightFishEyeCameraParameters = sensorInformation.getCameraParameters(AtlasSensorInformation.BLACKFLY_RIGHT_CAMERA_ID); FisheyeCameraReceiver leftFishEyeCameraReceiver = new FisheyeCameraReceiver( modelFactory, leftFishEyeCameraParameters, robotConfigurationDataBuffer, sensorSuitePacketCommunicator, ppsTimestampOffsetProvider, rosMainNode); FisheyeCameraReceiver rightFishEyeCameraReceiver = new FisheyeCameraReceiver( modelFactory, rightFishEyeCameraParameters, robotConfigurationDataBuffer, sensorSuitePacketCommunicator, ppsTimestampOffsetProvider, rosMainNode); leftFishEyeCameraReceiver.start(); rightFishEyeCameraReceiver.start(); VisionPoseEstimator visionPoseEstimator = new VisionPoseEstimator( sensorSuitePacketCommunicator, pointCloudDataReceiver, modelFactory, robotConfigurationDataBuffer, true); multiSenseSensorManager.registerCameraListener(visionPoseEstimator); blackFlyParameterSetters = new SideDependentList<BlackFlyParameterSetter>(); for (RobotSide side : RobotSide.values) blackFlyParameterSetters.put( side, new BlackFlyParameterSetter( rosMainNode, side, "/" + side.getLowerCaseName() + "/camera/camera_nodelet", sensorSuitePacketCommunicator)); ppsTimestampOffsetProvider.attachToRosMainNode(rosMainNode); // if (DRCConfigParameters.CALIBRATE_ARM_MODE) // { // ArmCalibrationHelper armCalibrationHelper = new // ArmCalibrationHelper(sensorSuitePacketCommunicator, jointMap); // multiSenseSensorManager.registerCameraListener(armCalibrationHelper); // } multiSenseSensorManager.initializeParameterListeners(); // IMUBasedHeadPoseCalculatorFactory.create(sensorSuitePacketCommunicator, // sensorInformation, rosMainNode); rosMainNode.execute(); }
public SlipRandomOnNextStepPerturber(HumanoidFloatingRootJointRobot robot) { super("SlipRandomOnNextStepPerturber"); String name = "SlipRandom"; this.robot = robot; groundContactPointsSlippers = new SideDependentList<GroundContactPointsSlipper>(); for (RobotSide robotSide : RobotSide.values()) { DoubleYoVariable touchdownTimeForSlip = new DoubleYoVariable( robotSide.getCamelCaseNameForStartOfExpression() + "TouchdownTimeForSlip" + robotSide.getCamelCaseNameForMiddleOfExpression(), registry); touchdownTimeForSlipMap.put(robotSide, touchdownTimeForSlip); EnumYoVariable<SlipState> slipState = new EnumYoVariable<SlipState>( name + "SlipState" + robotSide.getCamelCaseNameForMiddleOfExpression(), registry, SlipState.class); slipState.set(SlipState.NO_CONTACT); slipStateMap.put(robotSide, slipState); groundContactPointsMap.put(robotSide, robot.getFootGroundContactPoints(robotSide)); GroundContactPointsSlipper groundContactPointsSlipper = new GroundContactPointsSlipper(robotSide.getLowerCaseName()); groundContactPointsSlippers.put(robotSide, groundContactPointsSlipper); this.addRobotController(groundContactPointsSlipper); } this.minSlipAfterTimeDelta = new DoubleYoVariable(name + "MinSlipAfterTimeDelta", registry); this.maxSlipAfterTimeDelta = new DoubleYoVariable(name + "MaxSlipAfterTimeDelta", registry); this.nextSlipAfterTimeDelta = new DoubleYoVariable(name + "NextSlipAfterTimeDelta", registry); this.minSlipPercentSlipPerTick = new DoubleYoVariable(name + "MinSlipPercentSlipPerTick", registry); this.maxSlipPercentSlipPerTick = new DoubleYoVariable(name + "MaxSlipPercentSlipPerTick", registry); this.nextSlipPercentSlipPerTick = new DoubleYoVariable(name + "NextSlipPercentSlipPerTick", registry); this.slipNextStep = new BooleanYoVariable(name + "SlipNextStep", registry); maxTranslationToSlipNextStep = new YoFrameVector( name + "MaxTranslationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); minTranslationToSlipNextStep = new YoFrameVector( name + "MinTranslationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); nextTranslationToSlip = new YoFrameVector(name + "NextTranslationToSlip", ReferenceFrame.getWorldFrame(), registry); maxRotationToSlipNextStep = new YoFrameOrientation( name + "MaxRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); minRotationToSlipNextStep = new YoFrameOrientation( name + "MinRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry); nextRotationToSlip = new YoFrameOrientation( name + "NextRotationToSlip", ReferenceFrame.getWorldFrame(), registry); setTranslationRangeToSlipNextStep(new double[] {0.0, 0.0, 0.0}, new double[] {0.05, 0.05, 0.0}); setRotationRangeToSlipNextStep(new double[] {0.0, 0.0, 0.0}, new double[] {0.3, 0.15, 0.1}); setSlipAfterStepTimeDeltaRange(0.01, 0.10); setSlipPercentSlipPerTickRange(0.01, 0.05); setProbabilityOfSlip(1.0); }
private SideDependentList<FootSwitchInterface> createFootSwitches( SideDependentList<? extends ContactablePlaneBody> bipedFeet, ForceSensorDataHolderReadOnly forceSensorDataHolder, ContactSensorHolder contactSensorHolder, double totalRobotWeight, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry registry) { SideDependentList<FootSwitchInterface> footSwitches = new SideDependentList<FootSwitchInterface>(); for (RobotSide robotSide : RobotSide.values) { FootSwitchInterface footSwitch = null; String footName = bipedFeet.get(robotSide).getName(); ForceSensorDataReadOnly footForceSensor = forceSensorDataHolder.getByName(footSensorNames.get(robotSide)); double contactThresholdForce = walkingControllerParameters.getContactThresholdForce(); double footSwitchCoPThresholdFraction = walkingControllerParameters.getCoPThresholdFraction(); switch (walkingControllerParameters.getFootSwitchType()) { case KinematicBased: footSwitch = new KinematicsBasedFootSwitch( footName, bipedFeet, walkingControllerParameters.getContactThresholdHeight(), totalRobotWeight, robotSide, registry); // controller switch doesnt need com break; case WrenchBased: WrenchBasedFootSwitch wrenchBasedFootSwitch = new WrenchBasedFootSwitch( footName, footForceSensor, footSwitchCoPThresholdFraction, totalRobotWeight, bipedFeet.get(robotSide), yoGraphicsListRegistry, contactThresholdForce, registry); wrenchBasedFootSwitch.setSecondContactThresholdForce( walkingControllerParameters.getSecondContactThresholdForceIgnoringCoP()); footSwitch = wrenchBasedFootSwitch; break; case WrenchAndContactSensorFused: footSwitch = new WrenchAndContactSensorFusedFootSwitch( footName, footForceSensor, contactSensorHolder.getByName(footContactSensorNames.get(robotSide)), footSwitchCoPThresholdFraction, totalRobotWeight, bipedFeet.get(robotSide), yoGraphicsListRegistry, contactThresholdForce, registry); break; } assert footSwitch != null; footSwitches.put(robotSide, footSwitch); } return footSwitches; }