diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndClearDelayQueueMessageTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndClearDelayQueueMessageTest.java index 2ab41400221..3994a9cc235 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndClearDelayQueueMessageTest.java +++ b/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(); diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndPelvisOrientationTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndPelvisOrientationTest.java index 2d3fff719c2..6ac49a21fdd 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/controllerAPI/EndToEndPelvisOrientationTest.java +++ b/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()); diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/obstacleCourseTests/AvatarBigStepDownTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/obstacleCourseTests/AvatarBigStepDownTest.java index f34eb594fd9..a1fde603815 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/obstacleCourseTests/AvatarBigStepDownTest.java +++ b/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,7 +79,10 @@ 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); @@ -85,7 +90,7 @@ public void testWalkingOffOfLargePlatform() 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() { diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/EndToEndCinderBlockFieldTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/EndToEndCinderBlockFieldTest.java index 98376764d71..8e6779f3c84 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/EndToEndCinderBlockFieldTest.java +++ b/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()); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/SwingTrajectoryParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/SwingTrajectoryParameters.java index c2193f659cc..103df7a771f 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/SwingTrajectoryParameters.java +++ b/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; } /** diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/YoSwingTrajectoryParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/YoSwingTrajectoryParameters.java index a3f02f84c1a..91d5b477a5c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/YoSwingTrajectoryParameters.java +++ b/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(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/SwingTrajectoryCalculator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/SwingTrajectoryCalculator.java index 1cbb02e268d..54680da206f 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/SwingTrajectoryCalculator.java +++ b/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());