added an additional waypoint to prevent the toe from stubbing

ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndClearDelayQueueMessageTest.java

@@ -1,7 +1,5 @@
 package us.ihmc.avatar.controllerAPI;
 
-import static us.ihmc.robotics.Assert.assertEquals;
-import static us.ihmc.robotics.Assert.assertTrue;
 
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
@@ -29,6 +27,8 @@
 import us.ihmc.tools.MemoryTools;
 import us.ihmc.yoVariables.variable.YoVariable;
 
+import static org.junit.jupiter.api.Assertions.*;
+
 public abstract class EndToEndClearDelayQueueMessageTest implements MultiRobotTestInterface
 {
    private static final SimulationTestingParameters simulationTestingParameters = SimulationTestingParameters.createFromSystemProperties();

ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndPelvisOrientationTest.java

@@ -100,7 +100,7 @@ public void testGoHome()
 
       GoHomeMessage goHomeMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.PELVIS, trajectoryTime);
       simulationTestHelper.publishToController(goHomeMessage);
-      assertTrue(simulationTestHelper.simulateNow(trajectoryTime + 0.5));
+      assertTrue(simulationTestHelper.simulateNow(trajectoryTime + 0.75));
 
       humanoidReferenceFrames.updateFrames();
       FrameQuaternion homeOrientation = new FrameQuaternion(midFootZUpGroundFrame, new Quaternion());

ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/obstacleCourseTests/AvatarBigStepDownTest.java

@@ -22,10 +22,12 @@
 import us.ihmc.commons.MathTools;
 import us.ihmc.commons.thread.ThreadTools;
 import us.ihmc.euclid.geometry.BoundingBox3D;
+import us.ihmc.euclid.tools.EuclidCoreTools;
 import us.ihmc.euclid.tuple3D.Point3D;
 import us.ihmc.euclid.tuple3D.Vector3D;
 import us.ihmc.euclid.tuple4D.Quaternion;
 import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.simulationConstructionSetTools.tools.CITools;
 import us.ihmc.simulationconstructionset.util.simulationTesting.SimulationTestingParameters;
@@ -77,15 +79,18 @@ public void testWalkingOffOfLargePlatform()
       simulationTestHelperFactory.setStartingLocationOffset(selectedLocation.getStartingLocationOffset());
       simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation();
       simulationTestHelper.start(false);
+      boolean success = simulationTestHelper.simulateNow(0.1);
+
       ((YoBoolean) simulationTestHelper.getControllerRegistry().findVariable("doToeOffIfPossibleInSingleSupport")).set(true);
+      publishHeightOffset(-0.05);
 
       Point3D cameraFix = new Point3D(-4.68, -7.8, 0.55);
       Point3D cameraPosition = new Point3D(-8.6, -4.47, 0.58);
 
       simulationTestHelper.setCamera(cameraFix, cameraPosition);
 
       ThreadTools.sleep(1000);
-      boolean success = simulationTestHelper.simulateNow(2.0);
+      success &= simulationTestHelper.simulateNow(0.5);
 
       Quaternion footRotation = new Quaternion();
       footRotation.setToYawOrientation(selectedLocation.getStartingLocationOffset().getYaw());
@@ -125,6 +130,16 @@ public void testWalkingOffOfLargePlatform()
       CITools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
    }
 
+   private void publishHeightOffset(double heightOffset)
+   {
+      if (!Double.isFinite(heightOffset) || EuclidCoreTools.epsilonEquals(0.0, heightOffset, 1.0e-3))
+         return;
+      MovingReferenceFrame rootJointFrame = simulationTestHelper.getControllerFullRobotModel().getRootJoint().getFrameAfterJoint();
+      double z = rootJointFrame.getTransformToRoot().getTranslationZ();
+      simulationTestHelper.publishToController(HumanoidMessageTools.createPelvisHeightTrajectoryMessage(0.5, z + heightOffset));
+      Assertions.assertTrue(simulationTestHelper.simulateNow(0.5));
+   }
+
    @Test
    public void testSplitFractionInBigStepDown()
    {

ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/EndToEndCinderBlockFieldTest.java

@@ -175,6 +175,7 @@ public void testSteppingStonesB() throws Exception
       walkOverSlantedCinderBlockField(cinderBlockFieldEnvironment, footsteps);
    }
 
+   @Test
    public void testEastHillMountain()
    {
       CITools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/SwingTrajectoryParameters.java

@@ -242,13 +242,22 @@ public boolean addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown()
       return false;
    }
 
+   /**
+    * When {@link #addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown()} is true, this specifies the fraction
+    * through swing to add a normal waypoint before adding a foot pitch waypoint, so that the toes can clear any obstacles
+    */
+   public double getFractionOfSwingToPitchFootNormal()
+   {
+      return Math.min(0.2, getFractionOfSwingToPitchFootDown() - 0.1);
+   }
+
    /**
     * When {@link #addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown()} is true, this specifies the fraction
     * through swing to add the additional foot pitch waypoint
     */
    public double getFractionOfSwingToPitchFootDown()
    {
-      return 0.4;
+      return 0.6;
    }
 
    /**

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/YoSwingTrajectoryParameters.java

@@ -21,6 +21,7 @@ public class YoSwingTrajectoryParameters
    private final DoubleParameter defaultSwingStepDownHeight;
    private final BooleanParameter addOrientationMidpointForClearance;
    private final BooleanParameter addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown;
+   private final DoubleParameter fractionOfSwingToPitchFootNormal;
    private final DoubleParameter fractionOfSwingToPitchFootDown;
    private final DoubleParameter footPitchAngleToAvoidHeelStrike;
    private final DoubleParameter midpointOrientationInterpolationForClearance;
@@ -71,6 +72,9 @@ public YoSwingTrajectoryParameters(String namePrefix,
       addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown = new BooleanParameter(namePrefix + "AddFootPitchToAvoidHeelStrikeWhenSteppingDown",
                                                                                      registry,
                                                                                      parameters.addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown());
+      fractionOfSwingToPitchFootNormal = new DoubleParameter(namePrefix + "FractionOfSwingToPitchFootNormal",
+                                                           registry,
+                                                           parameters.getFractionOfSwingToPitchFootNormal());
       fractionOfSwingToPitchFootDown = new DoubleParameter(namePrefix + "FractionOfSwingToPitchFootDown",
                                                            registry,
                                                            parameters.getFractionOfSwingToPitchFootDown());
@@ -164,6 +168,11 @@ public boolean addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown()
       return addFootPitchToAvoidHeelStrikeWhenSteppingForwardAndDown.getValue();
    }
 
+   public double getFractionOfSwingToPitchFootNormal()
+   {
+      return fractionOfSwingToPitchFootNormal.getValue();
+   }
+
    public double getFractionOfSwingToPitchFootDown()
    {
       return fractionOfSwingToPitchFootDown.getValue();

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/SwingTrajectoryCalculator.java

@@ -492,14 +492,25 @@ private void setTrajectoryFromOptimizer(boolean initializeOptimizer)
          tmpOrientation.setToZero(worldFrame);
          tmpVector.setToZero(worldFrame);
          double remainingPitch = swingTrajectoryParameters.getFootPitchAngleToAvoidHeelStrike() - initialOrientation.getPitch();
+         // if the foot is already pitched down sufficiently at the initial orientation, we don't need to add a midpoint, as it will maintain the desired pitch
+         // the whole time.
          if (remainingPitch > 0.0)
          {
             // compute the linear velocity to go from A to B, and then zero out the pitch velocity.
             tmpOrientation.difference(initialOrientation, finalOrientation);
             tmpOrientation.getRotationVector(tmpVector);
             tmpVector.scale(1.0 / swingDuration.getDoubleValue());
+
+            // add a lifting waypoint that has a smooth yaw, roll, and pitch, so that the foot doesn't aggressively start to pitch down in the initial phase
+            // of swing. This is useful to keep from stubbing the toe when trying to clear ground when the foot is initially pitched up.
+            double initialLiftFraction = swingTrajectoryParameters.getFractionOfSwingToPitchFootNormal();
+            tmpOrientation.interpolate(initialOrientation, finalOrientation, initialLiftFraction);
+            swingTrajectory.appendOrientationWaypoint(initialLiftFraction * swingDuration.getDoubleValue(), tmpOrientation, tmpVector);
+
+            // now zero the final pitch velocity out, because we want it to be stationary when the foot is pitched down.
             tmpVector.setY(0.0);
 
+            // interpolate so that the pitch at the fraction is the final pitch, but the yaw and roll are not.
             tmpOrientation.interpolate(initialOrientation, finalOrientation, swingTrajectoryParameters.getFractionOfSwingToPitchFootDown());
             tmpOrientation.setYawPitchRoll(tmpOrientation.getYaw(), swingTrajectoryParameters.getFootPitchAngleToAvoidHeelStrike(), tmpOrientation.getRoll());