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/capturePoint/ContactStateManager.java

@@ -114,7 +114,6 @@ public void initialize()
 
    public void initializeForStanding()
    {
-
       inSingleSupport.set(false);
       inStanding.set(true);
 
@@ -136,6 +135,7 @@ public void initializeForStanding()
    public void initializeForTransfer(double transferDuration, double swingDuration)
    {
       timeAtStartOfSupportSequence.set(time.getValue());
+
       timeInSupportSequence.set(0.0);
       currentStateDuration.set(transferDuration);
       totalStateDuration.set(transferDuration + swingDuration);
@@ -159,14 +159,14 @@ public void initializeForSingleSupport(double transferDuration, double swingDura
 
       double stepDuration = transferDuration + swingDuration;
       timeAtStartOfSupportSequence.set(time.getValue() - transferDuration);
+
       timeInSupportSequence.set(transferDuration);
       currentStateDuration.set(stepDuration);
       totalStateDuration.set(stepDuration);
       remainingTimeInContactSequence.set(swingDuration);
       adjustedRemainingTimeUnderDisturbance.set(swingDuration);
       offsetTimeInState.set(transferDuration);
 
-
       adjustedTimeInSupportSequence.set(transferDuration);
       totalTimeAdjustment.set(0.0);
 
@@ -176,6 +176,7 @@ public void initializeForSingleSupport(double transferDuration, double swingDura
    public void initializeForTransferToStanding(double finalTransferDuration )
    {
       timeAtStartOfSupportSequence.set(time.getValue());
+
       timeInSupportSequence.set(0.0);
       currentStateDuration.set(finalTransferDuration);
       totalStateDuration.set(finalTransferDuration);

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java

@@ -417,8 +417,7 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly
 
       UnrolledInverseFromMinor_DDRM.inv(transformedGains, inverseTransformedGains);
 
-      solver.resetCoPFeedbackConditions();
-      solver.resetFeedbackDirection();
+      solver.resetFeedbackConditions();
       solver.setFeedbackConditions(scaledCoPFeedbackWeight, transformedGains, dynamicsObjectiveWeight.getValue());
       solver.setMaxCMPDistanceFromEdge(maxAllowedDistanceCMPSupport.getValue());
       solver.setCopSafeDistanceToEdge(safeCoPDistanceToEdge.getValue());
@@ -442,6 +441,11 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly
 
          solver.setMaximumFeedbackRate(feedbackGains.getFeedbackPartMaxRate(), controlDT);
       }
+      else
+      {
+         solver.removeMaximumFeedbackMagnitude();
+         solver.removeFeedbackRateLimit();
+      }
       ignoreRateLimitThisTick = false;
 
       double feedbackRateWeight = usingHighCoPDamping.getValue() ? highlyDampedFeedbackRateWeight.getValue() : this.feedbackRateWeight.getValue();

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java

@@ -294,6 +294,18 @@ public void setMaximumFeedbackRate(double maximumFeedbackRate, double controlDT)
       this.controlDT = controlDT;
    }
 
+   public void removeFeedbackRateLimit()
+   {
+      this.maximumFeedbackRate = Double.POSITIVE_INFINITY;
+   }
+
+   public void removeMaximumFeedbackMagnitude()
+   {
+      CommonOps_DDRM.setIdentity(maxFeedbackTransform);
+      maxFeedbackXMagnitude = Double.POSITIVE_INFINITY;
+      maxFeedbackYMagnitude = Double.POSITIVE_INFINITY;
+   }
+
    /**
     * Zeros all the pertinent scalars, vectors, and matrices for the solver. Should be called at the beginning of every computation tick.
     */
@@ -357,6 +369,15 @@ private void reshape()
       solution.reshape(problemSize, 1);
    }
 
+   public void resetFeedbackConditions()
+   {
+      resetCoPFeedbackConditions();
+      resetCMPFeedbackConditions();
+      resetFeedbackDirection();
+      removeFeedbackRateLimit();
+      removeMaximumFeedbackMagnitude();
+   }
+
    /**
     * Resets the controller conditions on the feedback minimization task, the feedback gains, and the dynamic relaxation minimization task.
     * Also sets that the controller is not to attempt to regularize the feedback minimization task.