diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java index 3b1dc38a1ec2..3d7d07b63375 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java +++ b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java @@ -85,7 +85,8 @@ public void setUseExperimentalPhysicsEngine(boolean useExperimentalPhysicsEngine this.useExperimentalPhysicsEngine = useExperimentalPhysicsEngine; } - public void testStairs(TestInfo testInfo, boolean squareUpSteps, boolean goingUp, double swingDuration, double transferDuration, double heightOffset) throws Exception + public void testStairs(TestInfo testInfo, boolean squareUpSteps, boolean goingUp, double swingDuration, double transferDuration, double heightOffset) + throws Exception { testStairs(testInfo, squareUpSteps, goingUp, swingDuration, transferDuration, heightOffset, null); } @@ -96,8 +97,7 @@ public void testStairs(TestInfo testInfo, double swingDuration, double transferDuration, double heightOffset, - Consumer corruptor) - throws Exception + Consumer corruptor) throws Exception { DRCRobotModel robotModel = getRobotModel(); double actualFootLength = robotModel.getWalkingControllerParameters().getSteppingParameters().getActualFootLength(); @@ -105,7 +105,9 @@ public void testStairs(TestInfo testInfo, double startZ = goingUp ? 0.0 : numberOfSteps * stepHeight; StairsEnvironment environment = new StairsEnvironment(numberOfSteps, stepHeight, stepLength, true); - SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, environment, simulationTestingParameters); + SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, + environment, + simulationTestingParameters); simulationTestHelperFactory.setStartingLocationOffset(new OffsetAndYawRobotInitialSetup(startX, 0, startZ)); simulationTestHelperFactory.setUseImpulseBasedPhysicsEngine(useExperimentalPhysicsEngine); simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation(); @@ -138,6 +140,88 @@ public void testStairs(TestInfo testInfo, } } + public void testSpecialStairs(boolean goingUp, + double swingDuration, + double transferDuration) throws Exception + { + DRCRobotModel robotModel = getRobotModel(); + double startX = 0.4; + double startZ = goingUp ? 0.0 : numberOfSteps * stepHeight; + + // 9" high step + double stepLength = 0.3; + double stepHeight = 9 * 2.54 / 100.0; + double stanceWidth = 0.22; + + StairsEnvironment environment = new StairsEnvironment(2, stepHeight, stepLength, true); + SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, + environment, + simulationTestingParameters); + simulationTestHelperFactory.setStartingLocationOffset(new OffsetAndYawRobotInitialSetup(startX, 0, startZ)); + simulationTestHelperFactory.setUseImpulseBasedPhysicsEngine(useExperimentalPhysicsEngine); + simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation(); + simulationTestHelper.start(); + + simulationTestHelper.setCameraFocusPosition(startX, 0.0, 0.8 + startZ); + simulationTestHelper.setCameraPosition(startX, -5.0, 0.8 + startZ); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + FootstepDataListMessage firstStepUpList = new FootstepDataListMessage(); + FootstepDataMessage firstStepUp = firstStepUpList.getFootstepDataList().add(); + firstStepUp.setRobotSide(RobotSide.LEFT.toByte()); + firstStepUp.getLocation().setX(startX + stepLength); + firstStepUp.getLocation().setY(stanceWidth / 2.0); + firstStepUp.getLocation().setZ(stepHeight); + firstStepUp.setSwingDuration(swingDuration); + firstStepUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, firstStepUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + FootstepDataListMessage firstSquareUpList = new FootstepDataListMessage(); + FootstepDataMessage firstSquareUp = firstSquareUpList.getFootstepDataList().add(); + firstSquareUp.setRobotSide(RobotSide.RIGHT.toByte()); + firstSquareUp.getLocation().setX(startX + stepLength); + firstSquareUp.getLocation().setY(-stanceWidth / 2.0); + firstSquareUp.getLocation().setZ(stepHeight); + firstSquareUp.setSwingDuration(swingDuration); + firstSquareUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, firstSquareUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + FootstepDataListMessage secondStepUpList = new FootstepDataListMessage(); + FootstepDataMessage secondStepUp = secondStepUpList.getFootstepDataList().add(); + secondStepUp.setRobotSide(RobotSide.LEFT.toByte()); + secondStepUp.getLocation().setX(2.0 * startX + stepLength); + secondStepUp.getLocation().setY(stanceWidth / 2.0); + secondStepUp.getLocation().setZ(2.0 * stepHeight); + secondStepUp.setSwingDuration(swingDuration); + secondStepUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, secondStepUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + + FootstepDataListMessage secondSquareUpList = new FootstepDataListMessage(); + FootstepDataMessage secondSquareUp = secondSquareUpList.getFootstepDataList().add(); + secondSquareUp.setRobotSide(RobotSide.RIGHT.toByte()); + secondSquareUp.getLocation().setX(2.0 * startX + stepLength); + secondSquareUp.getLocation().setY(-stanceWidth / 2.0); + secondSquareUp.getLocation().setZ(2.0 * stepHeight); + secondSquareUp.setSwingDuration(swingDuration); + secondSquareUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, secondSquareUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + } + private void publishHeightOffset(double heightOffset) { if (!Double.isFinite(heightOffset) || EuclidCoreTools.epsilonEquals(0.0, heightOffset, 1.0e-3)) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java index f73c85141907..a5e66bced836 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java @@ -422,11 +422,15 @@ private void computeWaypoints(FramePoint3DReadOnly startCoMPosition, fourthMidpoint.setXY(fourthMidpointX, fourthMidpointY); endWaypoint.setXY(endWaypointX, endWaypointY); - // Handle the case where the feet are basically squared up when stepping up. In that case, the max height needs to be pinned by the "from" foot, as the + // Handle the case where the feet are basically squared up when stepping up, but are at different heights In that case, the max height needs to be pinned + // by the "from" foot, as the // way the phase variable works means we can approach what is meant to be swing on the "to" foot while still in the "from" foot. - if (Math.abs(transferToPosition.getX()) < 0.1 && endGroundHeight > startGroundHeight) + double THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP = 0.1; + double THRESHOLD_FOR_STEP_UP_HEIGHT = 0.05; + if (Math.abs(transferToPosition.getX()) < THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP && endGroundHeight > startGroundHeight + && transferToPosition.getZ() > transferFromPosition.getZ() + THRESHOLD_FOR_STEP_UP_HEIGHT) { - double blend = Math.abs(transferToPosition.getX()) / 0.1; + double blend = Math.abs(transferToPosition.getX()) / THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP; double minHeight = Math.min(startGroundHeight + extraToeOffHeight, endGroundHeight); endGroundHeight = EuclidCoreTools.interpolate(minHeight, endGroundHeight, blend); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java index 9cb731604944..08aaf271dff4 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java @@ -12,6 +12,7 @@ import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler; import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.communication.controllerAPI.CommandInputManager; +import us.ihmc.humanoidRobotics.footstep.Footstep; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.robotSide.RobotSide; @@ -34,6 +35,8 @@ public class StandingState extends WalkingState private final SideDependentList handManagers = new SideDependentList<>(); private final FeetManager feetManager; + private RobotSide supportingSide; + public StandingState(CommandInputManager commandInputManager, WalkingMessageHandler walkingMessageHandler, TouchdownErrorCompensator touchdownErrorCompensator, @@ -74,7 +77,7 @@ public StandingState(CommandInputManager commandInputManager, public void doAction(double timeInState) { if (!holdDesiredHeightConstantWhenStanding) - comHeightManager.setSupportLeg(RobotSide.LEFT); + comHeightManager.setSupportLeg(supportingSide); balanceManager.computeICPPlan(); controllerToolbox.getWalkingTrajectoryPath().updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), feetManager.getCurrentConstraintType(RobotSide.RIGHT), @@ -99,10 +102,17 @@ public void onEntry() if (holdDesiredHeightConstantWhenStanding) { comHeightManager.initializeToNominalDesiredHeight(); + supportingSide = RobotSide.LEFT; } else { - TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(RobotSide.RIGHT); + Footstep footstepLeft = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.LEFT); + Footstep footstepRight = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.RIGHT); + + supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight); + supportingSide = supportingSide == null ? RobotSide.LEFT : supportingSide; + + TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide.getOppositeSide()); comHeightManager.initialize(transferToAndNextFootstepsDataForDoubleSupport, 0.0); } @@ -151,4 +161,23 @@ public boolean isDone(double timeInState) { return true; } + + private RobotSide getSideCarryingMostWeight(Footstep leftFootstep, Footstep rightFootstep) + { + WalkingStateEnum previousWalkingState = getPreviousWalkingStateEnum(); + if (previousWalkingState == null) + return null; + + RobotSide mostSupportingSide = null; + boolean leftStepLower = leftFootstep.getZ() <= rightFootstep.getZ(); + boolean rightStepLower = leftFootstep.getZ() > rightFootstep.getZ(); + if (leftStepLower) + mostSupportingSide = RobotSide.LEFT; + else if (rightStepLower) + mostSupportingSide = RobotSide.RIGHT; + else if (previousWalkingState.getTransferToSide() != null) + mostSupportingSide = previousWalkingState.getTransferToSide().getOppositeSide(); + + return mostSupportingSide; + } } \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java index abdfad8cb5c8..9f2a57904467 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java @@ -11,6 +11,7 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.commonWalkingControlModules.referenceFrames.WalkingTrajectoryPath; import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.humanoidRobotics.footstep.Footstep; @@ -33,6 +34,7 @@ public class TransferToStandingState extends WalkingState private final FeetManager feetManager; private final Point3D midFootPosition = new Point3D(); + private RobotSide supportingSide; public TransferToStandingState(WalkingMessageHandler walkingMessageHandler, TouchdownErrorCompensator touchdownErrorCompensator, @@ -65,7 +67,7 @@ public void doAction(double timeInState) switchToPointToeOffIfAlreadyInLine(); // Always do this so that when a foot slips or is loaded in the air, the height gets adjusted. - comHeightManager.setSupportLeg(RobotSide.LEFT); + comHeightManager.setSupportLeg(supportingSide); } private final FramePoint2D desiredCoP = new FramePoint2D(); @@ -171,14 +173,14 @@ else if (previousStateEnum.getTransferToSide() != null) Footstep footstepLeft = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.LEFT); Footstep footstepRight = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.RIGHT); - RobotSide supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight); + supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight); supportingSide = supportingSide == null ? RobotSide.RIGHT : supportingSide; double extraToeOffHeight = 0.0; if (feetManager.getCurrentConstraintType(supportingSide.getOppositeSide()) == FootControlModule.ConstraintType.TOES) extraToeOffHeight = feetManager.getExtraCoMMaxHeightWithToes(); - TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide); + TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide.getOppositeSide()); comHeightManager.setSupportLeg(supportingSide); comHeightManager.initialize(transferToAndNextFootstepsDataForDoubleSupport, extraToeOffHeight); diff --git a/open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java b/open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java deleted file mode 100644 index 66f6e7524501..000000000000 --- a/open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java +++ /dev/null @@ -1,68 +0,0 @@ -package us.ihmc.openAlexander.controllerAPI; - -import org.junit.jupiter.api.Disabled; -import org.junit.jupiter.api.Test; -import org.junit.jupiter.api.TestInfo; -import us.ihmc.openAlexander.OpenAlexanderVersion; -import us.ihmc.openAlexander.OpenAlexanderRobotModel; -import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.avatar.roughTerrainWalking.HumanoidEndToEndStairsTest; -import us.ihmc.simulationConstructionSetTools.tools.CITools; -import us.ihmc.simulationConstructionSetTools.tools.CITools.SimpleRobotNameKeys; - -public class AlexanderEndToEndStairsTest extends HumanoidEndToEndStairsTest -{ - @Override - public DRCRobotModel getRobotModel() - { - return new OpenAlexanderRobotModel(OpenAlexanderVersion.V1_FULL_ROBOT); - } - - @Disabled - @Test - public void testUpStairsSlow(TestInfo testInfo) throws Exception - { - testStairs(testInfo, true, true, 0.6, 0.25, 0.0); - } - - @Disabled - @Test - public void testDownStairsSlow(TestInfo testInfo) throws Exception - { - testStairs(testInfo, true, false, 0.9, 0.25, 0.0); - } - - @Disabled - @Test - public void testUpStairs(TestInfo testInfo) throws Exception - { - testStairs(testInfo, false, true, 0.9, 0.25, 0.0); - } - - @Disabled - @Test - public void testDownStairs(TestInfo testInfo) throws Exception - { - testStairs(testInfo, false, false, 1.0, 0.35, 0.0); - } - - /** - * Test for comparison against real robot data \\Gideon\LogData\Atlas\20210624_AtlasDemoFilming\20210624_140852_AtlasStairsPipeBombDisposalBestRunYet - */ - @Disabled - @Test - public void testUpStairsSlowFBDemo(TestInfo testInfo) throws Exception - { - setStepHeight(6.75 * 0.0254); - setStepLength(0.285); - setUseExperimentalPhysicsEngine(true); - setNumberOfSteps(3); - testStairs(testInfo, true, true, 2.0, 0.8, 0.0); - } - - @Override - public String getSimpleRobotName() - { - return CITools.getSimpleRobotNameFor(SimpleRobotNameKeys.ALEXANDER); - } -} diff --git a/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java b/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java index f7d30a11e0b7..e5ea6e77c863 100644 --- a/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java +++ b/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java @@ -42,4 +42,11 @@ public void testDownStairs(TestInfo testInfo) throws Exception { testStairs(testInfo, false, false, 1.0, 0.35, 0.0); } + + @Test + public void testSpecialStairs() throws Exception + { + testSpecialStairs(true, 0.8, 0.5); + } + }