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