added the special case for the stairs

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,16 +97,17 @@ public void testStairs(TestInfo testInfo,
                           double swingDuration,
                           double transferDuration,
                           double heightOffset,
-                          Consumer<FootstepDataListMessage> corruptor)
-         throws Exception
+                          Consumer<FootstepDataListMessage> corruptor) throws Exception
    {
       DRCRobotModel robotModel = getRobotModel();
       double actualFootLength = robotModel.getWalkingControllerParameters().getSteppingParameters().getActualFootLength();
       double startX = goingUp ? 0.0 : 1.2 + numberOfSteps * stepLength + 0.3;
       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))

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

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<RigidBodyControlManager> 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;
+   }
 }

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

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