public void initializeDesiredFootstep(RobotSide supportLegSide) {
RobotSide swingLegSide = supportLegSide.getOppositeSide();
ReferenceFrame supportAnkleZUpFrame = ankleZUpFrames.get(supportLegSide);
ReferenceFrame supportAnkleFrame = ankleFrames.get(supportLegSide);
computeDistanceAndAngleToDestination(
supportAnkleZUpFrame, swingLegSide, desiredDestination.getFramePoint2dCopy());
if (distanceToDestination.getDoubleValue() < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
numberBlindStepsInPlace.increment();
}
Matrix3d footToWorldRotation =
computeDesiredFootRotation(
angleToDestination.getDoubleValue(), swingLegSide, supportAnkleFrame);
FramePoint footstepPosition =
getDesiredFootstepPositionCopy(
supportAnkleZUpFrame,
supportAnkleFrame,
swingLegSide,
desiredDestination.getFramePoint2dCopy(),
footToWorldRotation);
setYoVariables(swingLegSide, footToWorldRotation, footstepPosition);
}
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 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;
}
@Override
public void receivedPacket(SteeringWheelInformationPacket packet) {
if (packet == null) return;
if (!PacketValidityChecker.validateSteeringWheelInformationPacket(
packet, steeringWheelIdAtomic, globalDataProducer)) return;
RobotSide robotSide = packet.getRobotSide();
steeringWheelCenterAtomic.get(robotSide).set(packet.getSteeringWheelCenter());
steeringWheelRotationAxisAtomic.get(robotSide).set(packet.getSteeringWheelRotationAxis());
steeringWheelZeroAxisAtomic.get(robotSide).set(packet.getSteeringWheelZeroAxis());
steeringWheelRadiusAtomic.get(robotSide).set(packet.getSteeringWheelRadius());
graspOffsetFromCotnrolFrameAtomic.get(robotSide).set(packet.getGraspOffsetFromControlFrame());
desiredSteeringSpeedAtomic.get(robotSide).set(packet.getDesiredSteeringSpeed());
steeringWheelIdAtomic.get(robotSide).set(packet.getSteeringWheelId());
hasReceivedNewSteeringWheelInformation.get(robotSide).set(true);
}
public KinematicsBasedFootSwitch(
String footName,
SideDependentList<? extends ContactablePlaneBody> bipedFeet,
double switchZThreshold,
double totalRobotWeight,
RobotSide side,
YoVariableRegistry parentRegistry) {
registry = new YoVariableRegistry(footName + getClass().getSimpleName());
foot = bipedFeet.get(side);
ContactablePlaneBody oppositeFoot = bipedFeet.get(side.getOppositeSide());
otherFeet = new ContactablePlaneBody[] {oppositeFoot};
this.totalRobotWeight = totalRobotWeight;
hitGround = new BooleanYoVariable(footName + "hitGround", registry);
fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
soleZ = new DoubleYoVariable(footName + "soleZ", registry);
ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
this.switchZThreshold.set(switchZThreshold);
yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);
parentRegistry.addChild(registry);
}
private void startSlipping(RobotSide robotSide) {
GroundContactPointsSlipper groundContactPointsSlipper =
groundContactPointsSlippers.get(robotSide);
generateRandomSlipParamters();
groundContactPointsSlipper.setGroundContactPoints(groundContactPointsMap.get(robotSide));
groundContactPointsSlipper.setPercentToSlipPerTick(nextSlipPercentSlipPerTick.getDoubleValue());
groundContactPointsSlipper.setDoSlip(true);
groundContactPointsSlipper.setSlipTranslation(nextTranslationToSlip.getVector3dCopy());
groundContactPointsSlipper.setSlipRotationYawPitchRoll(nextRotationToSlip.getYawPitchRoll());
// System.out.println("Slip of " + robotSide.getLowerCaseName() + " foot with amount" +
// nextTranslationToSlip.getVector3dCopy().toString()
// + " with rotation amount " +
// nextRotationToSlip.getFrameOrientationCopy().toStringAsYawPitchRoll()
// + " with slippercentage per tick " +
// nextSlipPercentSlipPerTick.getDoubleValue() + ", " + nextSlipAfterTimeDelta.getDoubleValue()
// + " [s] after touchdown.");
}
public boolean toeOnTheFloor(RobotSide robotSide) {
return GCpointsToe.get(robotSide).isInContact();
}
// Feet
public boolean heelOnTheFloor(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).isInContact();
}
public double getDesiredAbsoluteSteeringAngle(RobotSide robotSide) {
return desiredAbsoluteSteeringAngle.get(robotSide).get();
}
public double getDesiredSteeringSpeed(RobotSide robotSide) {
return desiredSteeringSpeedAtomic.get(robotSide).get();
}
public double getSteeringWheelRadius(RobotSide robotSide) {
return steeringWheelRadiusAtomic.get(robotSide).get();
}
public double getGraspOffsetFromControlFrame(RobotSide robotSide) {
return graspOffsetFromCotnrolFrameAtomic.get(robotSide).get();
}
public boolean checkForNewSteeringWheelInformation(RobotSide robotSide) {
return hasReceivedNewSteeringWheelInformation.get(robotSide).getAndSet(false);
}
public boolean checkForNewDesiredAbsoluteSteeringAngle(RobotSide robotSide) {
return hasReceivedNewDesiredSteeringAngle.get(robotSide).getAndSet(false);
}
@Override
public void doControl() {
super.doControl();
for (RobotSide robotSide : RobotSide.values()) {
GroundContactPointsSlipper groundContactPointsSlipper =
groundContactPointsSlippers.get(robotSide);
switch (slipStateMap.get(robotSide).getEnumValue()) {
case NO_CONTACT:
{
if (footTouchedDown(robotSide)) {
if (doSlipThisStance()) {
slipStateMap.get(robotSide).set(SlipState.CONTACT_WILL_SLIP);
touchdownTimeForSlipMap.get(robotSide).set(robot.getTime());
} else
// Wait till foot lift back up before allowing a slip.
{
slipStateMap.get(robotSide).set(SlipState.CONTACT);
}
}
break;
}
case CONTACT_WILL_SLIP:
{
if (robot.getTime()
> (touchdownTimeForSlipMap.get(robotSide).getDoubleValue()
+ nextSlipAfterTimeDelta.getDoubleValue())) {
if (slipStateMap.get(robotSide.getOppositeSide()).getEnumValue()
== SlipState.CONTACT_SLIP) {
// Stop other foot from slipping, two slipping feet no implemented yet
groundContactPointsSlipper.setDoSlip(false);
slipStateMap.get(robotSide.getOppositeSide()).set(SlipState.CONTACT_DONE_SLIP);
}
slipStateMap.get(robotSide).set(SlipState.CONTACT_SLIP);
startSlipping(robotSide);
}
break;
}
case CONTACT_SLIP:
{
if (groundContactPointsSlipper.isDoneSlipping()) {
slipStateMap.get(robotSide).set(SlipState.CONTACT_DONE_SLIP);
}
break;
}
case CONTACT_DONE_SLIP:
case CONTACT:
{
if (footLiftedUp(robotSide)) {
slipStateMap.get(robotSide).set(SlipState.NO_CONTACT);
}
break;
}
}
}
}
public boolean heelToeOffAhead(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getX()
> GCpointsHeel.get(robotSide.getOppositeSide()).getX();
}
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;
}
public abstract class DRCPushRecoveryMultiStepTest implements MultiRobotTestInterface {
private static final SimulationTestingParameters simulationTestingParameters =
SimulationTestingParameters.createFromEnvironmentVariables();
private DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack;
private static final boolean VISUALIZE_FORCE = false;
private static final double PUSH_DELAY = 0.5;
protected PushRobotController pushRobotController;
protected BlockingSimulationRunner blockingSimulationRunner;
protected double forceMagnitude;
protected double forceDuration;
protected SideDependentList<StateTransitionCondition> doubleSupportStartConditions =
new SideDependentList<>();
StateTransitionCondition pushCondition = doubleSupportStartConditions.get(RobotSide.LEFT);
@Before
public void showMemoryUsageBeforeTest() {
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " before test.");
}
@After
public void destroySimulationAndRecycleMemory() {
if (simulationTestingParameters.getKeepSCSUp()) {
ThreadTools.sleepForever();
}
// Do this here in case a test fails. That way the memory will be recycled.
if (drcFlatGroundWalkingTrack != null) {
drcFlatGroundWalkingTrack.destroySimulation();
drcFlatGroundWalkingTrack = null;
}
if (blockingSimulationRunner != null) {
blockingSimulationRunner.destroySimulation();
blockingSimulationRunner = null;
}
pushRobotController = null;
doubleSupportStartConditions = null;
pushCondition = null;
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " after test.");
}
@Ignore("Needs to be improved")
@ContinuousIntegrationTest(estimatedDuration = 67.1)
@Test(timeout = 340000)
public void testMultiStepForwardAndContinueWalking()
throws SimulationExceededMaximumTimeException, InterruptedException,
ControllerFailureException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
setupTest(getRobotModel());
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
setForwardPushParameters();
Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(4.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);
// simulate for a little while longer
blockingSimulationRunner.simulateAndBlock(forceDuration + 6.0);
// re-enable walking
walk.set(true);
blockingSimulationRunner.simulateAndBlock(6.0);
Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
Vector3d plusMinusVector =
new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
BoundingBox3d boundingBox =
BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
@ContinuousIntegrationTest(estimatedDuration = 53.2)
@Test(timeout = 270000)
public void testMultiStepBackwardAndContinueWalking()
throws SimulationExceededMaximumTimeException, InterruptedException,
ControllerFailureException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
setupTest(getRobotModel());
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
setBackwardPushParameters();
Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(1.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);
// simulate for a little while longer
blockingSimulationRunner.simulateAndBlock(forceDuration + 5.0);
// re-enable walking
walk.set(true);
blockingSimulationRunner.simulateAndBlock(6.0);
Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
Vector3d plusMinusVector =
new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
BoundingBox3d boundingBox =
BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
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();
}
private class DoubleSupportStartCondition implements StateTransitionCondition {
private final EnumYoVariable<WalkingStateEnum> walkingState;
private final RobotSide side;
public DoubleSupportStartCondition(
EnumYoVariable<WalkingStateEnum> walkingState, RobotSide side) {
this.walkingState = walkingState;
this.side = side;
}
@Override
public boolean checkCondition() {
if (side == RobotSide.LEFT) {
return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
|| (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_LEFT_SUPPORT);
} else {
return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
|| (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_RIGHT_SUPPORT);
}
}
}
private void createVideo(SimulationConstructionSet scs) {
if (simulationTestingParameters.getCreateSCSVideos()) {
BambooTools.createVideoAndDataWithDateTimeClassMethodAndShareOnSharedDriveIfAvailable(
getSimpleRobotName(), scs, 1);
}
}
protected abstract void setForwardPushParameters();
protected abstract void setBackwardPushParameters();
}
public double getHeelZ(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getZ();
}
public double getToeX(RobotSide robotSide) {
return GCpointsToe.get(robotSide).getX();
}
public void setFStoTrue(RobotSide robotSide) {
GCpointsHeel.get(robotSide).getYoFootSwitch().set(1.0);
}
