From 716b4a45b536788dcf743d3e6b950fdaf0e9a947 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 14 Feb 2025 16:17:28 -0600 Subject: [PATCH 01/17] made ALIP tools class with static methods for ALIP calculations and control --- .../ALIPCalculatorTools.java | 137 ++++++++++++++++++ .../QuicksterFootstepProvider.java | 38 +++-- 2 files changed, 164 insertions(+), 11 deletions(-) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java new file mode 100644 index 00000000000..d6b0c58cae5 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -0,0 +1,137 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; + +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint2DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.robotics.robotSide.RobotSide; + +public class ALIPCalculatorTools +{ + private final static double GRAVITY = -9.81; + + private final static FramePoint3D tempPosition = new FramePoint3D(); + private final static FrameVector3D tempAngularMomentum = new FrameVector3D(); + private final static FrameVector3D tempAngularMomentum2 = new FrameVector3D(); + private final static FrameVector3D tempVelocity = new FrameVector3D(); + private final static FrameVector3D tempCentroidalAngularMomentum = new FrameVector3D(); + + public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentPosition, + FrameVector3DReadOnly currentAngularMomentum, + FramePoint3D futurePositionToPack, + FrameVector3D futureAngularMomentumToPack, + double deltaT, + double pendulumMass, + double pendulumHeight, + ReferenceFrame stanceFootFrame) + { + // Use future position/angular momentum as placeholder variables to make sure current position/angular momentum are in correct frame + futurePositionToPack.setMatchingFrame(currentPosition); + futurePositionToPack.changeFrame(stanceFootFrame); + + futureAngularMomentumToPack.setMatchingFrame(currentAngularMomentum); + futureAngularMomentumToPack.changeFrame(stanceFootFrame); + + double omega = calculateOmega(pendulumHeight); + + // Current position and angular momentum + double x0 = futurePositionToPack.getX(); + double Ly0 = futureAngularMomentumToPack.getY(); + + double y0 = futurePositionToPack.getY(); + double Lx0 = futureAngularMomentumToPack.getX(); + + // Final position and angular momentum + double xf = x0 * Math.cosh(omega * deltaT) + Ly0 * Math.sinh(omega * deltaT) / (pendulumMass * pendulumHeight * omega); + double Lyf = x0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * deltaT) + Ly0 * Math.cosh(omega * deltaT); + + double yf = y0 * Math.cosh(omega * deltaT) - Lx0 * Math.sinh(omega * deltaT) / (pendulumMass * pendulumHeight * omega); + double Lxf = -y0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * deltaT) + Lx0 * Math.cosh(omega * deltaT); + + futurePositionToPack.setX(xf); + futurePositionToPack.setY(yf); + + futureAngularMomentumToPack.setX(Lxf); + futureAngularMomentumToPack.setY(Lyf); + } + + public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentPosition, + FrameVector3DReadOnly currentVelocity, + FrameVector3DReadOnly currentCentroidalAngularMomentum, + RobotSide swingSide, + double desiredVelocityX, + double desiredVelocityY, + double desiredStanceWidth, + double timeRemainingInCurrentStep, + double stepDuration, + double pendulumMass, + double pendulumHeight, + FramePoint2DBasics touchdownPositionToPack, + ReferenceFrame stanceFootFrame, + ReferenceFrame swingFootFrame, + ReferenceFrame controlFrame) + { + double omega = calculateOmega(pendulumHeight); + + double LyDesired = pendulumMass * desiredVelocityX * pendulumHeight; + double LxDesired = computeDesiredAngularMomentumForStanceWidth(swingSide, desiredStanceWidth, pendulumMass, pendulumHeight, stepDuration) -pendulumMass * desiredVelocityY * pendulumHeight; + + ////// + // Get CoM velocity and change frame to CoM control frame + tempVelocity.setIncludingFrame(currentVelocity); + tempVelocity.changeFrame(stanceFootFrame); + + // Get CoM angular momentum and change frame to CoM control frame + tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); + tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); + ///// + + tempAngularMomentum2.setToZero(stanceFootFrame); + tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); + tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); + computeFutureStateUsingALIP(currentPosition, tempAngularMomentum2, tempPosition, tempAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); + + double LyEndOfCurrentStep = tempAngularMomentum.getY(); + double LxEndOfCurrentStep = tempAngularMomentum.getX(); + + double desiredFootstepPositionX = (LyDesired - Math.cosh(omega * stepDuration) * LyEndOfCurrentStep) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); + double desiredFootstepPositionY = (Math.cosh(omega * stepDuration) * LxEndOfCurrentStep - LxDesired) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); + + ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); + touchdownPositionToPack.setToZero(controlFrame); + touchdownPositionToPack.set(-desiredFootstepPositionX, -desiredFootstepPositionY); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); + } + + public static double computeDesiredAngularMomentumForStanceWidth(RobotSide swingSide, double desiredStanceWidth, double pendulumMass, double pendulumHeight, double stepDuration) + { + double sideSignMultiplier = swingSide == RobotSide.LEFT ? 1.0 : -1.0; + double omega = calculateOmega(pendulumHeight); + return sideSignMultiplier * 0.5 * pendulumMass * pendulumHeight * desiredStanceWidth * (omega * Math.sinh(omega * stepDuration)) / (1 + Math.cosh(omega * stepDuration)); + } + + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, FramePoint3D touchdownPositionToPack, double pole, double deltaT, double omega) + { + computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, deltaT, omega), velocity, touchdownPositionToPack); + } + + public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, FramePoint3D touchdownPositionToPack) + { + velocity.checkReferenceFrameMatch(touchdownPositionToPack); + touchdownPositionToPack.setAndScale(timeConstant, velocity); + } + + public static double calculateTimeConstantUsingPolePlacement(double pole, double deltaT, double omega) + { + return (Math.cosh(omega * deltaT) - pole) / (omega * Math.sinh(omega * deltaT)); + } + + public static double calculateOmega(double height) + { + return Math.sqrt(Math.abs(GRAVITY / height)); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 6da37a185b0..9427b76b603 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -257,18 +257,34 @@ public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalcula FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean isInDoubleSupport, - FixedFramePoint2DBasics touchdownPositionToPack) + FramePoint2DBasics touchdownPositionToPack) { - tempPendulumBase.setIncludingFrame(pendulumBase); - tempPendulumBase.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - - tempNetPendulumBase.setIncludingFrame(netPendulumBase); - tempNetPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(getTrailingSide())); - - touchdownCalculator.computeDesiredTouchdownPosition(swingSide.getOppositeSide(), timeToReachGoal, tempPendulumBase, tempNetPendulumBase, isInDoubleSupport); - tempTouchdownPosition.setIncludingFrame(touchdownCalculator.getDesiredTouchdownPosition2D()); - tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); - touchdownPositionToPack.set(tempTouchdownPosition); +// tempPendulumBase.setIncludingFrame(pendulumBase); +// tempPendulumBase.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); +// +// tempNetPendulumBase.setIncludingFrame(netPendulumBase); +// tempNetPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(getTrailingSide())); +// +// touchdownCalculator.computeDesiredTouchdownPosition(swingSide.getOppositeSide(), timeToReachGoal, tempPendulumBase, tempNetPendulumBase, isInDoubleSupport); +// tempTouchdownPosition.setIncludingFrame(touchdownCalculator.getDesiredTouchdownPosition2D()); +// tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); +// touchdownPositionToPack.set(tempTouchdownPosition); + + ALIPCalculatorTools.computeTouchdownPositionRegular(estimates.getCenterOfMassPosition(), + estimates.getCenterOfMassVelocity(), + estimates.getCenterOfMassAngularMomentum(), + swingSide, + desiredVelocity.getX(), + desiredVelocity.getY(), + parameters.getStanceWidth(swingSide).getDoubleValue(), + timeToReachGoal, + getStepDuration(swingSide), + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + touchdownPositionToPack, + referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), + referenceFrames.getSoleZUpFrame(swingSide), + centerOfMassControlZUpFrame); } private void calculateNetPendulumBase() From 47619a34edf80749c7edf6049ca1da61e52ade4f Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 18 Feb 2025 17:56:21 -0600 Subject: [PATCH 02/17] added raibert heuristic controller to ALIP tools --- .../ALIPCalculatorTools.java | 124 ++++++++++++++++-- .../QuicksterFootstepProvider.java | 4 +- 2 files changed, 116 insertions(+), 12 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index d6b0c58cae5..3ff88d0b1cc 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -4,10 +4,8 @@ import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.robotics.robotSide.RobotSide; public class ALIPCalculatorTools @@ -107,6 +105,75 @@ public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentP touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePoint3DReadOnly currentPosition, + FrameVector3DReadOnly currentVelocity, + FrameVector3DReadOnly currentCentroidalAngularMomentum, + RobotSide swingSide, + double desiredVelocityX, + double desiredVelocityY, + double desiredStanceWidth, + double timeRemainingInCurrentStep, + double stepDuration, + double doubleSupportDuration, + double pendulumMass, + double pendulumHeight, + double pole, + FramePoint2DBasics touchdownPositionToPack, + ReferenceFrame stanceFootFrame, + ReferenceFrame swingFootFrame, + ReferenceFrame controlFrame) + { + double omega = calculateOmega(pendulumHeight); + + ////// + // Get CoM velocity and change frame to CoM control frame + tempVelocity.setIncludingFrame(currentVelocity); + tempVelocity.changeFrame(stanceFootFrame); + + // Get CoM angular momentum and change frame to CoM control frame + tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); + tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); + ///// + + tempAngularMomentum2.setToZero(stanceFootFrame); + tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); + tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); + computeFutureStateUsingALIP(currentPosition, tempAngularMomentum2, tempPosition, tempAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); + + tempVelocity.setX(tempAngularMomentum.getY() / (pendulumMass * pendulumHeight)); + tempVelocity.setY(-tempAngularMomentum.getX() / (pendulumMass * pendulumHeight)); + + computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempVelocity, touchdownPositionToPack, pole, stepDuration, omega, controlFrame); + + double swingDuration = stepDuration - doubleSupportDuration; + + ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrame); + touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, omega, desiredVelocityX)); + touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, swingSide.getOppositeSide(), omega, desiredVelocityY)); + touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, doubleSupportDuration, desiredStanceWidth, swingSide.getOppositeSide(), omega)); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); + } + + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, FramePoint2DBasics touchdownPositionToPack, double pole, double stepDuration, double omega, ReferenceFrame controlFrame) + { + computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), velocity, touchdownPositionToPack, controlFrame); + } + + public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, FramePoint2DBasics touchdownPositionToPack, ReferenceFrame controlFrame) + { + ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrame); + touchdownPositionToPack.set(velocity.getX(), velocity.getY()); + touchdownPositionToPack.scale(timeConstant); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); + } + + public static double calculateTimeConstantUsingPolePlacement(double pole, double stepDuration, double omega) + { + return (Math.cosh(omega * stepDuration) - pole) / (omega * Math.sinh(omega * stepDuration)); + } + public static double computeDesiredAngularMomentumForStanceWidth(RobotSide swingSide, double desiredStanceWidth, double pendulumMass, double pendulumHeight, double stepDuration) { double sideSignMultiplier = swingSide == RobotSide.LEFT ? 1.0 : -1.0; @@ -114,20 +181,55 @@ public static double computeDesiredAngularMomentumForStanceWidth(RobotSide swing return sideSignMultiplier * 0.5 * pendulumMass * pendulumHeight * desiredStanceWidth * (omega * Math.sinh(omega * stepDuration)) / (1 + Math.cosh(omega * stepDuration)); } - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, FramePoint3D touchdownPositionToPack, double pole, double deltaT, double omega) + private static double computeForwardTouchdownOffsetForVelocity(double swingDuration, double doubleSupportDuration, double omega, double desiredVelocity) { - computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, deltaT, omega), velocity, touchdownPositionToPack); + double stepDuration = doubleSupportDuration + swingDuration; + double exponential = Math.exp(omega * stepDuration); + double forwardMultiplier = 1.0; + if (doubleSupportDuration > 0.0) + { + forwardMultiplier += (Math.exp(omega * doubleSupportDuration) - 1.0) / (omega * doubleSupportDuration) - 1.0; + } + + return -forwardMultiplier * desiredVelocity * stepDuration / (exponential - 1.0); } - public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, FramePoint3D touchdownPositionToPack) + private static double computeLateralTouchdownOffsetForVelocity(double swingDuration, + double doubleSupportDuration, + RobotSide supportSide, + double omega, + double desiredVelocity) { - velocity.checkReferenceFrameMatch(touchdownPositionToPack); - touchdownPositionToPack.setAndScale(timeConstant, velocity); + double stepDuration = doubleSupportDuration + swingDuration; + double exponential = Math.exp(omega * stepDuration); + + double lateralOffset = 0.0; + if (desiredVelocity > 0.0 && supportSide == RobotSide.LEFT) + { + lateralOffset = -(desiredVelocity * 2.0 * swingDuration) / (exponential - 1.0); + } + else if (desiredVelocity < 0.0 && supportSide == RobotSide.RIGHT) + { + lateralOffset = -(desiredVelocity * 2.0 * swingDuration) / (exponential - 1.0); + } + + return lateralOffset; } - public static double calculateTimeConstantUsingPolePlacement(double pole, double deltaT, double omega) + public static double computeDesiredTouchdownOffsetForStanceWidth(double swingDuration, + double doubleSupportDuration, + double stanceWidth, + RobotSide supportSide, + double omega) { - return (Math.cosh(omega * deltaT) - pole) / (omega * Math.sinh(omega * deltaT)); + double widthMultiplier = 1.0; + if (doubleSupportDuration > 0.0) + { + widthMultiplier += (Math.exp(omega * doubleSupportDuration) - 1.0) / (omega * doubleSupportDuration) - 1.0; + } + // this is the desired offset distance to make the robot stably walk in place at our desired step width + double stepWidthOffset = widthMultiplier * stanceWidth / (1.0 + Math.exp(omega * (swingDuration + doubleSupportDuration))); + return supportSide.negateIfLeftSide(stepWidthOffset); } public static double calculateOmega(double height) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 9427b76b603..6e5e6e6d2c0 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -270,7 +270,7 @@ public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalcula // tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); // touchdownPositionToPack.set(tempTouchdownPosition); - ALIPCalculatorTools.computeTouchdownPositionRegular(estimates.getCenterOfMassPosition(), + ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(estimates.getCenterOfMassPosition(), estimates.getCenterOfMassVelocity(), estimates.getCenterOfMassAngularMomentum(), swingSide, @@ -279,8 +279,10 @@ public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalcula parameters.getStanceWidth(swingSide).getDoubleValue(), timeToReachGoal, getStepDuration(swingSide), + getTransferDuration(swingSide), robotModel.getTotalMass(), parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + parameters.getPole(swingSide).getDoubleValue(), touchdownPositionToPack, referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), referenceFrames.getSoleZUpFrame(swingSide), From 3fc6f1aec9a014c68e909be5228a3a3fd264fa65 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 18 Feb 2025 22:53:32 -0600 Subject: [PATCH 03/17] made full footstepDataList calculated using QFP, not fully working properly yet --- .../ContinuousStepGenerator.java | 43 +++--- .../ALIPCalculatorTools.java | 55 ++++---- .../QuicksterFootstepProvider.java | 126 ++++++++++++++---- .../QuicksterFootstepProviderEstimates.java | 10 ++ 4 files changed, 159 insertions(+), 75 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index 82c1f0acdaa..7521752cd06 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -343,7 +343,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD) quicksterFootstepProvider.get().initialize(); - quicksterFootstepProvider.get().update(time); + quicksterFootstepProvider.get().update(parameters.getNumberOfFootstepsToPlan()); } for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++) @@ -351,26 +351,27 @@ else if (statusToProcess == FootstepStatus.COMPLETED) if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) { // FIXME we want all steps in plan to be QFP eventually - if (i == startingIndexToAdjust) - { - quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D); - } - else - { - calculateNextFootstepPose2D(stepTime.getValue(), - desiredVelocityX, - desiredVelocityY, - desiredTurningVelocity.getDoubleValue(), - swingSide, - maxStepLength, - maxStepWidth, - defaultStepWidth, - minStepWidth, - turnMaxAngleInward, - turnMaxAngleOutward, - footstepPose2D, - nextFootstepPose2D); - } + quicksterFootstepProvider.get().getDesiredTouchdownPose(i, nextFootstepPose2D); +// if (i == startingIndexToAdjust) +// { +// quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D); +// } +// else +// { +// calculateNextFootstepPose2D(stepTime.getValue(), +// desiredVelocityX, +// desiredVelocityY, +// desiredTurningVelocity.getDoubleValue(), +// swingSide, +// maxStepLength, +// maxStepWidth, +// defaultStepWidth, +// minStepWidth, +// turnMaxAngleInward, +// turnMaxAngleOutward, +// footstepPose2D, +// nextFootstepPose2D); +// } } else { diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 3ff88d0b1cc..23d5ab1c5e2 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -1,20 +1,18 @@ package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; -import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.*; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.robotics.robotSide.RobotSide; public class ALIPCalculatorTools { private final static double GRAVITY = -9.81; - private final static FramePoint3D tempPosition = new FramePoint3D(); - private final static FrameVector3D tempAngularMomentum = new FrameVector3D(); - private final static FrameVector3D tempAngularMomentum2 = new FrameVector3D(); + private final static FramePoint3D tempFuturePosition = new FramePoint3D(); + private final static FrameVector3D tempFutureContactPointAngularMomentum = new FrameVector3D(); + private final static FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final static FrameVector3D tempVelocity = new FrameVector3D(); private final static FrameVector3D tempCentroidalAngularMomentum = new FrameVector3D(); @@ -88,13 +86,13 @@ public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentP tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); ///// - tempAngularMomentum2.setToZero(stanceFootFrame); - tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); - tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); - computeFutureStateUsingALIP(currentPosition, tempAngularMomentum2, tempPosition, tempAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); + tempCurrentContactPointAngularMomentum.setToZero(stanceFootFrame); + tempCurrentContactPointAngularMomentum.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); + tempCurrentContactPointAngularMomentum.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); + computeFutureStateUsingALIP(currentPosition, tempCurrentContactPointAngularMomentum, tempFuturePosition, tempFutureContactPointAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); - double LyEndOfCurrentStep = tempAngularMomentum.getY(); - double LxEndOfCurrentStep = tempAngularMomentum.getX(); + double LyEndOfCurrentStep = tempFutureContactPointAngularMomentum.getY(); + double LxEndOfCurrentStep = tempFutureContactPointAngularMomentum.getX(); double desiredFootstepPositionX = (LyDesired - Math.cosh(omega * stepDuration) * LyEndOfCurrentStep) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); double desiredFootstepPositionY = (Math.cosh(omega * stepDuration) * LxEndOfCurrentStep - LxDesired) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); @@ -106,8 +104,7 @@ public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentP } public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePoint3DReadOnly currentPosition, - FrameVector3DReadOnly currentVelocity, - FrameVector3DReadOnly currentCentroidalAngularMomentum, + FrameVector3DReadOnly currentContactPointAngularMomentum, RobotSide swingSide, double desiredVelocityX, double desiredVelocityY, @@ -120,28 +117,32 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement double pole, FramePoint2DBasics touchdownPositionToPack, ReferenceFrame stanceFootFrame, - ReferenceFrame swingFootFrame, ReferenceFrame controlFrame) { double omega = calculateOmega(pendulumHeight); - ////// - // Get CoM velocity and change frame to CoM control frame - tempVelocity.setIncludingFrame(currentVelocity); - tempVelocity.changeFrame(stanceFootFrame); +// ////// +// // Get CoM velocity and change frame to CoM control frame +// tempVelocity.setIncludingFrame(currentVelocity); +// tempVelocity.changeFrame(stanceFootFrame); +// +// // Get CoM angular momentum and change frame to CoM control frame +// tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); +// tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); +// ///// +// +// tempAngularMomentum2.setToZero(stanceFootFrame); +// tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); +// tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); // Get CoM angular momentum and change frame to CoM control frame - tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); - tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); - ///// + tempCurrentContactPointAngularMomentum.setIncludingFrame(currentContactPointAngularMomentum); + tempCurrentContactPointAngularMomentum.changeFrame(stanceFootFrame); - tempAngularMomentum2.setToZero(stanceFootFrame); - tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); - tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); - computeFutureStateUsingALIP(currentPosition, tempAngularMomentum2, tempPosition, tempAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); + computeFutureStateUsingALIP(currentPosition, tempCurrentContactPointAngularMomentum, tempFuturePosition, tempFutureContactPointAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); - tempVelocity.setX(tempAngularMomentum.getY() / (pendulumMass * pendulumHeight)); - tempVelocity.setY(-tempAngularMomentum.getX() / (pendulumMass * pendulumHeight)); + tempVelocity.setX(tempFutureContactPointAngularMomentum.getY() / (pendulumMass * pendulumHeight)); + tempVelocity.setY(-tempFutureContactPointAngularMomentum.getX() / (pendulumMass * pendulumHeight)); computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempVelocity, touchdownPositionToPack, pole, stepDuration, omega, controlFrame); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 6e5e6e6d2c0..52bfa6fd280 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -4,11 +4,9 @@ import us.ihmc.commonWalkingControlModules.controllers.Updatable; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.*; import us.ihmc.commons.MathTools; +import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePose2D; -import us.ihmc.euclid.referenceFrame.FrameQuaternion; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.*; import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; @@ -35,7 +33,7 @@ import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoEnum; -public class QuicksterFootstepProvider implements Updatable +public class QuicksterFootstepProvider { private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); @@ -59,6 +57,10 @@ public class QuicksterFootstepProvider implements Updatable private final SideDependentList desiredTouchdownPoses = new SideDependentList<>(); private final SideDependentList desiredTouchdownPosition3DInWorld = new SideDependentList<>(); + // + private final RecyclingArrayList desiredTouchdownPositionsList = new RecyclingArrayList<>(FramePoint2D.class); + private final RecyclingArrayList desiredTouchdownPosesList = new RecyclingArrayList<>(FramePose2D.class); + // Desired inputs private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); private final YoVector2D desiredVelocity = new YoVector2D("desiredVelocity" + variableNameSuffix, registry); @@ -209,12 +211,23 @@ public void initialize() firstTick = false; } - @Override - public void update(double time) + private double numberOfFootstepsToPlan; + public void update(int numberOfFootstepsToPlan) { if (firstTick) initialize(); +// desiredTouchdownPositionsList.clear(); +// desiredTouchdownPosesList.clear(); +// +// for (int i = 0; i< numberOfFootstepsToPlan ; i++) +// { +// desiredTouchdownPositionsList.add(); +// desiredTouchdownPosesList.add(); +// } + this.numberOfFootstepsToPlan = numberOfFootstepsToPlan; + + updateEstimates(); updateDesireds(); @@ -248,12 +261,22 @@ private void updateDesireds() desiredTouchdownPoses.get(robotSide).getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); } + desiredTouchdownPosesList.clear(); + for (int i = 0; i< desiredTouchdownPositionsList.size() ; i++) + { + desiredTouchdownPosesList.getAndGrowIfNeeded(i).getPosition().set(desiredTouchdownPositionsList.get(i)); + desiredTouchdownPosesList.getAndGrowIfNeeded(i).getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); + } + netPendulumBase3DInWorld.setMatchingFrame(netPendulumBase, 0.0); } + private final FramePoint3D tempPosition = new FramePoint3D(); + private final FrameVector3D tempAngularMomentum = new FrameVector3D(); + public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalculator touchdownCalculator, - RobotSide swingSide, - double timeToReachGoal, + RobotSide startingSwingSide, + double timeRemainingInCurrentStep, FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean isInDoubleSupport, @@ -270,23 +293,66 @@ public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalcula // tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); // touchdownPositionToPack.set(tempTouchdownPosition); - ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(estimates.getCenterOfMassPosition(), - estimates.getCenterOfMassVelocity(), - estimates.getCenterOfMassAngularMomentum(), - swingSide, - desiredVelocity.getX(), - desiredVelocity.getY(), - parameters.getStanceWidth(swingSide).getDoubleValue(), - timeToReachGoal, - getStepDuration(swingSide), - getTransferDuration(swingSide), - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - parameters.getPole(swingSide).getDoubleValue(), - touchdownPositionToPack, - referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), - referenceFrames.getSoleZUpFrame(swingSide), - centerOfMassControlZUpFrame); + desiredTouchdownPositionsList.clear(); + for (int i = 0; i < numberOfFootstepsToPlan ; i++) + { + FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.add(); + + FramePoint3DReadOnly currentCoMPosition; + FrameVector3DReadOnly currentAngularMomentum; + double timeToReachGoal; + RobotSide swingSide; + + if (i == 0) + { + currentCoMPosition = estimates.getCenterOfMassPosition(); + currentAngularMomentum = estimates.getContactPointAngularMomentum(); + swingSide = startingSwingSide; + timeToReachGoal = timeRemainingInCurrentStep; + } + else if (i % 2 == 0) + { + currentCoMPosition = tempPosition; + currentAngularMomentum = tempAngularMomentum; + swingSide = startingSwingSide; + timeToReachGoal = getSwingDuration(swingSide); + } + else + { + currentCoMPosition = tempPosition; + currentAngularMomentum = tempAngularMomentum; + swingSide = startingSwingSide.getOppositeSide(); + timeToReachGoal = getSwingDuration(swingSide); + } + + ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( + currentCoMPosition, + currentAngularMomentum, + swingSide, + desiredVelocity.getX(), + desiredVelocity.getY(), + parameters.getStanceWidth(swingSide).getDoubleValue(), + timeToReachGoal, + getStepDuration(swingSide), + getTransferDuration(swingSide), + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + parameters.getPole(swingSide).getDoubleValue(), + desiredTouchdownPositionToPack, + referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), + centerOfMassControlZUpFrame); + + ALIPCalculatorTools.computeFutureStateUsingALIP( + currentCoMPosition, + currentAngularMomentum, + tempPosition, + tempAngularMomentum, + timeToReachGoal, + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide())); + + } } private void calculateNetPendulumBase() @@ -425,7 +491,7 @@ public void setDesiredVelocityProvider(DesiredVelocityProvider desiredVelocityPr /** * Sets a provider that is to be used to update the state of {@link #walk} internally on each call - * to {@link #update(double)}. + * to {@link #update(int)}. * * @param walkInputProvider the provider used to determine whether to walk or not walk. */ @@ -469,6 +535,12 @@ public void getDesiredTouchdownPose(RobotSide robotSide, FramePose2D touchdownPo touchdownPoseToPack.setMatchingFrame(desiredTouchdownPoses.get(robotSide)); } + public void getDesiredTouchdownPose(int footstepIndex, FramePose2D touchdownPoseToPack) + { + // TODO 3D transform between two 2D poses? + touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(footstepIndex)); + } + public double getSwingDuration(RobotSide swingSide) { return parameters.getSwingDuration(swingSide).getDoubleValue(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java index c17026a95a2..4e3d7636a04 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java @@ -27,6 +27,7 @@ public class QuicksterFootstepProviderEstimates private final YoFrameVector3D currentCoMVelocity; private final YoFrameVector3D currentCoMLinearMomentum; private final YoFrameVector3D currentCoMAngularMomentum; + private final YoFrameVector3D currentContactPointAngularMomentum; // CoM Frames private final MovingReferenceFrame centerOfMassFrame; @@ -58,6 +59,7 @@ public QuicksterFootstepProviderEstimates(FullHumanoidRobotModel robotModel, currentCoMVelocity = new YoFrameVector3D("currentCoMVelocity" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); currentCoMLinearMomentum = new YoFrameVector3D("currentCoMLinearMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); currentCoMAngularMomentum = new YoFrameVector3D("currentCoMAngularMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + currentContactPointAngularMomentum = new YoFrameVector3D("currentContactPointAngularMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); centerOfMassFrame = (MovingReferenceFrame) referenceFrames.getCenterOfMassFrame(); @@ -109,6 +111,9 @@ public void update(RobotSide swingSide) angularExcursionCalculator.compute(); currentCoMLinearMomentum.setMatchingFrame(angularExcursionCalculator.getLinearMomentum()); currentCoMAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); + currentContactPointAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); + currentContactPointAngularMomentum.addX(-mass * parameters.getDesiredCoMHeight(swingSide).getDoubleValue() * currentCoMVelocity.getY()); + currentContactPointAngularMomentum.addY(mass * parameters.getDesiredCoMHeight(swingSide).getDoubleValue() * currentCoMVelocity.getX()); // Update CoM control frames centerOfMassControlFrame.update(); @@ -147,6 +152,11 @@ public FrameVector3DReadOnly getCenterOfMassAngularMomentum() return currentCoMAngularMomentum; } + public FrameVector3DReadOnly getContactPointAngularMomentum() + { + return currentContactPointAngularMomentum; + } + public MovingZUpFrame getCenterOfMassControlZUpFrame() { return centerOfMassControlZUpFrame; From 02bc3129b785d587f2e9757d217b9fd69b8e43cd Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 19 Feb 2025 14:12:37 -0600 Subject: [PATCH 04/17] removed reliance on reference frames in ALIP calculator tools class --- .../ContinuousStepGenerator.java | 42 ++-- .../ALIPCalculatorTools.java | 209 ++++++++++-------- .../QuicksterFootstepProvider.java | 143 +++++++----- 3 files changed, 220 insertions(+), 174 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index 7521752cd06..6ef634e7906 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -351,27 +351,27 @@ else if (statusToProcess == FootstepStatus.COMPLETED) if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) { // FIXME we want all steps in plan to be QFP eventually - quicksterFootstepProvider.get().getDesiredTouchdownPose(i, nextFootstepPose2D); -// if (i == startingIndexToAdjust) -// { -// quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D); -// } -// else -// { -// calculateNextFootstepPose2D(stepTime.getValue(), -// desiredVelocityX, -// desiredVelocityY, -// desiredTurningVelocity.getDoubleValue(), -// swingSide, -// maxStepLength, -// maxStepWidth, -// defaultStepWidth, -// minStepWidth, -// turnMaxAngleInward, -// turnMaxAngleOutward, -// footstepPose2D, -// nextFootstepPose2D); -// } +// quicksterFootstepProvider.get().getDesiredTouchdownPose(i, nextFootstepPose2D); + if (i == startingIndexToAdjust) + { + quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D); + } + else + { + calculateNextFootstepPose2D(stepTime.getValue(), + desiredVelocityX, + desiredVelocityY, + desiredTurningVelocity.getDoubleValue(), + swingSide, + maxStepLength, + maxStepWidth, + defaultStepWidth, + minStepWidth, + turnMaxAngleInward, + turnMaxAngleOutward, + footstepPose2D, + nextFootstepPose2D); + } } else { diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 23d5ab1c5e2..c2bacbe078d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -10,36 +10,48 @@ public class ALIPCalculatorTools { private final static double GRAVITY = -9.81; - private final static FramePoint3D tempFuturePosition = new FramePoint3D(); + private final static FramePoint3D tempFutureCoMPosition = new FramePoint3D(); + private final static FrameVector3D tempFutureCoMVelocity = new FrameVector3D(); private final static FrameVector3D tempFutureContactPointAngularMomentum = new FrameVector3D(); + + private final static FramePoint3D tempCurrentCoMPosition = new FramePoint3D(); private final static FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); - private final static FrameVector3D tempVelocity = new FrameVector3D(); + private final static FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); + private final static FrameVector3D tempCentroidalAngularMomentum = new FrameVector3D(); - public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentPosition, - FrameVector3DReadOnly currentAngularMomentum, + public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentCoMPosition, + FrameVector3DReadOnly currentContactPointAngularMomentum, + FramePoint3DReadOnly currentStanceFootPosition, FramePoint3D futurePositionToPack, FrameVector3D futureAngularMomentumToPack, double deltaT, double pendulumMass, - double pendulumHeight, - ReferenceFrame stanceFootFrame) + double pendulumHeight) { // Use future position/angular momentum as placeholder variables to make sure current position/angular momentum are in correct frame - futurePositionToPack.setMatchingFrame(currentPosition); - futurePositionToPack.changeFrame(stanceFootFrame); +// futurePositionToPack.setMatchingFrame(currentCoMPosition); +// futurePositionToPack.distance(stanceFootPosition); +// +// futureAngularMomentumToPack.setMatchingFrame(currentContactPointAngularMomentum); +// futureAngularMomentumToPack.(stanceFootFrame); - futureAngularMomentumToPack.setMatchingFrame(currentAngularMomentum); - futureAngularMomentumToPack.changeFrame(stanceFootFrame); + tempCurrentCoMPosition.setMatchingFrame(currentCoMPosition); + tempCurrentContactPointAngularMomentum.setMatchingFrame(currentContactPointAngularMomentum); + tempCurrentStanceFootPosition.setMatchingFrame(currentStanceFootPosition); + + tempCurrentCoMPosition.setX(tempCurrentCoMPosition.getX() - tempCurrentStanceFootPosition.getX()); + tempCurrentCoMPosition.setY(tempCurrentCoMPosition.getY() - tempCurrentStanceFootPosition.getY()); + tempCurrentCoMPosition.setZ(tempCurrentCoMPosition.getZ() - tempCurrentStanceFootPosition.getZ()); double omega = calculateOmega(pendulumHeight); // Current position and angular momentum - double x0 = futurePositionToPack.getX(); - double Ly0 = futureAngularMomentumToPack.getY(); + double x0 = tempCurrentCoMPosition.getX(); + double Ly0 = tempCurrentContactPointAngularMomentum.getY(); - double y0 = futurePositionToPack.getY(); - double Lx0 = futureAngularMomentumToPack.getX(); + double y0 = tempCurrentCoMPosition.getY(); + double Lx0 = tempCurrentContactPointAngularMomentum.getX(); // Final position and angular momentum double xf = x0 * Math.cosh(omega * deltaT) + Ly0 * Math.sinh(omega * deltaT) / (pendulumMass * pendulumHeight * omega); @@ -55,69 +67,69 @@ public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentPosit futureAngularMomentumToPack.setY(Lyf); } - public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentPosition, - FrameVector3DReadOnly currentVelocity, - FrameVector3DReadOnly currentCentroidalAngularMomentum, - RobotSide swingSide, - double desiredVelocityX, - double desiredVelocityY, - double desiredStanceWidth, - double timeRemainingInCurrentStep, - double stepDuration, - double pendulumMass, - double pendulumHeight, - FramePoint2DBasics touchdownPositionToPack, - ReferenceFrame stanceFootFrame, - ReferenceFrame swingFootFrame, - ReferenceFrame controlFrame) - { - double omega = calculateOmega(pendulumHeight); - - double LyDesired = pendulumMass * desiredVelocityX * pendulumHeight; - double LxDesired = computeDesiredAngularMomentumForStanceWidth(swingSide, desiredStanceWidth, pendulumMass, pendulumHeight, stepDuration) -pendulumMass * desiredVelocityY * pendulumHeight; - - ////// - // Get CoM velocity and change frame to CoM control frame - tempVelocity.setIncludingFrame(currentVelocity); - tempVelocity.changeFrame(stanceFootFrame); - - // Get CoM angular momentum and change frame to CoM control frame - tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); - tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); - ///// - - tempCurrentContactPointAngularMomentum.setToZero(stanceFootFrame); - tempCurrentContactPointAngularMomentum.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); - tempCurrentContactPointAngularMomentum.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); - computeFutureStateUsingALIP(currentPosition, tempCurrentContactPointAngularMomentum, tempFuturePosition, tempFutureContactPointAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); - - double LyEndOfCurrentStep = tempFutureContactPointAngularMomentum.getY(); - double LxEndOfCurrentStep = tempFutureContactPointAngularMomentum.getX(); - - double desiredFootstepPositionX = (LyDesired - Math.cosh(omega * stepDuration) * LyEndOfCurrentStep) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); - double desiredFootstepPositionY = (Math.cosh(omega * stepDuration) * LxEndOfCurrentStep - LxDesired) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); - - ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); - touchdownPositionToPack.setToZero(controlFrame); - touchdownPositionToPack.set(-desiredFootstepPositionX, -desiredFootstepPositionY); - touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); - } - - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePoint3DReadOnly currentPosition, - FrameVector3DReadOnly currentContactPointAngularMomentum, - RobotSide swingSide, - double desiredVelocityX, - double desiredVelocityY, - double desiredStanceWidth, - double timeRemainingInCurrentStep, - double stepDuration, - double doubleSupportDuration, - double pendulumMass, - double pendulumHeight, - double pole, - FramePoint2DBasics touchdownPositionToPack, - ReferenceFrame stanceFootFrame, - ReferenceFrame controlFrame) +// public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentPosition, +// FrameVector3DReadOnly currentVelocity, +// FrameVector3DReadOnly currentCentroidalAngularMomentum, +// RobotSide swingSide, +// double desiredVelocityX, +// double desiredVelocityY, +// double desiredStanceWidth, +// double timeRemainingInCurrentStep, +// double stepDuration, +// double pendulumMass, +// double pendulumHeight, +// FramePoint2DBasics touchdownPositionToPack, +// ReferenceFrame stanceFootFrame, +// ReferenceFrame swingFootFrame, +// ReferenceFrame controlFrame) +// { +// double omega = calculateOmega(pendulumHeight); +// +// double LyDesired = pendulumMass * desiredVelocityX * pendulumHeight; +// double LxDesired = computeDesiredAngularMomentumForStanceWidth(swingSide, desiredStanceWidth, pendulumMass, pendulumHeight, stepDuration) -pendulumMass * desiredVelocityY * pendulumHeight; +// +// ////// +// // Get CoM velocity and change frame to CoM control frame +// tempVelocity.setIncludingFrame(currentVelocity); +// tempVelocity.changeFrame(stanceFootFrame); +// +// // Get CoM angular momentum and change frame to CoM control frame +// tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); +// tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); +// ///// +// +// tempCurrentContactPointAngularMomentum.setToZero(stanceFootFrame); +// tempCurrentContactPointAngularMomentum.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); +// tempCurrentContactPointAngularMomentum.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); +// computeFutureStateUsingALIP(currentPosition, tempCurrentContactPointAngularMomentum, tempFuturePosition, tempFutureContactPointAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); +// +// double LyEndOfCurrentStep = tempFutureContactPointAngularMomentum.getY(); +// double LxEndOfCurrentStep = tempFutureContactPointAngularMomentum.getX(); +// +// double desiredFootstepPositionX = (LyDesired - Math.cosh(omega * stepDuration) * LyEndOfCurrentStep) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); +// double desiredFootstepPositionY = (Math.cosh(omega * stepDuration) * LxEndOfCurrentStep - LxDesired) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); +// +// ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); +// touchdownPositionToPack.setToZero(controlFrame); +// touchdownPositionToPack.set(-desiredFootstepPositionX, -desiredFootstepPositionY); +// touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); +// } + + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePoint3DReadOnly currentCoMPosition, + FrameVector3DReadOnly currentContactPointAngularMomentum, + FramePoint3DReadOnly currentStanceFootPosition, + FramePoint2DBasics touchdownPositionToPack, + RobotSide swingSide, + double desiredVelocityX, + double desiredVelocityY, + double desiredStanceWidth, + double timeRemainingInCurrentStep, + double stepDuration, + double doubleSupportDuration, + double pendulumMass, + double pendulumHeight, + double pole, + boolean useFutureCoM) { double omega = calculateOmega(pendulumHeight); @@ -134,39 +146,50 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement // tempAngularMomentum2.setToZero(stanceFootFrame); // tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); // tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); - - // Get CoM angular momentum and change frame to CoM control frame - tempCurrentContactPointAngularMomentum.setIncludingFrame(currentContactPointAngularMomentum); - tempCurrentContactPointAngularMomentum.changeFrame(stanceFootFrame); - - computeFutureStateUsingALIP(currentPosition, tempCurrentContactPointAngularMomentum, tempFuturePosition, tempFutureContactPointAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); - - tempVelocity.setX(tempFutureContactPointAngularMomentum.getY() / (pendulumMass * pendulumHeight)); - tempVelocity.setY(-tempFutureContactPointAngularMomentum.getX() / (pendulumMass * pendulumHeight)); - - computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempVelocity, touchdownPositionToPack, pole, stepDuration, omega, controlFrame); +// +// // Get CoM angular momentum and change frame to CoM control frame +// tempCurrentContactPointAngularMomentum.setMatchingFrame(currentContactPointAngularMomentum); + + computeFutureStateUsingALIP(currentCoMPosition, + currentContactPointAngularMomentum, + currentStanceFootPosition, + tempFutureCoMPosition, + tempFutureContactPointAngularMomentum, + timeRemainingInCurrentStep, + pendulumMass, + pendulumHeight); + + tempCurrentCoMPosition.setMatchingFrame(currentCoMPosition); + tempFutureCoMVelocity.setX(tempFutureContactPointAngularMomentum.getY() / (pendulumMass * pendulumHeight)); + tempFutureCoMVelocity.setY(-tempFutureContactPointAngularMomentum.getX() / (pendulumMass * pendulumHeight)); + + if (useFutureCoM) + computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempFutureCoMVelocity, tempFutureCoMPosition, touchdownPositionToPack, pole, stepDuration, omega); + else + computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempFutureCoMVelocity, tempCurrentCoMPosition, touchdownPositionToPack, pole, stepDuration, omega); double swingDuration = stepDuration - doubleSupportDuration; ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); - touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrame); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, omega, desiredVelocityX)); touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, swingSide.getOppositeSide(), omega, desiredVelocityY)); touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, doubleSupportDuration, desiredStanceWidth, swingSide.getOppositeSide(), omega)); touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, FramePoint2DBasics touchdownPositionToPack, double pole, double stepDuration, double omega, ReferenceFrame controlFrame) + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, FramePoint3DReadOnly position, FramePoint2DBasics touchdownPositionToPack, double pole, double stepDuration, double omega) { - computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), velocity, touchdownPositionToPack, controlFrame); + computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), velocity, position, touchdownPositionToPack); } - public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, FramePoint2DBasics touchdownPositionToPack, ReferenceFrame controlFrame) + public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, FramePoint3DReadOnly position, FramePoint2DBasics touchdownPositionToPack) { ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); - touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrame); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); touchdownPositionToPack.set(velocity.getX(), velocity.getY()); touchdownPositionToPack.scale(timeConstant); + touchdownPositionToPack.add(position.getX(), position.getY()); touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 52bfa6fd280..38c58189e85 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -271,8 +271,9 @@ private void updateDesireds() netPendulumBase3DInWorld.setMatchingFrame(netPendulumBase, 0.0); } - private final FramePoint3D tempPosition = new FramePoint3D(); - private final FrameVector3D tempAngularMomentum = new FrameVector3D(); + private final FramePoint3D tempCurrentCoMPosition = new FramePoint3D(); + private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); + private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalculator touchdownCalculator, RobotSide startingSwingSide, @@ -293,66 +294,88 @@ public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalcula // tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); // touchdownPositionToPack.set(tempTouchdownPosition); - desiredTouchdownPositionsList.clear(); - for (int i = 0; i < numberOfFootstepsToPlan ; i++) - { - FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.add(); - - FramePoint3DReadOnly currentCoMPosition; - FrameVector3DReadOnly currentAngularMomentum; - double timeToReachGoal; - RobotSide swingSide; - - if (i == 0) - { - currentCoMPosition = estimates.getCenterOfMassPosition(); - currentAngularMomentum = estimates.getContactPointAngularMomentum(); - swingSide = startingSwingSide; - timeToReachGoal = timeRemainingInCurrentStep; - } - else if (i % 2 == 0) - { - currentCoMPosition = tempPosition; - currentAngularMomentum = tempAngularMomentum; - swingSide = startingSwingSide; - timeToReachGoal = getSwingDuration(swingSide); - } - else - { - currentCoMPosition = tempPosition; - currentAngularMomentum = tempAngularMomentum; - swingSide = startingSwingSide.getOppositeSide(); - timeToReachGoal = getSwingDuration(swingSide); - } - ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( - currentCoMPosition, - currentAngularMomentum, - swingSide, - desiredVelocity.getX(), - desiredVelocity.getY(), - parameters.getStanceWidth(swingSide).getDoubleValue(), - timeToReachGoal, - getStepDuration(swingSide), - getTransferDuration(swingSide), - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - parameters.getPole(swingSide).getDoubleValue(), - desiredTouchdownPositionToPack, - referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), - centerOfMassControlZUpFrame); - - ALIPCalculatorTools.computeFutureStateUsingALIP( - currentCoMPosition, - currentAngularMomentum, - tempPosition, - tempAngularMomentum, - timeToReachGoal, - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide())); +// desiredTouchdownPositionsList.clear(); +// for (int i = 0; i < numberOfFootstepsToPlan ; i++) +// { +// FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.add(); +// +// FramePoint3DReadOnly currentCoMPosition; +// FrameVector3DReadOnly currentAngularMomentum; +// double timeToReachGoal; +// RobotSide swingSide; +// +// if (i == 0) +// { +// currentCoMPosition = estimates.getCenterOfMassPosition(); +// currentAngularMomentum = estimates.getContactPointAngularMomentum(); +// swingSide = startingSwingSide; +// timeToReachGoal = timeRemainingInCurrentStep; +// } +// else if (i % 2 == 0) +// { +// currentCoMPosition = tempCurrentCoMPosition; +// currentAngularMomentum = tempCurrentContactPointAngularMomentum; +// swingSide = startingSwingSide; +// timeToReachGoal = getSwingDuration(swingSide); +// } +// else +// { +// currentCoMPosition = tempCurrentCoMPosition; +// currentAngularMomentum = tempCurrentContactPointAngularMomentum; +// swingSide = startingSwingSide.getOppositeSide(); +// timeToReachGoal = getSwingDuration(swingSide); +// } +// +// ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( +// currentCoMPosition, +// currentAngularMomentum, +// swingSide, +// desiredVelocity.getX(), +// desiredVelocity.getY(), +// parameters.getStanceWidth(swingSide).getDoubleValue(), +// timeToReachGoal, +// getStepDuration(swingSide), +// getTransferDuration(swingSide), +// robotModel.getTotalMass(), +// parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), +// parameters.getPole(swingSide).getDoubleValue(), +// desiredTouchdownPositionToPack, +// referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), +// centerOfMassControlZUpFrame); +// +// ALIPCalculatorTools.computeFutureStateUsingALIP( +// currentCoMPosition, +// currentAngularMomentum, +// tempPosition, +// tempAngularMomentum, +// timeToReachGoal, +// robotModel.getTotalMass(), +// parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), +// referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide())); +// +// } - } + tempCurrentCoMPosition.setMatchingFrame(estimates.getCenterOfMassPosition()); + tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); + tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(startingSwingSide.getOppositeSide())); + + ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( + tempCurrentCoMPosition, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + touchdownPositionToPack, + startingSwingSide, + desiredVelocity.getX(), + desiredVelocity.getY(), + parameters.getStanceWidth(startingSwingSide).getDoubleValue(), + timeRemainingInCurrentStep, + getStepDuration(startingSwingSide), + getTransferDuration(startingSwingSide), + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(startingSwingSide).getDoubleValue(), + parameters.getPole(startingSwingSide).getDoubleValue(), + false); } private void calculateNetPendulumBase() From d76bf0584bb61b78cd5c30343200b8faafb5d0ce Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 19 Feb 2025 17:29:03 -0600 Subject: [PATCH 05/17] can walk with all csg footsteps calculated via QFP, but turning is still broken --- .../ContinuousStepGenerator.java | 28 +- .../ALIPCalculatorTools.java | 28 +- .../QuicksterFootstepProvider.java | 239 +++++++----------- 3 files changed, 93 insertions(+), 202 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index 6ef634e7906..d547b6bdbe2 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -346,32 +346,16 @@ else if (statusToProcess == FootstepStatus.COMPLETED) quicksterFootstepProvider.get().update(parameters.getNumberOfFootstepsToPlan()); } + int perFootIndex = 0; + for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++) { if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) { - // FIXME we want all steps in plan to be QFP eventually -// quicksterFootstepProvider.get().getDesiredTouchdownPose(i, nextFootstepPose2D); - if (i == startingIndexToAdjust) - { - quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D); - } - else - { - calculateNextFootstepPose2D(stepTime.getValue(), - desiredVelocityX, - desiredVelocityY, - desiredTurningVelocity.getDoubleValue(), - swingSide, - maxStepLength, - maxStepWidth, - defaultStepWidth, - minStepWidth, - turnMaxAngleInward, - turnMaxAngleOutward, - footstepPose2D, - nextFootstepPose2D); - } + if (i != 0 && i % 2 == 0) + perFootIndex++; + + quicksterFootstepProvider.get().getDesiredTouchdownPose(perFootIndex, swingSide, nextFootstepPose2D); } else { diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index c2bacbe078d..23f506fe742 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -29,13 +29,6 @@ public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentCoMPo double pendulumMass, double pendulumHeight) { - // Use future position/angular momentum as placeholder variables to make sure current position/angular momentum are in correct frame -// futurePositionToPack.setMatchingFrame(currentCoMPosition); -// futurePositionToPack.distance(stanceFootPosition); -// -// futureAngularMomentumToPack.setMatchingFrame(currentContactPointAngularMomentum); -// futureAngularMomentumToPack.(stanceFootFrame); - tempCurrentCoMPosition.setMatchingFrame(currentCoMPosition); tempCurrentContactPointAngularMomentum.setMatchingFrame(currentContactPointAngularMomentum); tempCurrentStanceFootPosition.setMatchingFrame(currentStanceFootPosition); @@ -60,8 +53,8 @@ public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentCoMPo double yf = y0 * Math.cosh(omega * deltaT) - Lx0 * Math.sinh(omega * deltaT) / (pendulumMass * pendulumHeight * omega); double Lxf = -y0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * deltaT) + Lx0 * Math.cosh(omega * deltaT); - futurePositionToPack.setX(xf); - futurePositionToPack.setY(yf); + futurePositionToPack.setX(xf + tempCurrentStanceFootPosition.getX()); + futurePositionToPack.setY(yf + tempCurrentStanceFootPosition.getY()); futureAngularMomentumToPack.setX(Lxf); futureAngularMomentumToPack.setY(Lyf); @@ -133,23 +126,6 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement { double omega = calculateOmega(pendulumHeight); -// ////// -// // Get CoM velocity and change frame to CoM control frame -// tempVelocity.setIncludingFrame(currentVelocity); -// tempVelocity.changeFrame(stanceFootFrame); -// -// // Get CoM angular momentum and change frame to CoM control frame -// tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); -// tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); -// ///// -// -// tempAngularMomentum2.setToZero(stanceFootFrame); -// tempAngularMomentum2.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); -// tempAngularMomentum2.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); -// -// // Get CoM angular momentum and change frame to CoM control frame -// tempCurrentContactPointAngularMomentum.setMatchingFrame(currentContactPointAngularMomentum); - computeFutureStateUsingALIP(currentCoMPosition, currentContactPointAngularMomentum, currentStanceFootPosition, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 38c58189e85..b1d130c4a4b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -53,14 +53,10 @@ public class QuicksterFootstepProvider // Command/Desired output related stuff private final SideDependentList touchdownCalculator = new SideDependentList<>(); - private final SideDependentList desiredTouchdownPositions = new SideDependentList<>(); - private final SideDependentList desiredTouchdownPoses = new SideDependentList<>(); + private final SideDependentList> desiredTouchdownPositionsList = new SideDependentList<>(); + private final SideDependentList> desiredTouchdownPosesList = new SideDependentList<>(); private final SideDependentList desiredTouchdownPosition3DInWorld = new SideDependentList<>(); - // - private final RecyclingArrayList desiredTouchdownPositionsList = new RecyclingArrayList<>(FramePoint2D.class); - private final RecyclingArrayList desiredTouchdownPosesList = new RecyclingArrayList<>(FramePose2D.class); - // Desired inputs private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); private final YoVector2D desiredVelocity = new YoVector2D("desiredVelocity" + variableNameSuffix, registry); @@ -140,8 +136,8 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano desiredVelocity, registry)); - desiredTouchdownPositions.put(robotSide, new FramePoint2D()); - desiredTouchdownPoses.put(robotSide, new FramePose2D()); + desiredTouchdownPositionsList.put(robotSide, new RecyclingArrayList<>(FramePoint2D.class)); + desiredTouchdownPosesList.put(robotSide, new RecyclingArrayList<>(FramePose2D.class)); desiredTouchdownPosition3DInWorld.put(robotSide, new YoFramePoint3D(robotSide.getLowerCaseName() + "DesiredTouchdownPosition3DInWorld" + variableNameSuffix, ReferenceFrame.getWorldFrame(), @@ -255,17 +251,17 @@ private void updateDesireds() pendulumBase3DInWorld.get(robotSide).setMatchingFrame(pendulumBase.get(robotSide), 0.0); - desiredTouchdownPosition3DInWorld.get(robotSide).setMatchingFrame(desiredTouchdownPositions.get(robotSide), 0.0); + if (!desiredTouchdownPositionsList.get(robotSide).isEmpty()) + desiredTouchdownPosition3DInWorld.get(robotSide).setMatchingFrame(desiredTouchdownPositionsList.get(robotSide).get(0), 0.0); - desiredTouchdownPoses.get(robotSide).getPosition().set(desiredTouchdownPositions.get(robotSide)); - desiredTouchdownPoses.get(robotSide).getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); - } + desiredTouchdownPosesList.get(robotSide).clear(); + for (int i = 0; i < desiredTouchdownPositionsList.get(robotSide).size(); i++) + { + FramePose2D desiredTouchdownPose = desiredTouchdownPosesList.get(robotSide).add(); - desiredTouchdownPosesList.clear(); - for (int i = 0; i< desiredTouchdownPositionsList.size() ; i++) - { - desiredTouchdownPosesList.getAndGrowIfNeeded(i).getPosition().set(desiredTouchdownPositionsList.get(i)); - desiredTouchdownPosesList.getAndGrowIfNeeded(i).getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); + desiredTouchdownPose.getPosition().set(desiredTouchdownPositionsList.get(robotSide).get(i)); + desiredTouchdownPose.getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); + } } netPendulumBase3DInWorld.setMatchingFrame(netPendulumBase, 0.0); @@ -275,107 +271,76 @@ private void updateDesireds() private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); - public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalculator touchdownCalculator, - RobotSide startingSwingSide, - double timeRemainingInCurrentStep, - FramePoint2DReadOnly pendulumBase, - FramePoint2DReadOnly netPendulumBase, - boolean isInDoubleSupport, - FramePoint2DBasics touchdownPositionToPack) + public void calculateTouchdownPosition(RobotSide currentSwingSide, + double timeRemainingInCurrentStep) { -// tempPendulumBase.setIncludingFrame(pendulumBase); -// tempPendulumBase.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); -// -// tempNetPendulumBase.setIncludingFrame(netPendulumBase); -// tempNetPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(getTrailingSide())); -// -// touchdownCalculator.computeDesiredTouchdownPosition(swingSide.getOppositeSide(), timeToReachGoal, tempPendulumBase, tempNetPendulumBase, isInDoubleSupport); -// tempTouchdownPosition.setIncludingFrame(touchdownCalculator.getDesiredTouchdownPosition2D()); -// tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); -// touchdownPositionToPack.set(tempTouchdownPosition); + desiredTouchdownPositionsList.get(currentSwingSide).clear(); + desiredTouchdownPositionsList.get(currentSwingSide.getOppositeSide()).clear(); + for (int i = 0; i < numberOfFootstepsToPlan ; i++) + { + double timeToReachGoal; + RobotSide swingSide; + boolean useFutureCoM; -// desiredTouchdownPositionsList.clear(); -// for (int i = 0; i < numberOfFootstepsToPlan ; i++) -// { -// FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.add(); -// -// FramePoint3DReadOnly currentCoMPosition; -// FrameVector3DReadOnly currentAngularMomentum; -// double timeToReachGoal; -// RobotSide swingSide; -// -// if (i == 0) -// { -// currentCoMPosition = estimates.getCenterOfMassPosition(); -// currentAngularMomentum = estimates.getContactPointAngularMomentum(); -// swingSide = startingSwingSide; -// timeToReachGoal = timeRemainingInCurrentStep; -// } -// else if (i % 2 == 0) -// { -// currentCoMPosition = tempCurrentCoMPosition; -// currentAngularMomentum = tempCurrentContactPointAngularMomentum; -// swingSide = startingSwingSide; -// timeToReachGoal = getSwingDuration(swingSide); -// } -// else -// { -// currentCoMPosition = tempCurrentCoMPosition; -// currentAngularMomentum = tempCurrentContactPointAngularMomentum; -// swingSide = startingSwingSide.getOppositeSide(); -// timeToReachGoal = getSwingDuration(swingSide); -// } -// -// ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( -// currentCoMPosition, -// currentAngularMomentum, -// swingSide, -// desiredVelocity.getX(), -// desiredVelocity.getY(), -// parameters.getStanceWidth(swingSide).getDoubleValue(), -// timeToReachGoal, -// getStepDuration(swingSide), -// getTransferDuration(swingSide), -// robotModel.getTotalMass(), -// parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), -// parameters.getPole(swingSide).getDoubleValue(), -// desiredTouchdownPositionToPack, -// referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide()), -// centerOfMassControlZUpFrame); -// -// ALIPCalculatorTools.computeFutureStateUsingALIP( -// currentCoMPosition, -// currentAngularMomentum, -// tempPosition, -// tempAngularMomentum, -// timeToReachGoal, -// robotModel.getTotalMass(), -// parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), -// referenceFrames.getSoleZUpFrame(swingSide.getOppositeSide())); -// -// } + if (i == 0) + { + tempCurrentCoMPosition.setMatchingFrame(estimates.getCenterOfMassPosition()); + tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); + tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide())); + swingSide = currentSwingSide; + timeToReachGoal = timeRemainingInCurrentStep; + useFutureCoM = false; + } + else if (i % 2 == 0) + { + swingSide = currentSwingSide; + timeToReachGoal = getSwingDuration(swingSide); + useFutureCoM = true; + } + else + { + swingSide = currentSwingSide.getOppositeSide(); + timeToReachGoal = getSwingDuration(swingSide); + useFutureCoM = true; + } - tempCurrentCoMPosition.setMatchingFrame(estimates.getCenterOfMassPosition()); - tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); - tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(startingSwingSide.getOppositeSide())); - - ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( - tempCurrentCoMPosition, - tempCurrentContactPointAngularMomentum, - tempCurrentStanceFootPosition, - touchdownPositionToPack, - startingSwingSide, - desiredVelocity.getX(), - desiredVelocity.getY(), - parameters.getStanceWidth(startingSwingSide).getDoubleValue(), - timeRemainingInCurrentStep, - getStepDuration(startingSwingSide), - getTransferDuration(startingSwingSide), - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(startingSwingSide).getDoubleValue(), - parameters.getPole(startingSwingSide).getDoubleValue(), - false); + FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.get(swingSide).add(); + + ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( + tempCurrentCoMPosition, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + desiredTouchdownPositionToPack, + swingSide, + desiredVelocity.getX(), + desiredVelocity.getY(), + parameters.getStanceWidth(swingSide).getDoubleValue(), + timeToReachGoal, + getStepDuration(swingSide), + getTransferDuration(swingSide), + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + parameters.getPole(swingSide).getDoubleValue(), + useFutureCoM); + + ALIPCalculatorTools.computeFutureStateUsingALIP( + tempCurrentCoMPosition, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + tempCurrentCoMPosition, + tempCurrentContactPointAngularMomentum, + timeToReachGoal, + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue()); + + tempCurrentStanceFootPosition.setMatchingFrame(desiredTouchdownPositionToPack, 0.0); + if (!useFutureCoM) + { + tempCurrentStanceFootPosition.sub(estimates.getCenterOfMassPosition()); + tempCurrentStanceFootPosition.add(tempCurrentCoMPosition); + } + } } private void calculateNetPendulumBase() @@ -533,35 +498,13 @@ private RobotSide getTrailingSide() return trailingSide; } - public void getDesiredTouchdownPosition2D(RobotSide robotSide, FixedFramePoint2DBasics touchdownPositionToPack) - { - tempTouchdownPosition.setIncludingFrame(getDesiredTouchdownPosition2D(robotSide)); - tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); - touchdownPositionToPack.set(tempTouchdownPosition); - } - - public FramePoint2DReadOnly getDesiredTouchdownPosition2D(RobotSide robotSide) - { - if (desiredTouchdownPositions.get(robotSide).containsNaN()) - { - footStates.get(robotSide).set(FootState.SWING); - footStateMachines.get(robotSide).performTransition(FootState.SWING); - footStateMachines.get(robotSide).doAction(); - } - - return desiredTouchdownPositions.get(robotSide); - } - - public void getDesiredTouchdownPose(RobotSide robotSide, FramePose2D touchdownPoseToPack) + public void getDesiredTouchdownPose(int footstepIndex, RobotSide robotSide, FramePose2D touchdownPoseToPack) { // TODO 3D transform between two 2D poses? - touchdownPoseToPack.setMatchingFrame(desiredTouchdownPoses.get(robotSide)); - } + if (desiredTouchdownPosesList.get(robotSide).size() < 1) + desiredTouchdownPosesList.get(robotSide).add(); - public void getDesiredTouchdownPose(int footstepIndex, FramePose2D touchdownPoseToPack) - { - // TODO 3D transform between two 2D poses? - touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(footstepIndex)); + touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(robotSide).get(footstepIndex)); } public double getSwingDuration(RobotSide swingSide) @@ -660,13 +603,7 @@ private SwingFootState(RobotSide robotSide) @Override public void onEntry() { - calculateTouchdownPosition(touchdownCalculator.get(robotSide), - robotSide, - getStepDuration(robotSide), - pendulumBase.get(robotSide.getOppositeSide()), - netPendulumBase, - inDoubleSupport.getBooleanValue(), - desiredTouchdownPositions.get(robotSide)); + calculateTouchdownPosition(robotSide, getStepDuration(robotSide)); parameters.setParametersForUpcomingSwing(robotSide); } @@ -677,13 +614,7 @@ public void doAction(double timeInState) double timeToReachGoal = getStepDuration(robotSide) - timeInState; timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getStepDuration(robotSide)); - calculateTouchdownPosition(touchdownCalculator.get(robotSide), - robotSide, - timeToReachGoal, - pendulumBase.get(robotSide.getOppositeSide()), - netPendulumBase, - inDoubleSupport.getBooleanValue(), - desiredTouchdownPositions.get(robotSide)); + calculateTouchdownPosition(robotSide, timeToReachGoal); } @Override From cebff3c1b95f3aacd7ea11c4b26b6ee619e08515 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 20 Feb 2025 18:09:42 -0600 Subject: [PATCH 06/17] turning in place and turning while walking now work --- .../ALIPCalculatorTools.java | 121 ++++++++++++------ .../QuicksterFootstepProvider.java | 55 ++++---- 2 files changed, 107 insertions(+), 69 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 23f506fe742..6d92280e01d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -1,8 +1,6 @@ package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.*; import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.robotics.robotSide.RobotSide; @@ -10,54 +8,85 @@ public class ALIPCalculatorTools { private final static double GRAVITY = -9.81; - private final static FramePoint3D tempFutureCoMPosition = new FramePoint3D(); + private final static FramePose3D tempFutureCoMPose = new FramePose3D(); private final static FrameVector3D tempFutureCoMVelocity = new FrameVector3D(); private final static FrameVector3D tempFutureContactPointAngularMomentum = new FrameVector3D(); + private final static PoseReferenceFrame tempFutureControlFrame = new PoseReferenceFrame("futureControlFrameALIPCalculator", ReferenceFrame.getWorldFrame()); - private final static FramePoint3D tempCurrentCoMPosition = new FramePoint3D(); + private final static FramePose3D tempCurrentCoMPose = new FramePose3D(); private final static FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final static FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); - private final static FrameVector3D tempCentroidalAngularMomentum = new FrameVector3D(); - - public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentCoMPosition, + public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, FrameVector3DReadOnly currentContactPointAngularMomentum, FramePoint3DReadOnly currentStanceFootPosition, - FramePoint3D futurePositionToPack, - FrameVector3D futureAngularMomentumToPack, - double deltaT, + FramePose3D futureCoMPoseToPack, + FrameVector3D futureContactPointAngularMomentumToPack, + ReferenceFrame controlFrame, + double horizonDuration, double pendulumMass, - double pendulumHeight) + double pendulumHeight, + double desiredTurningVelocity, + double updateDt) { - tempCurrentCoMPosition.setMatchingFrame(currentCoMPosition); + tempCurrentCoMPose.setMatchingFrame(currentCoMPose); tempCurrentContactPointAngularMomentum.setMatchingFrame(currentContactPointAngularMomentum); tempCurrentStanceFootPosition.setMatchingFrame(currentStanceFootPosition); - tempCurrentCoMPosition.setX(tempCurrentCoMPosition.getX() - tempCurrentStanceFootPosition.getX()); - tempCurrentCoMPosition.setY(tempCurrentCoMPosition.getY() - tempCurrentStanceFootPosition.getY()); - tempCurrentCoMPosition.setZ(tempCurrentCoMPosition.getZ() - tempCurrentStanceFootPosition.getZ()); + tempCurrentCoMPose.changeFrame(controlFrame); + tempCurrentContactPointAngularMomentum.changeFrame(controlFrame); + tempCurrentStanceFootPosition.changeFrame(controlFrame); + + tempCurrentCoMPose.setX(-tempCurrentStanceFootPosition.getX()); + tempCurrentCoMPose.setY(-tempCurrentStanceFootPosition.getY()); + tempCurrentCoMPose.setZ(-tempCurrentStanceFootPosition.getZ()); double omega = calculateOmega(pendulumHeight); // Current position and angular momentum - double x0 = tempCurrentCoMPosition.getX(); + double x0 = tempCurrentCoMPose.getX(); double Ly0 = tempCurrentContactPointAngularMomentum.getY(); - double y0 = tempCurrentCoMPosition.getY(); + double y0 = tempCurrentCoMPose.getY(); double Lx0 = tempCurrentContactPointAngularMomentum.getX(); // Final position and angular momentum - double xf = x0 * Math.cosh(omega * deltaT) + Ly0 * Math.sinh(omega * deltaT) / (pendulumMass * pendulumHeight * omega); - double Lyf = x0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * deltaT) + Ly0 * Math.cosh(omega * deltaT); + double xf = x0 * Math.cosh(omega * horizonDuration) + Ly0 * Math.sinh(omega * horizonDuration) / (pendulumMass * pendulumHeight * omega); + double Lyf = x0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * horizonDuration) + Ly0 * Math.cosh(omega * horizonDuration); + + double yf = y0 * Math.cosh(omega * horizonDuration) - Lx0 * Math.sinh(omega * horizonDuration) / (pendulumMass * pendulumHeight * omega); + double Lxf = -y0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * horizonDuration) + Lx0 * Math.cosh(omega * horizonDuration); + + ReferenceFrame originalPositionFrame = futureCoMPoseToPack.getReferenceFrame(); + ReferenceFrame originalMomentumFrame = futureContactPointAngularMomentumToPack.getReferenceFrame(); + + futureCoMPoseToPack.setIncludingFrame(tempCurrentCoMPose); + futureContactPointAngularMomentumToPack.changeFrame(controlFrame); + + + futureCoMPoseToPack.setX(tempCurrentStanceFootPosition.getX()); + futureCoMPoseToPack.setY(tempCurrentStanceFootPosition.getY()); + futureCoMPoseToPack.setZ(tempCurrentCoMPose.getZ()); + + int intervals = (int) Math.round(horizonDuration / updateDt); + + for (double i = 0; i < intervals ; i ++) + { + futureCoMPoseToPack.appendYawRotation(desiredTurningVelocity * updateDt); + futureCoMPoseToPack.getPosition().addX(xf / intervals); + futureCoMPoseToPack.getPosition().addY(yf / intervals); + + + } + + futureContactPointAngularMomentumToPack.setX(Lxf); + futureContactPointAngularMomentumToPack.setY(Lyf); - double yf = y0 * Math.cosh(omega * deltaT) - Lx0 * Math.sinh(omega * deltaT) / (pendulumMass * pendulumHeight * omega); - double Lxf = -y0 * pendulumMass * pendulumHeight * omega * Math.sinh(omega * deltaT) + Lx0 * Math.cosh(omega * deltaT); - futurePositionToPack.setX(xf + tempCurrentStanceFootPosition.getX()); - futurePositionToPack.setY(yf + tempCurrentStanceFootPosition.getY()); + futureCoMPoseToPack.changeFrame(originalPositionFrame); + futureContactPointAngularMomentumToPack.changeFrame(originalMomentumFrame); + - futureAngularMomentumToPack.setX(Lxf); - futureAngularMomentumToPack.setY(Lyf); } // public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentPosition, @@ -108,13 +137,15 @@ public static void computeFutureStateUsingALIP(FramePoint3DReadOnly currentCoMPo // touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); // } - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePoint3DReadOnly currentCoMPosition, + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePose3DReadOnly currentCoMPose, FrameVector3DReadOnly currentContactPointAngularMomentum, FramePoint3DReadOnly currentStanceFootPosition, FramePoint2DBasics touchdownPositionToPack, RobotSide swingSide, + ReferenceFrame controlFrame, double desiredVelocityX, double desiredVelocityY, + double desiredTurningVelocity, double desiredStanceWidth, double timeRemainingInCurrentStep, double stepDuration, @@ -122,50 +153,60 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement double pendulumMass, double pendulumHeight, double pole, - boolean useFutureCoM) + boolean useFutureCoM, + double updateDt) { double omega = calculateOmega(pendulumHeight); - computeFutureStateUsingALIP(currentCoMPosition, + computeFutureStateUsingALIP(currentCoMPose, currentContactPointAngularMomentum, currentStanceFootPosition, - tempFutureCoMPosition, + tempFutureCoMPose, tempFutureContactPointAngularMomentum, + controlFrame, timeRemainingInCurrentStep, pendulumMass, - pendulumHeight); + pendulumHeight, + desiredTurningVelocity, + updateDt); + + tempFutureControlFrame.setPoseAndUpdate(tempFutureCoMPose); - tempCurrentCoMPosition.setMatchingFrame(currentCoMPosition); + tempFutureCoMVelocity.changeFrame(tempFutureContactPointAngularMomentum.getReferenceFrame()); tempFutureCoMVelocity.setX(tempFutureContactPointAngularMomentum.getY() / (pendulumMass * pendulumHeight)); tempFutureCoMVelocity.setY(-tempFutureContactPointAngularMomentum.getX() / (pendulumMass * pendulumHeight)); + ReferenceFrame controlFrameToUse; + if (useFutureCoM) - computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempFutureCoMVelocity, tempFutureCoMPosition, touchdownPositionToPack, pole, stepDuration, omega); + controlFrameToUse = tempFutureControlFrame; else - computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempFutureCoMVelocity, tempCurrentCoMPosition, touchdownPositionToPack, pole, stepDuration, omega); + controlFrameToUse = controlFrame; + + tempFutureCoMVelocity.changeFrame(controlFrameToUse); + computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempFutureCoMVelocity, controlFrameToUse, touchdownPositionToPack, pole, stepDuration, omega); double swingDuration = stepDuration - doubleSupportDuration; ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); - touchdownPositionToPack.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrameToUse); touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, omega, desiredVelocityX)); touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, swingSide.getOppositeSide(), omega, desiredVelocityY)); touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, doubleSupportDuration, desiredStanceWidth, swingSide.getOppositeSide(), omega)); touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, FramePoint3DReadOnly position, FramePoint2DBasics touchdownPositionToPack, double pole, double stepDuration, double omega) + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, ReferenceFrame controlFrame, FramePoint2DBasics touchdownPositionToPack, double pole, double stepDuration, double omega) { - computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), velocity, position, touchdownPositionToPack); + computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), velocity, controlFrame, touchdownPositionToPack); } - public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, FramePoint3DReadOnly position, FramePoint2DBasics touchdownPositionToPack) + public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, ReferenceFrame controlFrame, FramePoint2DBasics touchdownPositionToPack) { ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); - touchdownPositionToPack.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + touchdownPositionToPack.setToZero(controlFrame); touchdownPositionToPack.set(velocity.getX(), velocity.getY()); touchdownPositionToPack.scale(timeConstant); - touchdownPositionToPack.add(position.getX(), position.getY()); touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index b1d130c4a4b..d23ffbb29d1 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -213,17 +213,8 @@ public void update(int numberOfFootstepsToPlan) if (firstTick) initialize(); -// desiredTouchdownPositionsList.clear(); -// desiredTouchdownPosesList.clear(); -// -// for (int i = 0; i< numberOfFootstepsToPlan ; i++) -// { -// desiredTouchdownPositionsList.add(); -// desiredTouchdownPosesList.add(); -// } this.numberOfFootstepsToPlan = numberOfFootstepsToPlan; - updateEstimates(); updateDesireds(); @@ -250,32 +241,24 @@ private void updateDesireds() footStateMachines.get(robotSide).doActionAndTransition(); pendulumBase3DInWorld.get(robotSide).setMatchingFrame(pendulumBase.get(robotSide), 0.0); - - if (!desiredTouchdownPositionsList.get(robotSide).isEmpty()) - desiredTouchdownPosition3DInWorld.get(robotSide).setMatchingFrame(desiredTouchdownPositionsList.get(robotSide).get(0), 0.0); - - desiredTouchdownPosesList.get(robotSide).clear(); - for (int i = 0; i < desiredTouchdownPositionsList.get(robotSide).size(); i++) - { - FramePose2D desiredTouchdownPose = desiredTouchdownPosesList.get(robotSide).add(); - - desiredTouchdownPose.getPosition().set(desiredTouchdownPositionsList.get(robotSide).get(i)); - desiredTouchdownPose.getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); - } } netPendulumBase3DInWorld.setMatchingFrame(netPendulumBase, 0.0); } - private final FramePoint3D tempCurrentCoMPosition = new FramePoint3D(); + private final FramePose3D tempCurrentCoMPose = new FramePose3D(); private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); + private final PoseReferenceFrame tempControlFrame = new PoseReferenceFrame("tempControlFrameQFP", ReferenceFrame.getWorldFrame()); public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRemainingInCurrentStep) { desiredTouchdownPositionsList.get(currentSwingSide).clear(); desiredTouchdownPositionsList.get(currentSwingSide.getOppositeSide()).clear(); + desiredTouchdownPosesList.get(currentSwingSide).clear(); + desiredTouchdownPosesList.get(currentSwingSide.getOppositeSide()).clear(); + for (int i = 0; i < numberOfFootstepsToPlan ; i++) { @@ -285,9 +268,11 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, if (i == 0) { - tempCurrentCoMPosition.setMatchingFrame(estimates.getCenterOfMassPosition()); + tempCurrentCoMPose.setFromReferenceFrame(centerOfMassControlZUpFrame); tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide())); + tempControlFrame.setTransformAndUpdate(centerOfMassControlZUpFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); + swingSide = currentSwingSide; timeToReachGoal = timeRemainingInCurrentStep; useFutureCoM = false; @@ -306,15 +291,18 @@ else if (i % 2 == 0) } FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.get(swingSide).add(); + FramePose2D desiredTouchdownPose = desiredTouchdownPosesList.get(swingSide).add(); ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( - tempCurrentCoMPosition, + tempCurrentCoMPose, tempCurrentContactPointAngularMomentum, tempCurrentStanceFootPosition, desiredTouchdownPositionToPack, swingSide, + tempControlFrame, desiredVelocity.getX(), desiredVelocity.getY(), + desiredTurningVelocity.getDoubleValue(), parameters.getStanceWidth(swingSide).getDoubleValue(), timeToReachGoal, getStepDuration(swingSide), @@ -322,23 +310,32 @@ else if (i % 2 == 0) robotModel.getTotalMass(), parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), parameters.getPole(swingSide).getDoubleValue(), - useFutureCoM); + useFutureCoM, + updateDT); ALIPCalculatorTools.computeFutureStateUsingALIP( - tempCurrentCoMPosition, + tempCurrentCoMPose, tempCurrentContactPointAngularMomentum, tempCurrentStanceFootPosition, - tempCurrentCoMPosition, + tempCurrentCoMPose, tempCurrentContactPointAngularMomentum, + tempControlFrame, timeToReachGoal, robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue()); + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + desiredTurningVelocity.getDoubleValue(), + updateDT); + + desiredTouchdownPose.getPosition().set(desiredTouchdownPositionToPack); + desiredTouchdownPose.getOrientation().setFromReferenceFrame(tempControlFrame); + + tempControlFrame.setPoseAndUpdate(tempCurrentCoMPose); tempCurrentStanceFootPosition.setMatchingFrame(desiredTouchdownPositionToPack, 0.0); if (!useFutureCoM) { tempCurrentStanceFootPosition.sub(estimates.getCenterOfMassPosition()); - tempCurrentStanceFootPosition.add(tempCurrentCoMPosition); + tempCurrentStanceFootPosition.add(tempCurrentCoMPose.getPosition()); } } } From 77021d1cc6788479b7e1c76d1ff2c643bf221f46 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Mon, 10 Mar 2025 17:13:11 -0500 Subject: [PATCH 07/17] fixed broken touchdown position visualizer --- .../quicksterFootstepProvider/QuicksterFootstepProvider.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index d23ffbb29d1..a22dd81b3d5 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -338,6 +338,8 @@ else if (i % 2 == 0) tempCurrentStanceFootPosition.add(tempCurrentCoMPose.getPosition()); } } + + desiredTouchdownPosition3DInWorld.get(currentSwingSide).setMatchingFrame(desiredTouchdownPositionsList.get(currentSwingSide).get(0), 0.0); } private void calculateNetPendulumBase() From cf58f13ab966febf4ded66584a8d1e6c3f5b18a5 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Mon, 17 Mar 2025 15:48:49 -0500 Subject: [PATCH 08/17] fixed some visualization stuff, made QFP state machine more synced with controller. walking more steady-state periodic now --- .../LinearMomentumRateControlModule.java | 2 +- .../ALIPCalculatorTools.java | 26 ++++----- .../QuicksterFootstepProvider.java | 53 ++++++++----------- .../QuicksterFootstepProviderEstimates.java | 4 +- .../states/WalkingSingleSupportState.java | 7 +-- .../WalkingMessageHandler.java | 4 +- 6 files changed, 42 insertions(+), 54 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java index 1a882e9ec4f..0d754e371a7 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java @@ -350,7 +350,7 @@ public MomentumRateCommand getMomentumRateCommand() */ public CenterOfPressureCommand getCenterOfPressureCommand() { - return centerOfPressureCommand; + return centerOfPressureCommandCalculator.getCenterOfPressureCommand(); } /** diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 6d92280e01d..77989051a8e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -1,5 +1,6 @@ package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; +import us.ihmc.commons.MathTools; import us.ihmc.euclid.referenceFrame.*; import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.robotics.robotSide.RobotSide; @@ -41,7 +42,7 @@ public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPos tempCurrentCoMPose.setY(-tempCurrentStanceFootPosition.getY()); tempCurrentCoMPose.setZ(-tempCurrentStanceFootPosition.getZ()); - double omega = calculateOmega(pendulumHeight); + double omega = computeNaturalFrequency(pendulumHeight); // Current position and angular momentum double x0 = tempCurrentCoMPose.getX(); @@ -68,15 +69,15 @@ public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPos futureCoMPoseToPack.setY(tempCurrentStanceFootPosition.getY()); futureCoMPoseToPack.setZ(tempCurrentCoMPose.getZ()); + int intervals = (int) Math.round(horizonDuration / updateDt); + intervals = MathTools.clamp(intervals, 1, Integer.MAX_VALUE); for (double i = 0; i < intervals ; i ++) { futureCoMPoseToPack.appendYawRotation(desiredTurningVelocity * updateDt); futureCoMPoseToPack.getPosition().addX(xf / intervals); futureCoMPoseToPack.getPosition().addY(yf / intervals); - - } futureContactPointAngularMomentumToPack.setX(Lxf); @@ -156,7 +157,7 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement boolean useFutureCoM, double updateDt) { - double omega = calculateOmega(pendulumHeight); + double omega = computeNaturalFrequency(pendulumHeight); computeFutureStateUsingALIP(currentCoMPose, currentContactPointAngularMomentum, @@ -184,15 +185,16 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement controlFrameToUse = controlFrame; tempFutureCoMVelocity.changeFrame(controlFrameToUse); - computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempFutureCoMVelocity, controlFrameToUse, touchdownPositionToPack, pole, stepDuration, omega); - + double timeConstant = calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega); double swingDuration = stepDuration - doubleSupportDuration; + computeTouchdownPositionUsingRaibertHeuristic(timeConstant, tempFutureCoMVelocity, controlFrameToUse, touchdownPositionToPack); + ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrameToUse); - touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, omega, desiredVelocityX)); - touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, swingSide.getOppositeSide(), omega, desiredVelocityY)); - touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, doubleSupportDuration, desiredStanceWidth, swingSide.getOppositeSide(), omega)); + touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, 1/timeConstant, desiredVelocityX)); + touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, swingSide.getOppositeSide(), 1/timeConstant, desiredVelocityY)); + touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, doubleSupportDuration, desiredStanceWidth, swingSide.getOppositeSide(), 1/timeConstant)); touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } @@ -218,7 +220,7 @@ public static double calculateTimeConstantUsingPolePlacement(double pole, double public static double computeDesiredAngularMomentumForStanceWidth(RobotSide swingSide, double desiredStanceWidth, double pendulumMass, double pendulumHeight, double stepDuration) { double sideSignMultiplier = swingSide == RobotSide.LEFT ? 1.0 : -1.0; - double omega = calculateOmega(pendulumHeight); + double omega = computeNaturalFrequency(pendulumHeight); return sideSignMultiplier * 0.5 * pendulumMass * pendulumHeight * desiredStanceWidth * (omega * Math.sinh(omega * stepDuration)) / (1 + Math.cosh(omega * stepDuration)); } @@ -273,8 +275,8 @@ public static double computeDesiredTouchdownOffsetForStanceWidth(double swingDur return supportSide.negateIfLeftSide(stepWidthOffset); } - public static double calculateOmega(double height) + public static double computeNaturalFrequency(double pendulumHeight) { - return Math.sqrt(Math.abs(GRAVITY / height)); + return Math.sqrt(Math.abs(GRAVITY / pendulumHeight)); } } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index a22dd81b3d5..90507cf47b5 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -280,13 +280,13 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, else if (i % 2 == 0) { swingSide = currentSwingSide; - timeToReachGoal = getSwingDuration(swingSide); + timeToReachGoal = getStepDuration(swingSide); useFutureCoM = true; } else { swingSide = currentSwingSide.getOppositeSide(); - timeToReachGoal = getSwingDuration(swingSide); + timeToReachGoal = getStepDuration(swingSide); useFutureCoM = true; } @@ -381,9 +381,14 @@ private void calculateNetPendulumBase() alpha = MathTools.clamp(footStateMachines.get(trailingSide.getOppositeSide()).getTimeInCurrentState() / doubleSupportDuration, 0.0, 1.0); // interpolate between the two pendulum bases based on the percentage through double support - netPendulumBase.changeFrameAndProjectToXYPlane(pendulumBase.get(trailingSide).getReferenceFrame()); - netPendulumBase.interpolate(pendulumBase.get(trailingSide), pendulumBase.get(trailingSide.getOppositeSide()), alpha); netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(trailingSide)); + pendulumBase.get(trailingSide).changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(trailingSide)); + pendulumBase.get(trailingSide.getOppositeSide()).changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(trailingSide)); + + netPendulumBase.interpolate(pendulumBase.get(trailingSide), pendulumBase.get(trailingSide.getOppositeSide()), alpha); + + pendulumBase.get(trailingSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + pendulumBase.get(trailingSide.getOppositeSide()).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); } private void handleDesiredsFromProviders() @@ -441,6 +446,9 @@ public void notifyFootstepCompleted(RobotSide robotSide) footStates.get(robotSide).set(FootState.SUPPORT); footStateMachines.get(robotSide).performTransition(FootState.SUPPORT); newestSupportSide.set(robotSide); + + footStates.get(robotSide.getOppositeSide()).set(FootState.SWING); + footStateMachines.get(robotSide.getOppositeSide()).performTransition(FootState.SWING); } /** @@ -452,8 +460,6 @@ public void notifyFootstepCompleted(RobotSide robotSide) */ public void notifyFootstepStarted(RobotSide robotSide) { - footStates.get(robotSide).set(FootState.SWING); - footStateMachines.get(robotSide).performTransition(FootState.SWING); } /** @@ -503,7 +509,10 @@ public void getDesiredTouchdownPose(int footstepIndex, RobotSide robotSide, Fram if (desiredTouchdownPosesList.get(robotSide).size() < 1) desiredTouchdownPosesList.get(robotSide).add(); - touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(robotSide).get(footstepIndex)); + if (footstepIndex < desiredTouchdownPosesList.get(robotSide).size()) + touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(robotSide).get(footstepIndex)); + else + touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(robotSide).getLast()); } public double getSwingDuration(RobotSide swingSide) @@ -545,16 +554,6 @@ public void onEntry() { pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - calculateNetPendulumBase(); - - // if (inDoubleSupport.getBooleanValue()) - // calculate(touchdownCalculator.get(robotSide), - // robotSide, - // getTransferDuration(robotSide), - // pendulumBase.get(robotSide.getOppositeSide()), - // netPendulumBase, - // inDoubleSupport.getBooleanValue(), - // desiredTouchdownPositions.get(robotSide)); } @Override @@ -562,19 +561,6 @@ public void doAction(double timeInState) { pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - calculateNetPendulumBase(); - - double timeToReachGoal = getTransferDuration(robotSide) - timeInState; - timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getTransferDuration(robotSide)); - - // if (inDoubleSupport.getBooleanValue() && footStateMachines.get(robotSide.getOppositeSide()).getTimeInCurrentState() > timeInState) - // calculate(touchdownCalculator.get(robotSide), - // robotSide, - // timeToReachGoal, - // pendulumBase.get(robotSide.getOppositeSide()), - // netPendulumBase, - // inDoubleSupport.getBooleanValue(), - // desiredTouchdownPositions.get(robotSide)); } @Override @@ -586,17 +572,19 @@ public void onExit(double timeInState) @Override public boolean isDone(double timeInState) { - return footStates.get(robotSide).getEnumValue() == FootState.SWING; + return false; } } private class SwingFootState implements State { private final RobotSide robotSide; + private final YoDouble timeToReachGoal; private SwingFootState(RobotSide robotSide) { this.robotSide = robotSide; + this.timeToReachGoal = new YoDouble("timeToReachGoal" + robotSide + "_QFP", registry); } @Override @@ -612,6 +600,7 @@ public void doAction(double timeInState) { double timeToReachGoal = getStepDuration(robotSide) - timeInState; timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getStepDuration(robotSide)); + this.timeToReachGoal.set(timeToReachGoal); calculateTouchdownPosition(robotSide, timeToReachGoal); } @@ -624,7 +613,7 @@ public void onExit(double timeInState) @Override public boolean isDone(double timeInState) { - return footStates.get(robotSide).getEnumValue() == FootState.SUPPORT; + return false; } } } \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java index 4e3d7636a04..e8ed685c819 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java @@ -112,8 +112,8 @@ public void update(RobotSide swingSide) currentCoMLinearMomentum.setMatchingFrame(angularExcursionCalculator.getLinearMomentum()); currentCoMAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); currentContactPointAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); - currentContactPointAngularMomentum.addX(-mass * parameters.getDesiredCoMHeight(swingSide).getDoubleValue() * currentCoMVelocity.getY()); - currentContactPointAngularMomentum.addY(mass * parameters.getDesiredCoMHeight(swingSide).getDoubleValue() * currentCoMVelocity.getX()); + currentContactPointAngularMomentum.addX(-mass * currentCoMPosition.getZ() * currentCoMVelocity.getY()); + currentContactPointAngularMomentum.addY(mass * currentCoMPosition.getZ() * currentCoMVelocity.getX()); // Update CoM control frames centerOfMassControlFrame.update(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java index 3211bca6d6b..2e6a01eb71b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java @@ -332,8 +332,6 @@ private void handleNewFootstep(boolean firstTick) balanceManager.addFootstepToPlan(footsteps[i], footstepTimings[i]); } - - // updateHeightManager(); @@ -353,10 +351,7 @@ private void handleNewFootstep(boolean firstTick) // if (feetManager.adjustHeightIfNeeded(nextFootstep)) - { - walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); - } // controllerToolbox.updateBipedSupportPolygons(); @@ -364,6 +359,8 @@ private void handleNewFootstep(boolean firstTick) balanceManager.setSwingFootTrajectory(swingSide, feetManager.getSwingTrajectory(swingSide)); balanceManager.adjustFootstepInCoPPlan(nextFootstep); balanceManager.computeICPPlan(); + + walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); } private boolean haveWeEntered = false; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/messageHandlers/WalkingMessageHandler.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/messageHandlers/WalkingMessageHandler.java index b9d071cf688..3ed53f37acb 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/messageHandlers/WalkingMessageHandler.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/messageHandlers/WalkingMessageHandler.java @@ -337,11 +337,11 @@ else if (upcomingFootsteps.isEmpty()) checkForPause(); + updateVisualization(); + if (hasUpcomingFootsteps()) for (int i = 0; i < footstepConsumptionListenerList.size(); i++) footstepConsumptionListenerList.get(i).doListenerAction(); - - updateVisualization(); } public void handlePauseWalkingCommand(PauseWalkingCommand command) From e72af38b6b2f76ee0c65bbcc719e0b5329ecefa1 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 19 Mar 2025 18:37:36 -0500 Subject: [PATCH 09/17] made ALIPCalculatorTools not completely static. Modified calculation of future control frame. Added feedback alpha interpolation --- .../controller/ICPController.java | 24 ++- .../ALIPCalculatorTools.java | 52 +++++-- .../QuicksterFootstepProvider.java | 137 ++++++++++-------- 3 files changed, 136 insertions(+), 77 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index a94a55bed8e..69b63866fd0 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -15,6 +15,8 @@ import us.ihmc.commonWalkingControlModules.capturePoint.ICPControlPolygons; import us.ihmc.commonWalkingControlModules.capturePoint.ParameterizedICPControlGains; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.commons.InterpolationTools; +import us.ihmc.commons.MathTools; import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.FrameVector2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -108,6 +110,7 @@ public class ICPController implements ICPControllerInterface private final YoBoolean hasNotConvergedInPast = new YoBoolean(yoNamePrefix + "HasNotConvergedInPast", registry); private final YoBoolean previousTickFailed = new YoBoolean(yoNamePrefix + "PreviousTickFailed", registry); private final YoInteger hasNotConvergedCounts = new YoInteger(yoNamePrefix + "HasNotConvergedCounts", registry); + private final YoInteger ticksToInterpolateAlpha = new YoInteger("ticksToInterpolateAlpha", registry); private final IntegerProvider maxNumberOfIterations = new IntegerParameter(yoNamePrefix + "MaxNumberOfIterations", registry, 100); @@ -212,6 +215,8 @@ public ICPController(WalkingControllerParameters walkingControllerParameters, parameters.createFeedbackAlphaCalculator(registry, null); parameters.createFeedbackProjectionOperator(registry, null); + ticksToInterpolateAlpha.set(150); + if (yoGraphicsListRegistry != null) setupVisualizers(yoGraphicsListRegistry); @@ -452,14 +457,31 @@ private void extractSolutionsFromSolver(boolean converged) integrator.update(desiredICPVelocity, currentCoMVelocity, icpError); } + private int alphaInterpolationTicks = 0; private void computeFeedForwardAndFeedBackAlphas() { if (disableCoPFeedback.getValue() || !useCoPFeedbackByDefault.getValue()) - feedbackAlpha.set(1.0); + { + double maxNumberOfAlphaInterpolationTicks = MathTools.clamp(ticksToInterpolateAlpha.getIntegerValue(), 1, Integer.MAX_VALUE); + double interpolationFactor = (double) alphaInterpolationTicks / maxNumberOfAlphaInterpolationTicks; + interpolationFactor = MathTools.clamp(interpolationFactor, 0.0, 1.0); + double startValue = 0.0; //parameters.getFeedbackAlphaCalculator() != null ? parameters.getFeedbackAlphaCalculator().computeAlpha(currentICP, copConstraintHandler.getCoPConstraint()) : 0.0; + double endValue = 1.0; + double valueToUse = InterpolationTools.linearInterpolate(startValue, endValue, interpolationFactor); + feedbackAlpha.set(valueToUse); + if (alphaInterpolationTicks < ticksToInterpolateAlpha.getValue()) + alphaInterpolationTicks++; + } else if (parameters.getFeedbackAlphaCalculator() != null) + { feedbackAlpha.set(parameters.getFeedbackAlphaCalculator().computeAlpha(currentICP, copConstraintHandler.getCoPConstraint())); + alphaInterpolationTicks = 0; + } else + { feedbackAlpha.set(0.0); + alphaInterpolationTicks = 0; + } if (parameters.getFeedForwardAlphaCalculator() != null) feedForwardAlpha.set(parameters.getFeedForwardAlphaCalculator() diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 77989051a8e..880ef2e5971 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -9,16 +9,40 @@ public class ALIPCalculatorTools { private final static double GRAVITY = -9.81; - private final static FramePose3D tempFutureCoMPose = new FramePose3D(); - private final static FrameVector3D tempFutureCoMVelocity = new FrameVector3D(); - private final static FrameVector3D tempFutureContactPointAngularMomentum = new FrameVector3D(); - private final static PoseReferenceFrame tempFutureControlFrame = new PoseReferenceFrame("futureControlFrameALIPCalculator", ReferenceFrame.getWorldFrame()); + private final FramePose3D tempFutureCoMPose = new FramePose3D(); + private final FrameVector3D tempFutureCoMVelocity = new FrameVector3D(); + private final FrameVector3D tempFutureContactPointAngularMomentum = new FrameVector3D(); + private final PoseReferenceFrame tempFutureControlFrame = new PoseReferenceFrame("futureControlFrameALIPCalculator", ReferenceFrame.getWorldFrame()); - private final static FramePose3D tempCurrentCoMPose = new FramePose3D(); - private final static FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); - private final static FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); + private final FramePose3D tempCurrentCoMPose = new FramePose3D(); + private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); + private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); - public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, + public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, + FrameVector3DReadOnly currentContactPointAngularMomentum, + FramePoint3DReadOnly currentStanceFootPosition, + FramePose3D futureCoMPoseToPack, + FrameVector3D futureContactPointAngularMomentumToPack, + ReferenceFrame controlFrame, + double horizonDuration, + double pendulumMass, + double pendulumHeight, + double updateDt) + { + computeFutureStateUsingALIP(currentCoMPose, + currentContactPointAngularMomentum, + currentStanceFootPosition, + futureCoMPoseToPack, + futureContactPointAngularMomentumToPack, + controlFrame, + horizonDuration, + pendulumMass, + pendulumHeight, + 0.0, + updateDt); + } + + public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, FrameVector3DReadOnly currentContactPointAngularMomentum, FramePoint3DReadOnly currentStanceFootPosition, FramePose3D futureCoMPoseToPack, @@ -72,12 +96,13 @@ public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPos int intervals = (int) Math.round(horizonDuration / updateDt); intervals = MathTools.clamp(intervals, 1, Integer.MAX_VALUE); + horizonDuration = MathTools.clamp(horizonDuration, updateDt, Double.POSITIVE_INFINITY); - for (double i = 0; i < intervals ; i ++) + for (int i = 0; i < intervals ; i ++) { futureCoMPoseToPack.appendYawRotation(desiredTurningVelocity * updateDt); - futureCoMPoseToPack.getPosition().addX(xf / intervals); - futureCoMPoseToPack.getPosition().addY(yf / intervals); + futureCoMPoseToPack.getPosition().addX(xf * updateDt / horizonDuration); + futureCoMPoseToPack.getPosition().addY(yf * updateDt / horizonDuration); } futureContactPointAngularMomentumToPack.setX(Lxf); @@ -86,8 +111,6 @@ public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPos futureCoMPoseToPack.changeFrame(originalPositionFrame); futureContactPointAngularMomentumToPack.changeFrame(originalMomentumFrame); - - } // public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentPosition, @@ -138,7 +161,7 @@ public static void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPos // touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); // } - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePose3DReadOnly currentCoMPose, + public void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePose3DReadOnly currentCoMPose, FrameVector3DReadOnly currentContactPointAngularMomentum, FramePoint3DReadOnly currentStanceFootPosition, FramePoint2DBasics touchdownPositionToPack, @@ -171,6 +194,7 @@ public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement desiredTurningVelocity, updateDt); + tempFutureCoMPose.changeFrame(tempFutureControlFrame.getParent()); tempFutureControlFrame.setPoseAndUpdate(tempFutureCoMPose); tempFutureCoMVelocity.changeFrame(tempFutureContactPointAngularMomentum.getReferenceFrame()); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 90507cf47b5..6e17f37ac10 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -15,6 +15,7 @@ import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPosition; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepStatus; +import us.ihmc.log.LogTools; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; @@ -65,7 +66,9 @@ public class QuicksterFootstepProvider private final FrameQuaternion chestOrientation = new FrameQuaternion(); // Foot state information - public enum FootState {SUPPORT, SWING} + public enum FootState + {SUPPORT, SWING} + private final SideDependentList> footStates = new SideDependentList<>(); private final SideDependentList> footStateMachines = new SideDependentList<>(); private final YoEnum newestSupportSide = new YoEnum<>("newestSupportSide" + variableNameSuffix, registry, RobotSide.class); @@ -76,7 +79,9 @@ public enum FootState {SUPPORT, SWING} private final SideDependentList pendulumBase = new SideDependentList<>(); private final FramePoint2D netPendulumBase = new FramePoint2D(); private final SideDependentList pendulumBase3DInWorld = new SideDependentList<>(); - private final YoFramePoint3D netPendulumBase3DInWorld = new YoFramePoint3D("netPendulumBase3DInWorld" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + private final YoFramePoint3D netPendulumBase3DInWorld = new YoFramePoint3D("netPendulumBase3DInWorld" + variableNameSuffix, + ReferenceFrame.getWorldFrame(), + registry); // Temp variables for changing frames and stuff private final FramePoint2DBasics tempPendulumBase = new FramePoint2D(); @@ -89,7 +94,15 @@ public enum FootState {SUPPORT, SWING} private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; private DesiredTurningVelocityProvider desiredTurningVelocityProvider = () -> 0.0; - public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) + // + private final ALIPCalculatorTools alipCalculatorTools = new ALIPCalculatorTools(); + + public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, + double updateDT, + YoRegistry parentRegistry, + YoGraphicsListRegistry yoGraphicsListRegistry, + DoubleProvider yoTime) { this.robotModel = robotModel; this.referenceFrames = referenceFrames; @@ -100,7 +113,6 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano // Parameters parameters = new QuicksterFootstepProviderParameters(gravity, registry); - // Estimates estimates = new QuicksterFootstepProviderEstimates(robotModel, referenceFrames, @@ -113,8 +125,6 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano centerOfMassControlZUpFrame = estimates.getCenterOfMassControlZUpFrame(); - - // Side-dependant stuff, most of the desired-related things for (RobotSide robotSide : RobotSide.values) { @@ -122,28 +132,29 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano pendulumBase.put(robotSide, new FramePoint2D(centerOfMassControlZUpFrame)); - pendulumBase3DInWorld.put(robotSide, new YoFramePoint3D(robotSide.getLowerCaseName() + "PendulumBase3DInWorld" + variableNameSuffix, - ReferenceFrame.getWorldFrame(), - registry)); - - touchdownCalculator.put(robotSide, new QuicksterFootstepProviderTouchdownCalculator(robotSide, - centerOfMassControlZUpFrame, - estimates.getCenterOfMassVelocity(), - estimates.getCenterOfMassAngularMomentum(), - robotModel, - parameters, - gravity, - desiredVelocity, - registry)); + pendulumBase3DInWorld.put(robotSide, + new YoFramePoint3D(robotSide.getLowerCaseName() + "PendulumBase3DInWorld" + variableNameSuffix, + ReferenceFrame.getWorldFrame(), + registry)); + + touchdownCalculator.put(robotSide, + new QuicksterFootstepProviderTouchdownCalculator(robotSide, + centerOfMassControlZUpFrame, + estimates.getCenterOfMassVelocity(), + estimates.getCenterOfMassAngularMomentum(), + robotModel, + parameters, + gravity, + desiredVelocity, + registry)); desiredTouchdownPositionsList.put(robotSide, new RecyclingArrayList<>(FramePoint2D.class)); desiredTouchdownPosesList.put(robotSide, new RecyclingArrayList<>(FramePose2D.class)); - desiredTouchdownPosition3DInWorld.put(robotSide, new YoFramePoint3D(robotSide.getLowerCaseName() + "DesiredTouchdownPosition3DInWorld" + variableNameSuffix, - ReferenceFrame.getWorldFrame(), - registry)); - - + desiredTouchdownPosition3DInWorld.put(robotSide, + new YoFramePoint3D(robotSide.getLowerCaseName() + "DesiredTouchdownPosition3DInWorld" + variableNameSuffix, + ReferenceFrame.getWorldFrame(), + registry)); StateMachineFactory stateMachineFactory = new StateMachineFactory<>(FootState.class); stateMachineFactory.setRegistry(registry).setNamePrefix(robotSide.getLowerCaseName() + variableNameSuffix).buildYoClock(yoTime); @@ -179,7 +190,6 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano } } - if (yoGraphicsListRegistry != null) { YoGraphicPosition netPendulumBaseViz = new YoGraphicPosition("netPendulumBase" + variableNameSuffix, @@ -208,6 +218,7 @@ public void initialize() } private double numberOfFootstepsToPlan; + public void update(int numberOfFootstepsToPlan) { if (firstTick) @@ -250,17 +261,17 @@ private void updateDesireds() private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); private final PoseReferenceFrame tempControlFrame = new PoseReferenceFrame("tempControlFrameQFP", ReferenceFrame.getWorldFrame()); + private boolean haveWeMadeIt = false; - public void calculateTouchdownPosition(RobotSide currentSwingSide, - double timeRemainingInCurrentStep) + public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRemainingInCurrentStep) { desiredTouchdownPositionsList.get(currentSwingSide).clear(); desiredTouchdownPositionsList.get(currentSwingSide.getOppositeSide()).clear(); desiredTouchdownPosesList.get(currentSwingSide).clear(); desiredTouchdownPosesList.get(currentSwingSide.getOppositeSide()).clear(); + haveWeMadeIt = false; - - for (int i = 0; i < numberOfFootstepsToPlan ; i++) + for (int i = 0; i < numberOfFootstepsToPlan; i++) { double timeToReachGoal; RobotSide swingSide; @@ -292,39 +303,39 @@ else if (i % 2 == 0) FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.get(swingSide).add(); FramePose2D desiredTouchdownPose = desiredTouchdownPosesList.get(swingSide).add(); - - ALIPCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement( - tempCurrentCoMPose, - tempCurrentContactPointAngularMomentum, - tempCurrentStanceFootPosition, - desiredTouchdownPositionToPack, - swingSide, - tempControlFrame, - desiredVelocity.getX(), - desiredVelocity.getY(), - desiredTurningVelocity.getDoubleValue(), - parameters.getStanceWidth(swingSide).getDoubleValue(), - timeToReachGoal, - getStepDuration(swingSide), - getTransferDuration(swingSide), - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - parameters.getPole(swingSide).getDoubleValue(), - useFutureCoM, - updateDT); - - ALIPCalculatorTools.computeFutureStateUsingALIP( - tempCurrentCoMPose, - tempCurrentContactPointAngularMomentum, - tempCurrentStanceFootPosition, - tempCurrentCoMPose, - tempCurrentContactPointAngularMomentum, - tempControlFrame, - timeToReachGoal, - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - desiredTurningVelocity.getDoubleValue(), - updateDT); + if (swingSide == currentSwingSide) + haveWeMadeIt = true; + + alipCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempCurrentCoMPose, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + desiredTouchdownPositionToPack, + swingSide, + tempControlFrame, + desiredVelocity.getX(), + desiredVelocity.getY(), + desiredTurningVelocity.getDoubleValue(), + parameters.getStanceWidth(swingSide).getDoubleValue(), + timeToReachGoal, + getStepDuration(swingSide), + getTransferDuration(swingSide), + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + parameters.getPole(swingSide).getDoubleValue(), + useFutureCoM, + updateDT); + + alipCalculatorTools.computeFutureStateUsingALIP(tempCurrentCoMPose, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + tempCurrentCoMPose, + tempCurrentContactPointAngularMomentum, + tempControlFrame, + timeToReachGoal, + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + desiredTurningVelocity.getDoubleValue(), + updateDT); desiredTouchdownPose.getPosition().set(desiredTouchdownPositionToPack); desiredTouchdownPose.getOrientation().setFromReferenceFrame(tempControlFrame); @@ -339,6 +350,8 @@ else if (i % 2 == 0) } } +// if (!haveWeMadeIt) +// LogTools.error("FUCK FUCK FUCK FUCK SOMETHING IS WRONG FUCKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK"); desiredTouchdownPosition3DInWorld.get(currentSwingSide).setMatchingFrame(desiredTouchdownPositionsList.get(currentSwingSide).get(0), 0.0); } From 6bba95432dbd530e4cf4843a9bc1b64dc9d1ed8f Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 20 Mar 2025 17:48:55 -0500 Subject: [PATCH 10/17] added documentation and Gong-based footstep control to ALIPCalculatorTools --- .../ALIPCalculatorTools.java | 284 +++++++++++------- 1 file changed, 169 insertions(+), 115 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 880ef2e5971..78ec8f907ed 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -18,41 +18,21 @@ public class ALIPCalculatorTools private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); + /** + * Using the ALIP reduced-order model, this method computes a predicted future state given an initial + * state, and the time delta between the initial state and the future state. + */ public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, - FrameVector3DReadOnly currentContactPointAngularMomentum, - FramePoint3DReadOnly currentStanceFootPosition, - FramePose3D futureCoMPoseToPack, - FrameVector3D futureContactPointAngularMomentumToPack, - ReferenceFrame controlFrame, - double horizonDuration, - double pendulumMass, - double pendulumHeight, - double updateDt) - { - computeFutureStateUsingALIP(currentCoMPose, - currentContactPointAngularMomentum, - currentStanceFootPosition, - futureCoMPoseToPack, - futureContactPointAngularMomentumToPack, - controlFrame, - horizonDuration, - pendulumMass, - pendulumHeight, - 0.0, - updateDt); - } - - public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, - FrameVector3DReadOnly currentContactPointAngularMomentum, - FramePoint3DReadOnly currentStanceFootPosition, - FramePose3D futureCoMPoseToPack, - FrameVector3D futureContactPointAngularMomentumToPack, - ReferenceFrame controlFrame, - double horizonDuration, - double pendulumMass, - double pendulumHeight, - double desiredTurningVelocity, - double updateDt) + FrameVector3DReadOnly currentContactPointAngularMomentum, + FramePoint3DReadOnly currentStanceFootPosition, + FramePose3D futureCoMPoseToPack, + FrameVector3D futureContactPointAngularMomentumToPack, + ReferenceFrame controlFrame, + double horizonDuration, + double pendulumMass, + double pendulumHeight, + double desiredTurningVelocity, + double updateDt) { tempCurrentCoMPose.setMatchingFrame(currentCoMPose); tempCurrentContactPointAngularMomentum.setMatchingFrame(currentContactPointAngularMomentum); @@ -88,17 +68,15 @@ public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, futureCoMPoseToPack.setIncludingFrame(tempCurrentCoMPose); futureContactPointAngularMomentumToPack.changeFrame(controlFrame); - futureCoMPoseToPack.setX(tempCurrentStanceFootPosition.getX()); futureCoMPoseToPack.setY(tempCurrentStanceFootPosition.getY()); futureCoMPoseToPack.setZ(tempCurrentCoMPose.getZ()); - int intervals = (int) Math.round(horizonDuration / updateDt); intervals = MathTools.clamp(intervals, 1, Integer.MAX_VALUE); horizonDuration = MathTools.clamp(horizonDuration, updateDt, Double.POSITIVE_INFINITY); - for (int i = 0; i < intervals ; i ++) + for (int i = 0; i < intervals; i++) { futureCoMPoseToPack.appendYawRotation(desiredTurningVelocity * updateDt); futureCoMPoseToPack.getPosition().addX(xf * updateDt / horizonDuration); @@ -108,77 +86,94 @@ public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, futureContactPointAngularMomentumToPack.setX(Lxf); futureContactPointAngularMomentumToPack.setY(Lyf); - futureCoMPoseToPack.changeFrame(originalPositionFrame); futureContactPointAngularMomentumToPack.changeFrame(originalMomentumFrame); } -// public static void computeTouchdownPositionRegular(FramePoint3DReadOnly currentPosition, -// FrameVector3DReadOnly currentVelocity, -// FrameVector3DReadOnly currentCentroidalAngularMomentum, -// RobotSide swingSide, -// double desiredVelocityX, -// double desiredVelocityY, -// double desiredStanceWidth, -// double timeRemainingInCurrentStep, -// double stepDuration, -// double pendulumMass, -// double pendulumHeight, -// FramePoint2DBasics touchdownPositionToPack, -// ReferenceFrame stanceFootFrame, -// ReferenceFrame swingFootFrame, -// ReferenceFrame controlFrame) -// { -// double omega = calculateOmega(pendulumHeight); -// -// double LyDesired = pendulumMass * desiredVelocityX * pendulumHeight; -// double LxDesired = computeDesiredAngularMomentumForStanceWidth(swingSide, desiredStanceWidth, pendulumMass, pendulumHeight, stepDuration) -pendulumMass * desiredVelocityY * pendulumHeight; -// -// ////// -// // Get CoM velocity and change frame to CoM control frame -// tempVelocity.setIncludingFrame(currentVelocity); -// tempVelocity.changeFrame(stanceFootFrame); -// -// // Get CoM angular momentum and change frame to CoM control frame -// tempCentroidalAngularMomentum.setIncludingFrame(currentCentroidalAngularMomentum); -// tempCentroidalAngularMomentum.changeFrame(stanceFootFrame); -// ///// -// -// tempCurrentContactPointAngularMomentum.setToZero(stanceFootFrame); -// tempCurrentContactPointAngularMomentum.setY(pendulumMass * pendulumHeight * tempVelocity.getX() + tempCentroidalAngularMomentum.getY()); -// tempCurrentContactPointAngularMomentum.setX(-pendulumMass * pendulumHeight * tempVelocity.getY() + tempCentroidalAngularMomentum.getX()); -// computeFutureStateUsingALIP(currentPosition, tempCurrentContactPointAngularMomentum, tempFuturePosition, tempFutureContactPointAngularMomentum, timeRemainingInCurrentStep, pendulumMass, pendulumHeight, stanceFootFrame); -// -// double LyEndOfCurrentStep = tempFutureContactPointAngularMomentum.getY(); -// double LxEndOfCurrentStep = tempFutureContactPointAngularMomentum.getX(); -// -// double desiredFootstepPositionX = (LyDesired - Math.cosh(omega * stepDuration) * LyEndOfCurrentStep) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); -// double desiredFootstepPositionY = (Math.cosh(omega * stepDuration) * LxEndOfCurrentStep - LxDesired) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); -// -// ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); -// touchdownPositionToPack.setToZero(controlFrame); -// touchdownPositionToPack.set(-desiredFootstepPositionX, -desiredFootstepPositionY); -// touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); -// } + /** + * Using the ALIP reduced-order model and a desired steady-state CoM velocity, this method calculates the + * footstep position needed to achieve the desired steady-state CoM velocity by the end of the following step + */ + public void computeTouchdownPositionRegular(FramePose3DReadOnly currentCoMPose, + FrameVector3DReadOnly currentContactPointAngularMomentum, + FramePoint3DReadOnly currentStanceFootPosition, + FramePoint2DBasics touchdownPositionToPack, + RobotSide swingSide, + ReferenceFrame controlFrame, + double desiredVelocityX, + double desiredVelocityY, + double desiredTurningVelocity, + double desiredStanceWidth, + double timeRemainingInCurrentStep, + double stepDuration, + double pendulumMass, + double pendulumHeight, + boolean useFutureCoM, + double updateDt) + { + double omega = computeNaturalFrequency(pendulumHeight); + + double LyDesired = pendulumMass * desiredVelocityX * pendulumHeight; + double LxDesired = computeDesiredAngularMomentumForStanceWidth(swingSide, desiredStanceWidth, pendulumMass, pendulumHeight, stepDuration) + - pendulumMass * desiredVelocityY * pendulumHeight; + + computeFutureStateUsingALIP(currentCoMPose, + currentContactPointAngularMomentum, + currentStanceFootPosition, + tempFutureCoMPose, + tempFutureContactPointAngularMomentum, + controlFrame, + timeRemainingInCurrentStep, + pendulumMass, + pendulumHeight, + desiredTurningVelocity, + updateDt); + + double LyEndOfCurrentStep = tempFutureContactPointAngularMomentum.getY(); + double LxEndOfCurrentStep = tempFutureContactPointAngularMomentum.getX(); + + double desiredFootstepPositionX = + (LyDesired - Math.cosh(omega * stepDuration) * LyEndOfCurrentStep) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); + double desiredFootstepPositionY = + (Math.cosh(omega * stepDuration) * LxEndOfCurrentStep - LxDesired) / (pendulumMass * pendulumHeight * omega * Math.sinh(omega * stepDuration)); + + ReferenceFrame controlFrameToUse; + if (useFutureCoM) + controlFrameToUse = tempFutureControlFrame; + else + controlFrameToUse = controlFrame; + + ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); + touchdownPositionToPack.setToZero(controlFrameToUse); + touchdownPositionToPack.set(-desiredFootstepPositionX, -desiredFootstepPositionY); + touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); + } + + /** + * Using the ALIP reduced-order model, this method computes the predicted end-of-step state and uses the end-of-step CoM + * velocity to calculate a desired touchdown position for steady state walking using the Raibert Heuristic footstep + * control strategy. In this case the Raibert Heuristic gain is computed via pole placement and the desired closed loop + * pole of our linear inverted pendulum dynamical system + */ public void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FramePose3DReadOnly currentCoMPose, - FrameVector3DReadOnly currentContactPointAngularMomentum, - FramePoint3DReadOnly currentStanceFootPosition, - FramePoint2DBasics touchdownPositionToPack, - RobotSide swingSide, - ReferenceFrame controlFrame, - double desiredVelocityX, - double desiredVelocityY, - double desiredTurningVelocity, - double desiredStanceWidth, - double timeRemainingInCurrentStep, - double stepDuration, - double doubleSupportDuration, - double pendulumMass, - double pendulumHeight, - double pole, - boolean useFutureCoM, - double updateDt) + FrameVector3DReadOnly currentContactPointAngularMomentum, + FramePoint3DReadOnly currentStanceFootPosition, + FramePoint2DBasics touchdownPositionToPack, + RobotSide swingSide, + ReferenceFrame controlFrame, + double desiredVelocityX, + double desiredVelocityY, + double desiredTurningVelocity, + double desiredStanceWidth, + double timeRemainingInCurrentStep, + double stepDuration, + double doubleSupportDuration, + double pendulumMass, + double pendulumHeight, + double pole, + boolean useFutureCoM, + double updateDt) { double omega = computeNaturalFrequency(pendulumHeight); @@ -216,18 +211,47 @@ public void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameP ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); touchdownPositionToPack.changeFrameAndProjectToXYPlane(controlFrameToUse); - touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, 1/timeConstant, desiredVelocityX)); - touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, swingSide.getOppositeSide(), 1/timeConstant, desiredVelocityY)); - touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, doubleSupportDuration, desiredStanceWidth, swingSide.getOppositeSide(), 1/timeConstant)); + touchdownPositionToPack.addX(computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, 1 / timeConstant, desiredVelocityX)); + touchdownPositionToPack.addY(computeLateralTouchdownOffsetForVelocity(swingDuration, + doubleSupportDuration, + swingSide.getOppositeSide(), + 1 / timeConstant, + desiredVelocityY)); + touchdownPositionToPack.addY(computeDesiredTouchdownOffsetForStanceWidth(swingDuration, + doubleSupportDuration, + desiredStanceWidth, + swingSide.getOppositeSide(), + 1 / timeConstant)); touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } - public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, ReferenceFrame controlFrame, FramePoint2DBasics touchdownPositionToPack, double pole, double stepDuration, double omega) + /** + * Given a predicted end-of-step CoM velocity, this method computes a desired touchdown position to achieve + * steady state walking using the Raibert Heuristic footstep control strategy. In this case the Raibert + * Heuristic gain is computed via pole placement and the desired closed loop pole of our linear inverted + * pendulum dynamical system + */ + public static void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameVector3DReadOnly velocity, + ReferenceFrame controlFrame, + FramePoint2DBasics touchdownPositionToPack, + double pole, + double stepDuration, + double omega) { - computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), velocity, controlFrame, touchdownPositionToPack); + computeTouchdownPositionUsingRaibertHeuristic(calculateTimeConstantUsingPolePlacement(pole, stepDuration, omega), + velocity, + controlFrame, + touchdownPositionToPack); } - public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, FrameVector3DReadOnly velocity, ReferenceFrame controlFrame, FramePoint2DBasics touchdownPositionToPack) + /** + * Computes a desired touchdown position to achieve steady state walking using the Raibert Heuristic footstep + * control strategy + */ + public static void computeTouchdownPositionUsingRaibertHeuristic(double timeConstant, + FrameVector3DReadOnly velocity, + ReferenceFrame controlFrame, + FramePoint2DBasics touchdownPositionToPack) { ReferenceFrame originalFrame = touchdownPositionToPack.getReferenceFrame(); touchdownPositionToPack.setToZero(controlFrame); @@ -236,18 +260,20 @@ public static void computeTouchdownPositionUsingRaibertHeuristic(double timeCons touchdownPositionToPack.changeFrameAndProjectToXYPlane(originalFrame); } + /** + * Computes time constant, aka Raibert Heuristic gain, using pole placement in order to dictate the desired + * closed loop dynamic performance of the pendulum system + */ public static double calculateTimeConstantUsingPolePlacement(double pole, double stepDuration, double omega) { return (Math.cosh(omega * stepDuration) - pole) / (omega * Math.sinh(omega * stepDuration)); } - public static double computeDesiredAngularMomentumForStanceWidth(RobotSide swingSide, double desiredStanceWidth, double pendulumMass, double pendulumHeight, double stepDuration) - { - double sideSignMultiplier = swingSide == RobotSide.LEFT ? 1.0 : -1.0; - double omega = computeNaturalFrequency(pendulumHeight); - return sideSignMultiplier * 0.5 * pendulumMass * pendulumHeight * desiredStanceWidth * (omega * Math.sinh(omega * stepDuration)) / (1 + Math.cosh(omega * stepDuration)); - } - + /** + * Computes touchdown position offset in the forward/backwards (x) direction necessary to achieve a + * desired walking speed along that direction. This is used in conjunction with the Raibert Heuristic + * method of touchdown position control + */ private static double computeForwardTouchdownOffsetForVelocity(double swingDuration, double doubleSupportDuration, double omega, double desiredVelocity) { double stepDuration = doubleSupportDuration + swingDuration; @@ -261,6 +287,11 @@ private static double computeForwardTouchdownOffsetForVelocity(double swingDurat return -forwardMultiplier * desiredVelocity * stepDuration / (exponential - 1.0); } + /** + * Computes touchdown position offset in the lateral (y) direction necessary to achieve a desired + * walking speed along that direction. This is used in conjunction with the Raibert Heuristic + * method of touchdown position control + */ private static double computeLateralTouchdownOffsetForVelocity(double swingDuration, double doubleSupportDuration, RobotSide supportSide, @@ -283,6 +314,10 @@ else if (desiredVelocity < 0.0 && supportSide == RobotSide.RIGHT) return lateralOffset; } + /** + * Computes touchdown position offset necessary to achieve a desired stance width while walking. + * This is used in conjunction with the Raibert Heuristic method of touchdown position control + */ public static double computeDesiredTouchdownOffsetForStanceWidth(double swingDuration, double doubleSupportDuration, double stanceWidth, @@ -299,6 +334,25 @@ public static double computeDesiredTouchdownOffsetForStanceWidth(double swingDur return supportSide.negateIfLeftSide(stepWidthOffset); } + /** + * Computes desired angular momentum reference required to achieve a desired stance width while walking. + * This is used in conjunction with the Gong method of touchdown position control + */ + public static double computeDesiredAngularMomentumForStanceWidth(RobotSide swingSide, + double desiredStanceWidth, + double pendulumMass, + double pendulumHeight, + double stepDuration) + { + double sideSignMultiplier = swingSide == RobotSide.LEFT ? 1.0 : -1.0; + double omega = computeNaturalFrequency(pendulumHeight); + return sideSignMultiplier * 0.5 * pendulumMass * pendulumHeight * desiredStanceWidth * (omega * Math.sinh(omega * stepDuration)) / (1 + Math.cosh( + omega * stepDuration)); + } + + /** + * Computes natural frequency of Linear Inverted Pendulum + */ public static double computeNaturalFrequency(double pendulumHeight) { return Math.sqrt(Math.abs(GRAVITY / pendulumHeight)); From 8fe7701b544279bb1eb6d52ec36fb9e4eca51be3 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 26 Mar 2025 11:21:15 -0500 Subject: [PATCH 11/17] first qfp step uses regular controller (longer) transfer duration to improve stability --- .../controller/ICPController.java | 2 +- .../ContinuousStepGenerator.java | 19 +++++++++-- .../QuicksterFootstepProvider.java | 34 ++++++++++++++----- 3 files changed, 44 insertions(+), 11 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index 69b63866fd0..222d8cfcc99 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -215,7 +215,7 @@ public ICPController(WalkingControllerParameters walkingControllerParameters, parameters.createFeedbackAlphaCalculator(registry, null); parameters.createFeedbackProjectionOperator(registry, null); - ticksToInterpolateAlpha.set(150); + ticksToInterpolateAlpha.set(75); if (yoGraphicsListRegistry != null) setupVisualizers(yoGraphicsListRegistry); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index d547b6bdbe2..4e2d8e4df9c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -163,6 +163,7 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder private final YoEnum currentCSGMode = new YoEnum<>("currentCSGMode", registry, ContinuousStepGeneratorMode.class); private final YoEnum requestedCSGMode = new YoEnum<>("requestedCSGMode", registry, ContinuousStepGeneratorMode.class); private final OptionalFactoryField quicksterFootstepProvider = new OptionalFactoryField<>("QuicksterFootstepProviderField"); + private boolean useQFPParameters = false; /** * Creates a new step generator, its {@code YoVariable}s will not be attached to any registry. @@ -290,9 +291,21 @@ else if (statusToProcess == FootstepStatus.COMPLETED) // Set footstep parameters if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) { + if (useQFPParameters) + { + quicksterFootstepProvider.get().setUseAlternateTransferDuration(false); + footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + } + else + { + quicksterFootstepProvider.get().setUseAlternateTransferDuration(true); + quicksterFootstepProvider.get().setAlternateTransferDuration(parameters.getTransferDuration()); + footstepDataListMessage.setDefaultTransferDuration(parameters.getTransferDuration()); + footstepDataListMessage.setFinalTransferDuration(parameters.getTransferDuration()); + } + footstepDataListMessage.setDefaultSwingDuration(quicksterFootstepProvider.get().getSwingDuration(swingSide)); - footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); - footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); footstepDataListMessage.setAreFootstepsAdjustable(false); footstepDataListMessage.setOffsetFootstepsWithExecutionError(false); } @@ -674,6 +687,8 @@ public void notifyFootstepStarted(RobotSide robotSide) { latestStatusReceived.setValue(FootstepStatus.STARTED); currentSupportSide.set(robotSide.getOppositeSide()); + + useQFPParameters = currentCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.QFP); } /** diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 6e17f37ac10..391017f7238 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -38,8 +38,6 @@ public class QuicksterFootstepProvider { private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); - private final QuicksterFootstepProviderParameters parameters; - private final String variableNameSuffix = "QFP"; private final FullHumanoidRobotModel robotModel; @@ -48,6 +46,11 @@ public class QuicksterFootstepProvider private final double updateDT; private final double gravity = 9.81; + // Parameters + private final QuicksterFootstepProviderParameters parameters; + private boolean useAlternateTransferDuration = false; + private double alternateTransferDuration = 0.01; + // Estimates private final QuicksterFootstepProviderEstimates estimates; private final MovingZUpFrame centerOfMassControlZUpFrame; @@ -233,7 +236,8 @@ public void update(int numberOfFootstepsToPlan) private void updateEstimates() { - desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); + initialize(); + desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * 10 * updateDT); inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); @@ -337,11 +341,12 @@ else if (i % 2 == 0) desiredTurningVelocity.getDoubleValue(), updateDT); + tempCurrentCoMPose.changeFrame(tempControlFrame.getParent()); + tempControlFrame.setPoseAndUpdate(tempCurrentCoMPose); + desiredTouchdownPose.getPosition().set(desiredTouchdownPositionToPack); desiredTouchdownPose.getOrientation().setFromReferenceFrame(tempControlFrame); - tempControlFrame.setPoseAndUpdate(tempCurrentCoMPose); - tempCurrentStanceFootPosition.setMatchingFrame(desiredTouchdownPositionToPack, 0.0); if (!useFutureCoM) { @@ -350,8 +355,8 @@ else if (i % 2 == 0) } } -// if (!haveWeMadeIt) -// LogTools.error("FUCK FUCK FUCK FUCK SOMETHING IS WRONG FUCKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK"); + if (!haveWeMadeIt) + LogTools.error("FUCK FUCK FUCK FUCK SOMETHING IS WRONG FUCKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK"); desiredTouchdownPosition3DInWorld.get(currentSwingSide).setMatchingFrame(desiredTouchdownPositionsList.get(currentSwingSide).get(0), 0.0); } @@ -535,7 +540,20 @@ public double getSwingDuration(RobotSide swingSide) public double getTransferDuration(RobotSide swingSide) { - return parameters.getSwingDuration(swingSide).getDoubleValue() * parameters.getDoubleSupportFraction(swingSide).getDoubleValue(); + if (useAlternateTransferDuration) + return alternateTransferDuration; + else + return parameters.getSwingDuration(swingSide).getDoubleValue() * parameters.getDoubleSupportFraction(swingSide).getDoubleValue(); + } + + public void setAlternateTransferDuration(double alternateTransferDuration) + { + this.alternateTransferDuration = alternateTransferDuration; + } + + public void setUseAlternateTransferDuration(boolean useAlternateTransferDuration) + { + this.useAlternateTransferDuration = useAlternateTransferDuration; } public double getStepDuration(RobotSide swingSide) From feaff72570992abee58d94dbdda25443c59fb425 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 28 Mar 2025 14:32:56 -0500 Subject: [PATCH 12/17] added visualizers for future state. Added some double support consideration --- .../ContinuousStepGenerator.java | 92 ++++++------ .../ALIPCalculatorTools.java | 13 +- .../QuicksterFootstepProvider.java | 132 ++++++++++++------ 3 files changed, 142 insertions(+), 95 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index 4e2d8e4df9c..46d2b10f839 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -187,6 +187,12 @@ public ContinuousStepGenerator(YoRegistry parentRegistry) numberOfTicksBeforeSubmittingFootsteps.set(0); currentFootstepDataListCommandID.set(new Random().nextLong(0, Long.MAX_VALUE / 2)); // To make this command ID unique + requestedCSGMode.addListener(change -> + { +// if (requestedCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.STANDARD)) +// useQFPParameters = false; + }); + currentCSGMode.addListener(change -> { if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP) @@ -288,23 +294,32 @@ else if (statusToProcess == FootstepStatus.COMPLETED) previousFootstepPose.set(previousFootstep.getLocation(), previousFootstep.getOrientation()); } - // Set footstep parameters - if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) + if (quicksterFootstepProvider.hasValue()) { + if (currentCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.QFP)) + swingSide = quicksterFootstepProvider.get().getCurrentSwingSide(); + if (useQFPParameters) - { quicksterFootstepProvider.get().setUseAlternateTransferDuration(false); - footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); - footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); - } else { quicksterFootstepProvider.get().setUseAlternateTransferDuration(true); quicksterFootstepProvider.get().setAlternateTransferDuration(parameters.getTransferDuration()); - footstepDataListMessage.setDefaultTransferDuration(parameters.getTransferDuration()); - footstepDataListMessage.setFinalTransferDuration(parameters.getTransferDuration()); } + // If in standard mode, keep initializing QFP so its control frame matches pelvis yaw + if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD) + quicksterFootstepProvider.get().initialize(); + + // Continuously update QFP for data visualization purposes + quicksterFootstepProvider.get().update(parameters.getNumberOfFootstepsToPlan()); + } + + // Set footstep parameters + if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) + { + footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); footstepDataListMessage.setDefaultSwingDuration(quicksterFootstepProvider.get().getSwingDuration(swingSide)); footstepDataListMessage.setAreFootstepsAdjustable(false); footstepDataListMessage.setOffsetFootstepsWithExecutionError(false); @@ -348,17 +363,6 @@ else if (statusToProcess == FootstepStatus.COMPLETED) this.desiredTurningVelocity.set(turningVelocity); int startingIndexToAdjust = footsteps.size(); - - // Continuously update QFP for data visualization purposes - if (quicksterFootstepProvider.hasValue()) - { - // If in standard mode, keep initializing QFP so its control frame matches pelvis yaw - if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD) - quicksterFootstepProvider.get().initialize(); - - quicksterFootstepProvider.get().update(parameters.getNumberOfFootstepsToPlan()); - } - int perFootIndex = 0; for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++) @@ -405,7 +409,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD) footstep.setSwingHeight(parameters.getSwingHeight()); - else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP) + else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) footstep.setSwingHeight(quicksterFootstepProvider.get().getSwingHeight(swingSide)); else footstep.setSwingHeight(swingHeightInputProvider.getValue()); @@ -436,30 +440,30 @@ else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == Co } // run through and make sure these adjusted steps are valid. - for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++) - { - FootstepDataMessage footstepData = footstepDataListMessage.getFootstepDataList().get(i); - nextFootstepPose2D.getPosition().set(footstepData.getLocation()); - nextFootstepPose2D.getOrientation().set(footstepData.getOrientation()); - nextFootstepPose3D.getPosition().set(footstepData.getLocation()); - nextFootstepPose3D.getOrientation().set(footstepData.getOrientation()); - swingSide = RobotSide.fromByte(footstepData.getRobotSide()); - - if (!isStepValid(nextFootstepPose3D, previousFootstepPose, swingSide)) - { - alternateStepChooser.computeStep(footstepPose2D, nextFootstepPose2D, swingSide, footstepData); - - // remove all the other steps after the invalid one. - while (footstepDataListMessage.getFootstepDataList().size() > i + 1) - { - footstepDataListMessage.getFootstepDataList().remove(i + 1); - } - } - - previousFootstepPose.getPosition().set(footstepData.getLocation()); - previousFootstepPose.getOrientation().set(footstepData.getOrientation()); - footstepPose2D.set(previousFootstepPose); - } +// for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++) +// { +// FootstepDataMessage footstepData = footstepDataListMessage.getFootstepDataList().get(i); +// nextFootstepPose2D.getPosition().set(footstepData.getLocation()); +// nextFootstepPose2D.getOrientation().set(footstepData.getOrientation()); +// nextFootstepPose3D.getPosition().set(footstepData.getLocation()); +// nextFootstepPose3D.getOrientation().set(footstepData.getOrientation()); +// swingSide = RobotSide.fromByte(footstepData.getRobotSide()); +// +// if (!isStepValid(nextFootstepPose3D, previousFootstepPose, swingSide)) +// { +// alternateStepChooser.computeStep(footstepPose2D, nextFootstepPose2D, swingSide, footstepData); +// +// // remove all the other steps after the invalid one. +// while (footstepDataListMessage.getFootstepDataList().size() > i + 1) +// { +// footstepDataListMessage.getFootstepDataList().remove(i + 1); +// } +// } +// +// previousFootstepPose.getPosition().set(footstepData.getLocation()); +// previousFootstepPose.getOrientation().set(footstepData.getOrientation()); +// footstepPose2D.set(previousFootstepPose); +// } // Update the visualizers for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index 78ec8f907ed..fb68bc87d8c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -42,17 +42,13 @@ public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, tempCurrentContactPointAngularMomentum.changeFrame(controlFrame); tempCurrentStanceFootPosition.changeFrame(controlFrame); - tempCurrentCoMPose.setX(-tempCurrentStanceFootPosition.getX()); - tempCurrentCoMPose.setY(-tempCurrentStanceFootPosition.getY()); - tempCurrentCoMPose.setZ(-tempCurrentStanceFootPosition.getZ()); - double omega = computeNaturalFrequency(pendulumHeight); // Current position and angular momentum - double x0 = tempCurrentCoMPose.getX(); + double x0 = tempCurrentCoMPose.getX() - tempCurrentStanceFootPosition.getX(); double Ly0 = tempCurrentContactPointAngularMomentum.getY(); - double y0 = tempCurrentCoMPose.getY(); + double y0 = tempCurrentCoMPose.getY() - tempCurrentStanceFootPosition.getY(); double Lx0 = tempCurrentContactPointAngularMomentum.getX(); // Final position and angular momentum @@ -66,11 +62,10 @@ public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, ReferenceFrame originalMomentumFrame = futureContactPointAngularMomentumToPack.getReferenceFrame(); futureCoMPoseToPack.setIncludingFrame(tempCurrentCoMPose); - futureContactPointAngularMomentumToPack.changeFrame(controlFrame); - futureCoMPoseToPack.setX(tempCurrentStanceFootPosition.getX()); futureCoMPoseToPack.setY(tempCurrentStanceFootPosition.getY()); - futureCoMPoseToPack.setZ(tempCurrentCoMPose.getZ()); + + futureContactPointAngularMomentumToPack.changeFrame(controlFrame); int intervals = (int) Math.round(horizonDuration / updateDt); intervals = MathTools.clamp(intervals, 1, Integer.MAX_VALUE); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 391017f7238..6aad6cf9e34 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -1,7 +1,6 @@ package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; import controller_msgs.msg.dds.FootstepStatusMessage; -import us.ihmc.commonWalkingControlModules.controllers.Updatable; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.*; import us.ihmc.commons.MathTools; import us.ihmc.commons.lists.RecyclingArrayList; @@ -97,9 +96,13 @@ public enum FootState private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; private DesiredTurningVelocityProvider desiredTurningVelocityProvider = () -> 0.0; - // + // ALIP footstep calculator private final ALIPCalculatorTools alipCalculatorTools = new ALIPCalculatorTools(); + // Visualizers + private final YoFramePoint3D nextStanceFootPosition = new YoFramePoint3D("nextStanceFootPositionQFP", ReferenceFrame.getWorldFrame(), registry); + private final YoFramePoint3D nextCoMPosition = new YoFramePoint3D("nextCoMPositionQFP", ReferenceFrame.getWorldFrame(), registry); + public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, @@ -165,8 +168,10 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, stateMachineFactory.addState(FootState.SUPPORT, new SupportFootState(robotSide)); stateMachineFactory.addState(FootState.SWING, new SwingFootState(robotSide)); + stateMachineFactory.addDoneTransition(FootState.SUPPORT, FootState.SWING); + footStateMachines.put(robotSide, stateMachineFactory.build(FootState.SUPPORT)); - footStateMachines.get(robotSide).resetToInitialState(); +// footStateMachines.get(robotSide).resetToInitialState(); if (yoGraphicsListRegistry != null) { @@ -195,6 +200,20 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, if (yoGraphicsListRegistry != null) { + YoGraphicPosition nextStanceFootGraphic = new YoGraphicPosition("nextStanceFootGraphic" + variableNameSuffix, + nextStanceFootPosition, + 0.09, + YoAppearance.Purple()); + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, nextStanceFootGraphic); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, nextStanceFootGraphic.createArtifact()); + + YoGraphicPosition nextCoMGraphic = new YoGraphicPosition("nextCoMGraphic" + variableNameSuffix, + nextCoMPosition, + 0.09, + YoAppearance.Red()); + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, nextCoMGraphic); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, nextCoMGraphic.createArtifact()); + YoGraphicPosition netPendulumBaseViz = new YoGraphicPosition("netPendulumBase" + variableNameSuffix, netPendulumBase3DInWorld, 0.015, @@ -265,15 +284,13 @@ private void updateDesireds() private final FrameVector3D tempCurrentContactPointAngularMomentum = new FrameVector3D(); private final FramePoint3D tempCurrentStanceFootPosition = new FramePoint3D(); private final PoseReferenceFrame tempControlFrame = new PoseReferenceFrame("tempControlFrameQFP", ReferenceFrame.getWorldFrame()); - private boolean haveWeMadeIt = false; - public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRemainingInCurrentStep) + public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRemainingInCurrentStep, boolean inDoubleSupport) { desiredTouchdownPositionsList.get(currentSwingSide).clear(); desiredTouchdownPositionsList.get(currentSwingSide.getOppositeSide()).clear(); desiredTouchdownPosesList.get(currentSwingSide).clear(); desiredTouchdownPosesList.get(currentSwingSide.getOppositeSide()).clear(); - haveWeMadeIt = false; for (int i = 0; i < numberOfFootstepsToPlan; i++) { @@ -286,7 +303,7 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRe tempCurrentCoMPose.setFromReferenceFrame(centerOfMassControlZUpFrame); tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide())); - tempControlFrame.setTransformAndUpdate(centerOfMassControlZUpFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); + tempControlFrame.setTransformAndUpdate(centerOfMassControlZUpFrame.getTransformToDesiredFrame(tempControlFrame.getParent())); swingSide = currentSwingSide; timeToReachGoal = timeRemainingInCurrentStep; @@ -307,27 +324,28 @@ else if (i % 2 == 0) FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.get(swingSide).add(); FramePose2D desiredTouchdownPose = desiredTouchdownPosesList.get(swingSide).add(); - if (swingSide == currentSwingSide) - haveWeMadeIt = true; - - alipCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempCurrentCoMPose, - tempCurrentContactPointAngularMomentum, - tempCurrentStanceFootPosition, - desiredTouchdownPositionToPack, - swingSide, - tempControlFrame, - desiredVelocity.getX(), - desiredVelocity.getY(), - desiredTurningVelocity.getDoubleValue(), - parameters.getStanceWidth(swingSide).getDoubleValue(), - timeToReachGoal, - getStepDuration(swingSide), - getTransferDuration(swingSide), - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - parameters.getPole(swingSide).getDoubleValue(), - useFutureCoM, - updateDT); + + if (inDoubleSupport && i == 0) + desiredTouchdownPositionToPack.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide)); + else + alipCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempCurrentCoMPose, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + desiredTouchdownPositionToPack, + swingSide, + tempControlFrame, + desiredVelocity.getX(), + desiredVelocity.getY(), + desiredTurningVelocity.getDoubleValue(), + parameters.getStanceWidth(swingSide).getDoubleValue(), + timeToReachGoal, + getStepDuration(swingSide), + getTransferDuration(swingSide), + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + parameters.getPole(swingSide).getDoubleValue(), + useFutureCoM, + updateDT); alipCalculatorTools.computeFutureStateUsingALIP(tempCurrentCoMPose, tempCurrentContactPointAngularMomentum, @@ -348,16 +366,27 @@ else if (i % 2 == 0) desiredTouchdownPose.getOrientation().setFromReferenceFrame(tempControlFrame); tempCurrentStanceFootPosition.setMatchingFrame(desiredTouchdownPositionToPack, 0.0); - if (!useFutureCoM) + + if (i == 0 && !useFutureCoM) { - tempCurrentStanceFootPosition.sub(estimates.getCenterOfMassPosition()); - tempCurrentStanceFootPosition.add(tempCurrentCoMPose.getPosition()); + tempCurrentStanceFootPosition.changeFrame(centerOfMassControlZUpFrame); + double stanceFootX = tempCurrentStanceFootPosition.getX(); + double stanceFootY = tempCurrentStanceFootPosition.getY(); + double stanceFootZ = tempCurrentStanceFootPosition.getZ(); + + tempCurrentStanceFootPosition.changeFrame(tempControlFrame); + tempCurrentStanceFootPosition.set(stanceFootX, stanceFootY, stanceFootZ); + tempCurrentStanceFootPosition.changeFrame(tempControlFrame.getParent()); + + nextStanceFootPosition.setMatchingFrame(tempCurrentStanceFootPosition); + nextCoMPosition.setMatchingFrame(tempCurrentCoMPose.getPosition()); } } - if (!haveWeMadeIt) - LogTools.error("FUCK FUCK FUCK FUCK SOMETHING IS WRONG FUCKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK"); - desiredTouchdownPosition3DInWorld.get(currentSwingSide).setMatchingFrame(desiredTouchdownPositionsList.get(currentSwingSide).get(0), 0.0); + if (desiredTouchdownPositionsList.get(currentSwingSide).size() > 0) + desiredTouchdownPosition3DInWorld.get(currentSwingSide).setMatchingFrame(desiredTouchdownPositionsList.get(currentSwingSide).get(0), 0.0); + else + LogTools.error("Something may be wrong in QFP"); } private void calculateNetPendulumBase() @@ -461,12 +490,7 @@ else if (status == FootstepStatus.STARTED) */ public void notifyFootstepCompleted(RobotSide robotSide) { - footStates.get(robotSide).set(FootState.SUPPORT); footStateMachines.get(robotSide).performTransition(FootState.SUPPORT); - newestSupportSide.set(robotSide); - - footStates.get(robotSide.getOppositeSide()).set(FootState.SWING); - footStateMachines.get(robotSide.getOppositeSide()).performTransition(FootState.SWING); } /** @@ -478,6 +502,11 @@ public void notifyFootstepCompleted(RobotSide robotSide) */ public void notifyFootstepStarted(RobotSide robotSide) { + if (!footStateMachines.get(robotSide).getCurrentStateKey().equals(FootState.SWING)) + footStateMachines.get(robotSide).performTransition(FootState.SWING); + + if (!footStateMachines.get(robotSide.getOppositeSide()).getCurrentStateKey().equals(FootState.SUPPORT)) + footStateMachines.get(robotSide.getOppositeSide()).performTransition(FootState.SUPPORT); } /** @@ -566,6 +595,11 @@ public double getSwingHeight(RobotSide swingSide) return parameters.getSwingHeight(swingSide).getDoubleValue(); } + public RobotSide getCurrentSwingSide() + { + return newestSupportSide.getEnumValue().getOppositeSide(); + } + public YoRegistry getRegistry() { return registry; @@ -583,15 +617,27 @@ private SupportFootState(RobotSide robotSide) @Override public void onEntry() { + newestSupportSide.set(robotSide); + footStates.get(robotSide).set(FootState.SUPPORT); + pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + + if (robotSide == newestSupportSide.getEnumValue() && inDoubleSupport.getValue()) + calculateTouchdownPosition(robotSide, getTransferDuration(robotSide), true); } @Override public void doAction(double timeInState) { + double timeToReachGoal = getStepDuration(robotSide) - timeInState; + timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getTransferDuration(robotSide)); + pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + + if (robotSide == newestSupportSide.getEnumValue() && inDoubleSupport.getValue()) + calculateTouchdownPosition(robotSide, timeToReachGoal, true); } @Override @@ -603,7 +649,7 @@ public void onExit(double timeInState) @Override public boolean isDone(double timeInState) { - return false; + return inDoubleSupport.getValue() && (timeInState - getSwingDuration(robotSide)) > getTransferDuration(robotSide) && !robotSide.equals(newestSupportSide.getEnumValue()); } } @@ -621,7 +667,9 @@ private SwingFootState(RobotSide robotSide) @Override public void onEntry() { - calculateTouchdownPosition(robotSide, getStepDuration(robotSide)); + footStates.get(robotSide).set(FootState.SWING); + + calculateTouchdownPosition(robotSide, getStepDuration(robotSide), false); parameters.setParametersForUpcomingSwing(robotSide); } @@ -633,7 +681,7 @@ public void doAction(double timeInState) timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getStepDuration(robotSide)); this.timeToReachGoal.set(timeToReachGoal); - calculateTouchdownPosition(robotSide, timeToReachGoal); + calculateTouchdownPosition(robotSide, timeToReachGoal, false); } @Override From 7286f87b0e8a0051fadfbe1ba4b36b8d2c572f7f Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 1 Apr 2025 15:13:11 -0500 Subject: [PATCH 13/17] added elliptical reachability constraint to csg --- .../EllipticalStepPositionLimiter.java | 258 ++++++++++++++++ .../ContinuousStepGenerator.java | 147 +++++++-- .../QuicksterFootstepProviderParameters.java | 2 +- ...edFootstepDataMessageGeneratorFactory.java | 1 + .../EllipticalStepPositionLimiterTest.java | 278 ++++++++++++++++++ 5 files changed, 660 insertions(+), 26 deletions(-) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java create mode 100644 ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiterTest.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java new file mode 100644 index 00000000000..8752edc7a2a --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java @@ -0,0 +1,258 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep; + +import us.ihmc.commons.MathTools; +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.robotics.geometry.GroundPlaneEstimator; +import us.ihmc.robotics.robotSide.RobotSide; + +public class EllipticalStepPositionLimiter +{ + public final static double NOMINAL_STANCE_WIDTH_DEFAULT = 0.25; + public final static double MAX_STEP_FORWARD_DEFAULT = 0.85; + public final static double MIN_STANCE_WIDTH_DEFAULT = 0.075; + public final static double MAX_STANCE_WIDTH_DEFAULT = 0.75; + public final static double MIN_DISTANCE_FROM_STANCE_FOOT_DEFAULT = 0.075; + + private final FramePoint2D stanceFootPosition = new FramePoint2D(); + private final FramePoint2D constrainedTouchdownPosition2D = new FramePoint2D(); + private final FramePoint3DBasics constrainedSoleTouchdownPosition = new FramePoint3D(); + + public void enforceFootPositionConstraint(FramePoint3DReadOnly desiredSoleTouchdownPosition, + FixedFramePoint3DBasics constrainedSoleTouchdownPositionToPack, + ReferenceFrame desiredConstraintFrame, + ReferenceFrame stanceFootZUpFrame, + RobotSide swingSide) + { + enforceFootPositionConstraint(desiredSoleTouchdownPosition, + constrainedSoleTouchdownPositionToPack, + desiredConstraintFrame, + stanceFootZUpFrame, + NOMINAL_STANCE_WIDTH_DEFAULT, + MAX_STEP_FORWARD_DEFAULT, + MIN_STANCE_WIDTH_DEFAULT, + MAX_STANCE_WIDTH_DEFAULT, + MIN_DISTANCE_FROM_STANCE_FOOT_DEFAULT, + swingSide); + } + + /** + * @param desiredSoleTouchdownPosition The unconstrained position. Not Modified. + * @param constrainedSoleTouchdownPositionToPack The step position that is constrained. + * @param desiredConstraintFrame The frame in which to apply the constraint. + */ + public boolean enforceFootPositionConstraint(FramePoint3DReadOnly desiredSoleTouchdownPosition, + FixedFramePoint3DBasics constrainedSoleTouchdownPositionToPack, + ReferenceFrame desiredConstraintFrame, + ReferenceFrame stanceFootZUpFrame, + double nominalStanceWidth, + double maxStepForward, + double minStanceWidth, + double maxStanceWidth, + double minDistanceFromStanceFoot, + RobotSide swingSide) + { + return enforceFootPositionConstraint(desiredSoleTouchdownPosition, + constrainedSoleTouchdownPositionToPack, + desiredConstraintFrame, + stanceFootZUpFrame, + null, + nominalStanceWidth, + maxStepForward, + minStanceWidth, + maxStanceWidth, + minDistanceFromStanceFoot, + swingSide); + } + + /** + * @param desiredSoleTouchdownPosition The unconstrained position. Not Modified. + * @param constrainedSoleTouchdownPositionToPack The step position that is constrained. + * @param desiredConstraintFrame The frame in which to apply the constraint. + */ + public boolean enforceFootPositionConstraint(FramePoint3DReadOnly desiredSoleTouchdownPosition, + FixedFramePoint3DBasics constrainedSoleTouchdownPositionToPack, + ReferenceFrame desiredConstraintFrame, + ReferenceFrame stanceFootZUpFrame, + GroundPlaneEstimator groundPlaneEstimator, + double nominalStanceWidth, + double maxStepForward, + double minStanceWidth, + double maxStanceWidth, + double minDistanceFromStanceFoot, + RobotSide swingSide) + { + constrainedSoleTouchdownPosition.setIncludingFrame(desiredSoleTouchdownPosition); + + boolean wasLimited = false; + wasLimited |= enforceMinimumStepWidthInConstraintFrame(constrainedSoleTouchdownPosition, desiredConstraintFrame, minStanceWidth, swingSide); + + wasLimited |= enforceOuterEllipticalBoundInConstraintFrame(constrainedSoleTouchdownPosition, + desiredConstraintFrame, + nominalStanceWidth, + maxStepForward, + maxStanceWidth, + swingSide); + + wasLimited |= enforceStepDistanceFromStance(constrainedSoleTouchdownPosition, stanceFootZUpFrame, minDistanceFromStanceFoot); + + wasLimited |= enforceStepWidthFromStance(constrainedSoleTouchdownPosition, desiredConstraintFrame, stanceFootZUpFrame, minStanceWidth, swingSide); + + //TODO do i need to do this on line 90? + constrainedSoleTouchdownPosition.changeFrame(desiredConstraintFrame); + + //TODO limit z position as well + if (groundPlaneEstimator != null) + { + constrainedTouchdownPosition2D.setIncludingFrame(constrainedSoleTouchdownPosition); + // constrainedSoleTouchdownPosition.setMatchingFrame(groundPlaneEstimator.getGroundPosition(constrainedTouchdownPosition2D)); + } + + constrainedSoleTouchdownPositionToPack.setMatchingFrame(constrainedSoleTouchdownPosition); + return wasLimited; + } + + static boolean enforceMinimumStepWidthInConstraintFrame(FramePoint3DBasics desiredStepPositionToConstrain, + ReferenceFrame desiredConstraintFrame, + double minStanceWidth, + RobotSide swingSide) + { + desiredStepPositionToConstrain.changeFrame(desiredConstraintFrame); + + // make sure you don't step too narrowly. This is done by cropping the lateral position of the footstep. + // If the width is negative, we assume that we're allowing a cross over distance relative to the constraint frame. If it's positive, then that's the + // nominal stance width, as defined by one foot relative to the other. This means we then divide it by half when computing this value. + double desiredWidth = desiredStepPositionToConstrain.getY(); + double widthBound = minStanceWidth < 0.0 ? minStanceWidth : 0.5 * minStanceWidth; + if (swingSide == RobotSide.LEFT) + { + if (desiredWidth < widthBound) + { + desiredStepPositionToConstrain.setY(widthBound); + return true; + } + } + else + { + if (desiredWidth > -widthBound) + { + desiredStepPositionToConstrain.setY(-widthBound); + return true; + } + } + + return false; + } + + /** + * The constraint frame is assumed to be the center of the pelvis, or the center of mass frame. So the ellipse is centered half the nominal stance width from + * the constraint frame. + * @param desiredStepPositionToConstrain + * @param constraintFrame + * @param nominalStanceWidth + * @param maxStepForward + * @param maxStanceWidth + * @param stepSide + * @return + */ + static boolean enforceOuterEllipticalBoundInConstraintFrame(FramePoint3DBasics desiredStepPositionToConstrain, + ReferenceFrame constraintFrame, + double nominalStanceWidth, + double maxStepForward, + double maxStanceWidth, + RobotSide stepSide) + { + ReferenceFrame originalFrame = desiredStepPositionToConstrain.getReferenceFrame(); + desiredStepPositionToConstrain.changeFrame(constraintFrame); + + double ellipseYOrigin = stepSide.negateIfRightSide(0.5 * nominalStanceWidth); + double ellipseYMax = maxStanceWidth - nominalStanceWidth; + + double ellipseX = desiredStepPositionToConstrain.getX(); + double ellipseY = desiredStepPositionToConstrain.getY() - ellipseYOrigin; + + // check if the point is inside the ellipse + double r = MathTools.square(ellipseX / maxStepForward) + MathTools.square(ellipseY / ellipseYMax); + + if (r <= 1.0) + { + desiredStepPositionToConstrain.changeFrame(originalFrame); + return false; + } + + // project the constraint onto the ellipse bound, since it was outside + double theta = Math.atan2(desiredStepPositionToConstrain.getX(), ellipseYMax); + double sinTheta = Math.sin(theta); + double cosTheta = Math.cos(theta); + double k = maxStepForward * ellipseYMax / (Math.sqrt(MathTools.square(ellipseYMax * sinTheta) + MathTools.square(maxStepForward * cosTheta))); + double x = Math.abs(k * sinTheta) * Math.signum(ellipseX); + double y = Math.abs(k * cosTheta) * Math.signum(ellipseY); + + y += ellipseYOrigin; + + desiredStepPositionToConstrain.set(x, y, desiredStepPositionToConstrain.getZ()); + desiredStepPositionToConstrain.changeFrame(originalFrame); + + return true; + } + + private static boolean enforceStepDistanceFromStance(FramePoint3DBasics desiredTouchdownInStanceFootToPack, + ReferenceFrame stanceFootZUpFrame, + double minDistanceFromStanceFoot) + { + // make sure you don't step too close to the stance foot. This is done by making sure the desired touchdown position stays outside a circle + // around the stance foot. + if (minDistanceFromStanceFoot > 0.0) + { + desiredTouchdownInStanceFootToPack.changeFrame(stanceFootZUpFrame); + double distanceFromStance = desiredTouchdownInStanceFootToPack.distanceXY(EuclidCoreTools.origin2D); + if (distanceFromStance < minDistanceFromStanceFoot) + { + double scaleFactor = minDistanceFromStanceFoot / distanceFromStance; + desiredTouchdownInStanceFootToPack.scale(scaleFactor, scaleFactor, 1.0); + + return true; + } + } + + return false; + } + + private boolean enforceStepWidthFromStance(FramePoint3DBasics desiredStepPositionToConstrain, + ReferenceFrame desiredConstraintFrame, + ReferenceFrame stanceFootZUpFrame, + double minStanceWidth, + RobotSide swingSide) + { + // This makes sure step is no less than a defined distance in y away from the stance foot. + stanceFootPosition.setToZero(stanceFootZUpFrame); + stanceFootPosition.changeFrameAndProjectToXYPlane(desiredConstraintFrame); + desiredStepPositionToConstrain.changeFrame(desiredConstraintFrame); + + if (swingSide == RobotSide.LEFT) + { + double innerYBound = stanceFootPosition.getY() + minStanceWidth; + if (innerYBound > desiredStepPositionToConstrain.getY()) + { + desiredStepPositionToConstrain.setY(innerYBound); + return true; + } + } + else + { + double innerYBound = stanceFootPosition.getY() - minStanceWidth; + if (innerYBound < desiredStepPositionToConstrain.getY()) + { + desiredStepPositionToConstrain.setY(innerYBound); + return true; + } + } + + return false; + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index 46d2b10f839..b6970bf6ca3 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -13,6 +13,7 @@ import controller_msgs.msg.dds.FootstepStatusMessage; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.commonWalkingControlModules.controllers.Updatable; +import us.ihmc.commonWalkingControlModules.desiredFootStep.EllipticalStepPositionLimiter; import us.ihmc.commonWalkingControlModules.desiredFootStep.FootstepVisualizer; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider.QuicksterFootstepProvider; import us.ihmc.commons.MathTools; @@ -20,6 +21,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.euclid.referenceFrame.FramePose2D; import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.PoseReferenceFrame; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePose2DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; @@ -31,6 +33,7 @@ import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.packets.walking.ContinuousStepGeneratorMode; import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepStatus; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.SCS2YoGraphicHolder; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.RobotSide; @@ -38,6 +41,7 @@ import us.ihmc.robotics.time.ExecutionTimer; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition; +import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; import us.ihmc.tools.factories.OptionalFactoryField; import us.ihmc.yoVariables.euclid.YoVector2D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; @@ -165,6 +169,8 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder private final OptionalFactoryField quicksterFootstepProvider = new OptionalFactoryField<>("QuicksterFootstepProviderField"); private boolean useQFPParameters = false; + private final OptionalFactoryField referenceFrames = new OptionalFactoryField<>("referenceFramesCSG"); + /** * Creates a new step generator, its {@code YoVariable}s will not be attached to any registry. */ @@ -439,31 +445,11 @@ else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == Co } } - // run through and make sure these adjusted steps are valid. -// for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++) -// { -// FootstepDataMessage footstepData = footstepDataListMessage.getFootstepDataList().get(i); -// nextFootstepPose2D.getPosition().set(footstepData.getLocation()); -// nextFootstepPose2D.getOrientation().set(footstepData.getOrientation()); -// nextFootstepPose3D.getPosition().set(footstepData.getLocation()); -// nextFootstepPose3D.getOrientation().set(footstepData.getOrientation()); -// swingSide = RobotSide.fromByte(footstepData.getRobotSide()); -// -// if (!isStepValid(nextFootstepPose3D, previousFootstepPose, swingSide)) -// { -// alternateStepChooser.computeStep(footstepPose2D, nextFootstepPose2D, swingSide, footstepData); -// -// // remove all the other steps after the invalid one. -// while (footstepDataListMessage.getFootstepDataList().size() > i + 1) -// { -// footstepDataListMessage.getFootstepDataList().remove(i + 1); -// } -// } -// -// previousFootstepPose.getPosition().set(footstepData.getLocation()); -// previousFootstepPose.getOrientation().set(footstepData.getOrientation()); -// footstepPose2D.set(previousFootstepPose); -// } + // Apply reachability constraint + if (currentCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.STANDARD)) + applyReachabilityConstraintToStep(footsteps.get(0)); + else + applyReachabilityConstraintToStep(footsteps); // Update the visualizers for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++) @@ -506,6 +492,112 @@ else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == Co stepGeneratorTimer.stopMeasurement(); } + private final FramePose3D stanceFootPose = new FramePose3D(); + private final PoseReferenceFrame stanceFootFrame = new PoseReferenceFrame("Latest Stance Foot Frame in Plan", ReferenceFrame.getWorldFrame()); + + private final FramePose3D constraintFramePose = new FramePose3D(); + private final PoseReferenceFrame constraintFrame = new PoseReferenceFrame("Latest Control Frame in Plan", ReferenceFrame.getWorldFrame()); + + private final FramePose3D unConstrainedFootstepPose = new FramePose3D(); + private final FramePose3D constrainedFootstepPose = new FramePose3D(); + + EllipticalStepPositionLimiter stepPositionLimiter = new EllipticalStepPositionLimiter(); + + private void applyReachabilityConstraintToStep(FootstepDataMessage footstep) + { + unConstrainedFootstepPose.setMatchingFrame(ReferenceFrame.getWorldFrame(), footstep.getLocation(), footstep.getOrientation()); + + RobotSide swingSide = RobotSide.fromByte(footstep.getRobotSide()); + + stanceFootPose.setFromReferenceFrame(referenceFrames.get().getSoleZUpFrame(swingSide.getOppositeSide())); + constraintFramePose.setFromReferenceFrame(referenceFrames.get().getPelvisZUpFrame()); + + stanceFootPose.changeFrame(stanceFootFrame.getParent()); + constraintFramePose.changeFrame(constraintFrame.getParent()); + + stanceFootFrame.setPoseAndUpdate(stanceFootPose); + constraintFrame.setPoseAndUpdate(constraintFramePose); + + stepPositionLimiter.enforceFootPositionConstraint(unConstrainedFootstepPose.getPosition(), + constrainedFootstepPose.getPosition(), + constraintFrame, + stanceFootFrame, + swingSide); + + footstep.getLocation().set(constrainedFootstepPose.getPosition()); + } + + private void applyReachabilityConstraintToStep(List footsteps) + { + for (int i = 0; i < footsteps.size(); i++) + { + FootstepDataMessage footstepDataMessage = footsteps.get(i); + + unConstrainedFootstepPose.setMatchingFrame(ReferenceFrame.getWorldFrame(), footstepDataMessage.getLocation(), footstepDataMessage.getOrientation()); + + RobotSide swingSide = RobotSide.fromByte(footstepDataMessage.getRobotSide()); + + double offset = swingSide == RobotSide.LEFT ? EllipticalStepPositionLimiter.NOMINAL_STANCE_WIDTH_DEFAULT / 2.0 : -EllipticalStepPositionLimiter.NOMINAL_STANCE_WIDTH_DEFAULT / 2.0; + + if (i == 0) + { + stanceFootPose.setFromReferenceFrame(referenceFrames.get().getSoleZUpFrame(swingSide.getOppositeSide())); + + if (referenceFrames.hasValue()) + constraintFramePose.setFromReferenceFrame(referenceFrames.get().getPelvisZUpFrame()); + else + { + stanceFootFrame.setPoseAndUpdate(stanceFootPose); + + constraintFramePose.setFromReferenceFrame(stanceFootFrame); + constraintFramePose.getPosition().addY(offset); + } + } + else + { + constraintFramePose.changeFrame(stanceFootFrame); + + double x = constraintFramePose.getX(); + double y = constraintFramePose.getY(); + double z = constraintFramePose.getZ(); + + boolean stanceFootPoseHasBeenSet = false; + + // Make sure stance foot is set from latest footstep in plan that is opposite foot of swing foot + for (int k = 0 ; k < i; k++) + if (swingSide != RobotSide.fromByte(footsteps.get(k).getRobotSide())) + { + stanceFootPose.setMatchingFrame(ReferenceFrame.getWorldFrame(), footsteps.get(k).getLocation(), footsteps.get(k).getOrientation()); + stanceFootPoseHasBeenSet = true; + } + + // If all footsteps in plan are with the same foot, set stanceFootPose to robot's current stance foot + if (!stanceFootPoseHasBeenSet) + stanceFootPose.setFromReferenceFrame(referenceFrames.get().getSoleZUpFrame(swingSide.getOppositeSide())); + + stanceFootPose.changeFrame(stanceFootFrame.getParent()); + stanceFootFrame.setPoseAndUpdate(stanceFootPose); + + constraintFramePose.setFromReferenceFrame(stanceFootFrame); + constraintFramePose.getPosition().set(x, -y, z); + } + + stanceFootPose.changeFrame(stanceFootFrame.getParent()); + constraintFramePose.changeFrame(constraintFrame.getParent()); + + stanceFootFrame.setPoseAndUpdate(stanceFootPose); + constraintFrame.setPoseAndUpdate(constraintFramePose); + + stepPositionLimiter.enforceFootPositionConstraint(unConstrainedFootstepPose.getPosition(), + constrainedFootstepPose.getPosition(), + constraintFrame, + stanceFootFrame, + swingSide); + + footstepDataMessage.getLocation().set(constrainedFootstepPose.getPosition()); + } + } + private static void calculateNextFootstepPose2D(double stepTime, double desiredVelocityX, double desiredVelocityY, double desiredTurningVelocity, RobotSide swingSide, double maxStepLength, double maxStepWidth, double defaultStepWidth, double minStepWidth, double turnMaxAngleInward, double turnMaxAngleOutward, @@ -870,6 +962,11 @@ public void setQuicksterFootstepProvider(QuicksterFootstepProvider quicksterFoot this.quicksterFootstepProvider.set(quicksterFootstepProvider); } + public void setReferenceFrames(CommonHumanoidReferenceFrames referenceFrames) + { + this.referenceFrames.set(referenceFrames); + } + /** * Sets a footstep adjustment that uses the current support foot pose to adjust the generated * footsteps. diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderParameters.java index 627bf2545d7..3dce4b4c01d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderParameters.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderParameters.java @@ -13,7 +13,7 @@ public class QuicksterFootstepProviderParameters // Static variables private static final double SWING_DURATION = 0.55;//0.7;// private static final double DOUBLE_SUPPORT_FRACTION = 0.05;//0.1;// - private static final double STANCE_WIDTH = 0.275; + private static final double STANCE_WIDTH = 0.3; private static final double SWING_HEIGHT = 0.09; private static final double COM_HEIGHT = 0.95; private static final double POLE = 0.0; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java index 442f44f4138..435e55bcc6c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java @@ -139,6 +139,7 @@ public ComponentBasedFootstepDataMessageGenerator buildPlugin(FullHumanoidRobotM FactoryTools.checkAllFactoryFieldsAreSet(this); ContinuousStepGenerator continuousStepGenerator = new ContinuousStepGenerator(registryField.get()); + continuousStepGenerator.setReferenceFrames(referenceFrames); continuousStepGenerator.setQuicksterFootstepProvider(new QuicksterFootstepProvider(robotModel, referenceFrames, updateDT, diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiterTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiterTest.java new file mode 100644 index 00000000000..c3b0b7cdac4 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiterTest.java @@ -0,0 +1,278 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep; + +import org.junit.jupiter.api.Test; +import us.ihmc.commons.MathTools; +import us.ihmc.commons.RandomNumbers; +import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameRandomTools; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameTestTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.log.LogTools; +import us.ihmc.robotics.referenceFrames.PoseReferenceFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.scs2.SimulationConstructionSet2; +import us.ihmc.scs2.definition.visual.ColorDefinitions; +import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory; +import us.ihmc.scs2.definition.yoGraphic.YoGraphicPoint2DDefinition; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; +import us.ihmc.yoVariables.registry.YoRegistry; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.*; + +public class EllipticalStepPositionLimiterTest +{ + @Test + public void testMinWidths() + { + EllipticalStepPositionLimiter stepPositionLimiter = new EllipticalStepPositionLimiter(); + + FramePoint3D stepPositionUnlimited = new FramePoint3D(); + FramePoint3D stepPositionLimited = new FramePoint3D(); + FramePose3D stanceFootPose = new FramePose3D(); + PoseReferenceFrame stanceFootFrame = new PoseReferenceFrame("StanceFootFrame", ReferenceFrame.getWorldFrame()); + + double stanceWidth = 0.25; + double maxLength = 1.0; + double maxWidth = 0.75; + + Random random = new Random(1738L); + for (int i = 0; i < 10000; i++) + { + // wrap this in a loop so we test both side + for (RobotSide swingSide : RobotSide.values) + { + // test a position that shouldn't be limited. The desired step width here is 0.2, and the min is 0.1, so there should be no adjustment + double minWidth = 0.1; + stanceFootPose.getPosition().set(0.0, swingSide.negateIfLeftSide(0.1), 0.0); + // apply a random foot yaw to make sure that doesn't mess things up + stanceFootPose.getOrientation().setToYawOrientation(RandomNumbers.nextDouble(random, Math.PI)); + stanceFootFrame.setPoseAndUpdate(stanceFootPose); + + stepPositionUnlimited.set(0.0, swingSide.negateIfRightSide(0.1), 0.0); + + stepPositionLimiter.enforceFootPositionConstraint(stepPositionUnlimited, + stepPositionLimited, + ReferenceFrame.getWorldFrame(), + stanceFootFrame, + stanceWidth, + maxLength, + minWidth, + maxWidth, + 0.0, + swingSide); + + EuclidCoreTestTools.assertEquals(stepPositionUnlimited, stepPositionLimited, 1e-5); + + // increase the minimum width so that it forces the limited position out by 5 cm so that the robot is the minimum width from the stance foot + minWidth = 0.25; + stepPositionLimiter.enforceFootPositionConstraint(stepPositionUnlimited, + stepPositionLimited, + ReferenceFrame.getWorldFrame(), + stanceFootFrame, + stanceWidth, + maxLength, + minWidth, + maxWidth, + 0.0, + swingSide); + + FramePoint3D stepPositionExpected = new FramePoint3D(ReferenceFrame.getWorldFrame(), 0.0, swingSide.negateIfRightSide(0.15), 0.0); + EuclidCoreTestTools.assertEquals(stepPositionExpected, stepPositionLimited, 1e-5); + + // Move the stance foot to the left, so that the width is enforced by the constraint frame + stanceFootPose.getPosition().set(0.0, swingSide.negateIfLeftSide(0.5), 0.0); + stanceFootPose.getOrientation().setToYawOrientation(RandomNumbers.nextDouble(random, Math.PI)); + stanceFootFrame.setPoseAndUpdate(stanceFootPose); + + stepPositionLimiter.enforceFootPositionConstraint(stepPositionUnlimited, + stepPositionLimited, + ReferenceFrame.getWorldFrame(), + stanceFootFrame, + stanceWidth, + maxLength, + minWidth, + maxWidth, + 0.0, + swingSide); + + stepPositionExpected.set(0.0, 0.5 * swingSide.negateIfRightSide(minWidth), 0.0); + EuclidCoreTestTools.assertEquals(stepPositionExpected, stepPositionLimited, 1e-5); + + // decrease the desired position, such that the actual gets cropped + stepPositionUnlimited.set(0.0, swingSide.negateIfRightSide(0.0), 0.0); + stepPositionLimiter.enforceFootPositionConstraint(stepPositionUnlimited, + stepPositionLimited, + ReferenceFrame.getWorldFrame(), + stanceFootFrame, + stanceWidth, + maxLength, + 0.15, + maxWidth, + 0.0, + swingSide); + + // This is 0.075, becuase we know that the minimum width about the pelvis is HALF the min width vvalue + stepPositionExpected = new FramePoint3D(ReferenceFrame.getWorldFrame(), 0.0, swingSide.negateIfRightSide(0.075), 0.0); + EuclidCoreTestTools.assertEquals(stepPositionExpected, stepPositionLimited, 1e-5); + } + } + } + + @Test + public void testEnforceEllipticConstraint() + { + Random random = new Random(1738L); + + for (int i = 0; i < 100; i++) + { + ReferenceFrame constraintFrame = EuclidFrameRandomTools.nextReferenceFrame("constraintFrame", random); + ReferenceFrame originalFrame = EuclidFrameRandomTools.nextReferenceFrame("originalFrame", random); + + double nominalWidth = RandomNumbers.nextDouble(random, 0.1, 0.4); + double maxWidth = RandomNumbers.nextDouble(random, nominalWidth, 0.8); + double maxForward = RandomNumbers.nextDouble(random, 0.0, 1.0); + + RobotSide stepSide = RandomNumbers.nextEnum(random, RobotSide.class); + PoseReferenceFrame ellipseFrame = new PoseReferenceFrame("ellipseFrame", constraintFrame); + ellipseFrame.setPositionAndUpdate(new FramePoint3D(constraintFrame, 0.0, stepSide.negateIfRightSide(0.5 * nominalWidth), 0.0)); + + for (int pointIdx = 0; pointIdx < 5000; pointIdx++) + { + FramePoint3D pointToConstrain = EuclidFrameRandomTools.nextFramePoint3D(random, constraintFrame); + pointToConstrain.changeFrame(originalFrame); + FramePoint3D originalPoint = new FramePoint3D(pointToConstrain); + + boolean wasModified = EllipticalStepPositionLimiter.enforceOuterEllipticalBoundInConstraintFrame(pointToConstrain, + constraintFrame, + nominalWidth, + maxForward, + maxWidth, + stepSide); + + // make sure the frames never changed + assertEquals(originalFrame, pointToConstrain.getReferenceFrame()); + + // check that points that were outside were changed, and points that were inside did not. + boolean isOriginalPointInside = isPointInsideEllipse(originalPoint, ellipseFrame, maxForward, maxWidth - nominalWidth, 1e-16); + boolean isNewPointInside = isPointInsideEllipse(pointToConstrain, ellipseFrame, maxForward, maxWidth - nominalWidth, 1e-6); + if (isOriginalPointInside) + { + pointToConstrain.changeFrame(constraintFrame); + originalPoint.changeFrame(constraintFrame); + if (wasModified) + LogTools.info("crap"); + + // point should not have been modified + assertFalse(wasModified); + // original point is inside, so the after point should be the exact same + assertTrue(isNewPointInside); + EuclidFrameTestTools.assertEquals(originalPoint, pointToConstrain, 1e-12); + } + else + { + pointToConstrain.changeFrame(ellipseFrame); + originalPoint.changeFrame(ellipseFrame); + if (!isNewPointInside) + LogTools.info("crap"); + + double ellipseNumber = getEllipseNumber(pointToConstrain, ellipseFrame, maxForward, maxWidth - nominalWidth); + // poitn should have been modified + assertTrue(wasModified); + // point should be on edge + assertEquals(ellipseNumber, 1.0, 1e-9); + assertTrue(isNewPointInside); + // we should never project to opposite sides of the ellipse, as that wouldn't be the closest point + assertEquals(Math.signum(originalPoint.getX()), Math.signum(pointToConstrain.getX())); + assertEquals(Math.signum(originalPoint.getY()), Math.signum(pointToConstrain.getY())); + // make sure the points arent' equal + assertTrue(originalPoint.distance(pointToConstrain) > 1e-6); + assertTrue(originalPoint.distanceXY(pointToConstrain) > 1e-6); + } + } + } + } + + private boolean isPointInsideEllipse(FramePoint3DReadOnly pointToCheck, ReferenceFrame ellipseFrame, double rx, double ry, double epsilon) + { + return getEllipseNumber(pointToCheck, ellipseFrame, rx, ry) <= 1.0 + epsilon; + } + + private static double getEllipseNumber(FramePoint3DReadOnly pointToCheck, ReferenceFrame ellipseFrame, double rx, double ry) + { + FramePoint3D pointCopy = new FramePoint3D(pointToCheck); + pointCopy.changeFrame(ellipseFrame); + + return MathTools.square(pointCopy.getX() / rx) + MathTools.square(pointCopy.getY() / ry); + } + + public static void main(String[] args) + { + SimulationConstructionSet2 scs = new SimulationConstructionSet2(); + + EllipticalStepPositionLimiter stepPositionLimiter = new EllipticalStepPositionLimiter(); + + PoseReferenceFrame stanceFootFrame = new PoseReferenceFrame("StanceFootFrame", ReferenceFrame.getWorldFrame()); + + double maxLength = 0.6; + double maxWidth = 0.5; + double minWidth = -0.2; + double nominalWidth = 0.2; + double distanceFromStance = 0.15; + + stanceFootFrame.setPositionAndUpdate(new FramePoint3D(ReferenceFrame.getWorldFrame(), 0.0, -0.5 * nominalWidth, 0.0)); + + YoRegistry registry = new YoRegistry(EllipticalStepPositionLimiter.class.getSimpleName()); + int index = 0; + for (double x = -maxLength - 0.2; x <= maxLength + 0.2; x += 0.025) + { + for (double y = minWidth - 0.1; y <= maxWidth + 0.1; y += 0.025) + { + FramePoint3D stepPosition = new FramePoint3D(); + FramePoint3D constrainedStepPosition = new FramePoint3D(); + stepPosition.set(x, y, 0.0); + + stepPositionLimiter.enforceFootPositionConstraint(stepPosition, + constrainedStepPosition, + ReferenceFrame.getWorldFrame(), + stanceFootFrame, + nominalWidth, + maxLength, + minWidth, + maxWidth, + distanceFromStance, + RobotSide.LEFT); + + YoFramePoint2D unconstrainedPosition = new YoFramePoint2D("unconstrainedPosition" + index, ReferenceFrame.getWorldFrame(), registry); + YoFramePoint2D constrainedPosition = new YoFramePoint2D("constrainedPosition" + index, ReferenceFrame.getWorldFrame(), registry); + index++; + + unconstrainedPosition.set(stepPosition); + constrainedPosition.set(constrainedStepPosition); + + YoGraphicPoint2DDefinition originGraphic = YoGraphicDefinitionFactory.newYoGraphicPoint2D(unconstrainedPosition.getNamePrefix(), + unconstrainedPosition, + 0.0075, + ColorDefinitions.Blue(), + YoGraphicDefinitionFactory.DefaultPoint2DGraphic.CIRCLE); + YoGraphicPoint2DDefinition goalGraphic = YoGraphicDefinitionFactory.newYoGraphicPoint2D(constrainedPosition.getNamePrefix(), + constrainedPosition, + 0.006, + ColorDefinitions.Red(), + YoGraphicDefinitionFactory.DefaultPoint2DGraphic.CIRCLE_FILLED); + scs.addYoGraphic(originGraphic); + scs.addYoGraphic(goalGraphic); + } + } + + scs.addRegistry(registry); + scs.startSimulationThread(); + + ThreadTools.sleepForever(); + } +} From e34b5ba15bb390a0ccb07b27e4b8af5b3b2d38b2 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 1 Apr 2025 23:48:32 -0500 Subject: [PATCH 14/17] fixed some bugs, but everything is worse now --- .../controller/ICPController.java | 4 +- .../ContinuousStepGenerator.java | 18 ++-- .../ALIPCalculatorTools.java | 3 +- .../QuicksterFootstepProvider.java | 90 ++++++++++++------- 4 files changed, 76 insertions(+), 39 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index 222d8cfcc99..b75c055ce55 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -215,7 +215,7 @@ public ICPController(WalkingControllerParameters walkingControllerParameters, parameters.createFeedbackAlphaCalculator(registry, null); parameters.createFeedbackProjectionOperator(registry, null); - ticksToInterpolateAlpha.set(75); + ticksToInterpolateAlpha.set(100); if (yoGraphicsListRegistry != null) setupVisualizers(yoGraphicsListRegistry); @@ -469,7 +469,7 @@ private void computeFeedForwardAndFeedBackAlphas() double endValue = 1.0; double valueToUse = InterpolationTools.linearInterpolate(startValue, endValue, interpolationFactor); feedbackAlpha.set(valueToUse); - if (alphaInterpolationTicks < ticksToInterpolateAlpha.getValue()) + if (alphaInterpolationTicks < maxNumberOfAlphaInterpolationTicks) alphaInterpolationTicks++; } else if (parameters.getFeedbackAlphaCalculator() != null) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index b6970bf6ca3..46f23507905 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -324,8 +324,17 @@ else if (statusToProcess == FootstepStatus.COMPLETED) // Set footstep parameters if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue()) { - footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); - footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + if (useQFPParameters) + { + footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + } + else + { + footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getAlternateTransferDuration()); + footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getAlternateTransferDuration()); + } + footstepDataListMessage.setDefaultSwingDuration(quicksterFootstepProvider.get().getSwingDuration(swingSide)); footstepDataListMessage.setAreFootstepsAdjustable(false); footstepDataListMessage.setOffsetFootstepsWithExecutionError(false); @@ -446,10 +455,7 @@ else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == Co } // Apply reachability constraint - if (currentCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.STANDARD)) - applyReachabilityConstraintToStep(footsteps.get(0)); - else - applyReachabilityConstraintToStep(footsteps); + applyReachabilityConstraintToStep(footsteps.get(0)); // Update the visualizers for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java index fb68bc87d8c..54d599a24e2 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/ALIPCalculatorTools.java @@ -65,6 +65,7 @@ public void computeFutureStateUsingALIP(FramePose3DReadOnly currentCoMPose, futureCoMPoseToPack.setX(tempCurrentStanceFootPosition.getX()); futureCoMPoseToPack.setY(tempCurrentStanceFootPosition.getY()); + futureContactPointAngularMomentumToPack.setIncludingFrame(tempCurrentContactPointAngularMomentum); futureContactPointAngularMomentumToPack.changeFrame(controlFrame); int intervals = (int) Math.round(horizonDuration / updateDt); @@ -187,7 +188,7 @@ public void computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(FrameP tempFutureCoMPose.changeFrame(tempFutureControlFrame.getParent()); tempFutureControlFrame.setPoseAndUpdate(tempFutureCoMPose); - tempFutureCoMVelocity.changeFrame(tempFutureContactPointAngularMomentum.getReferenceFrame()); + tempFutureCoMVelocity.setToZero(tempFutureContactPointAngularMomentum.getReferenceFrame()); tempFutureCoMVelocity.setX(tempFutureContactPointAngularMomentum.getY() / (pendulumMass * pendulumHeight)); tempFutureCoMVelocity.setY(-tempFutureContactPointAngularMomentum.getX() / (pendulumMass * pendulumHeight)); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 6aad6cf9e34..9ad941b88a0 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -239,7 +239,7 @@ public void initialize() firstTick = false; } - private double numberOfFootstepsToPlan; + private int numberOfFootstepsToPlan; public void update(int numberOfFootstepsToPlan) { @@ -292,9 +292,15 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRe desiredTouchdownPosesList.get(currentSwingSide).clear(); desiredTouchdownPosesList.get(currentSwingSide.getOppositeSide()).clear(); - for (int i = 0; i < numberOfFootstepsToPlan; i++) + int numberOfSteps = numberOfFootstepsToPlan; + if (inDoubleSupport) + numberOfSteps++; + + for (int i = 0; i < numberOfSteps; i++) { double timeToReachGoal; + double stepDuration; + double transferDuration; RobotSide swingSide; boolean useFutureCoM; @@ -302,23 +308,29 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRe { tempCurrentCoMPose.setFromReferenceFrame(centerOfMassControlZUpFrame); tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); - tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide())); + tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide()));//setMatchingFrame(netPendulumBase3DInWorld);// tempControlFrame.setTransformAndUpdate(centerOfMassControlZUpFrame.getTransformToDesiredFrame(tempControlFrame.getParent())); swingSide = currentSwingSide; timeToReachGoal = timeRemainingInCurrentStep; + stepDuration = useAlternateTransferDuration ? getSwingDuration(swingSide) + getAlternateTransferDuration() : getStepDuration(swingSide); + transferDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(swingSide); useFutureCoM = false; } else if (i % 2 == 0) { swingSide = currentSwingSide; timeToReachGoal = getStepDuration(swingSide); + stepDuration = getStepDuration(swingSide); + transferDuration = getTransferDuration(swingSide); useFutureCoM = true; } else { swingSide = currentSwingSide.getOppositeSide(); timeToReachGoal = getStepDuration(swingSide); + stepDuration = getStepDuration(swingSide); + transferDuration = getTransferDuration(swingSide); useFutureCoM = true; } @@ -339,8 +351,8 @@ else if (i % 2 == 0) desiredTurningVelocity.getDoubleValue(), parameters.getStanceWidth(swingSide).getDoubleValue(), timeToReachGoal, - getStepDuration(swingSide), - getTransferDuration(swingSide), + stepDuration, + transferDuration, robotModel.getTotalMass(), parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), parameters.getPole(swingSide).getDoubleValue(), @@ -369,14 +381,22 @@ else if (i % 2 == 0) if (i == 0 && !useFutureCoM) { - tempCurrentStanceFootPosition.changeFrame(centerOfMassControlZUpFrame); - double stanceFootX = tempCurrentStanceFootPosition.getX(); - double stanceFootY = tempCurrentStanceFootPosition.getY(); - double stanceFootZ = tempCurrentStanceFootPosition.getZ(); - - tempCurrentStanceFootPosition.changeFrame(tempControlFrame); - tempCurrentStanceFootPosition.set(stanceFootX, stanceFootY, stanceFootZ); - tempCurrentStanceFootPosition.changeFrame(tempControlFrame.getParent()); + if (inDoubleSupport) + { + desiredTouchdownPositionsList.get(swingSide).remove(0); + desiredTouchdownPosesList.get(swingSide).remove(0); + } + else + { + tempCurrentStanceFootPosition.changeFrame(centerOfMassControlZUpFrame); + double stanceFootX = tempCurrentStanceFootPosition.getX(); + double stanceFootY = tempCurrentStanceFootPosition.getY(); + double stanceFootZ = tempCurrentStanceFootPosition.getZ(); + + tempCurrentStanceFootPosition.changeFrame(tempControlFrame); + tempCurrentStanceFootPosition.set(stanceFootX, stanceFootY, stanceFootZ); + tempCurrentStanceFootPosition.changeFrame(tempControlFrame.getParent()); + } nextStanceFootPosition.setMatchingFrame(tempCurrentStanceFootPosition); nextCoMPosition.setMatchingFrame(tempCurrentCoMPose.getPosition()); @@ -420,7 +440,7 @@ private void calculateNetPendulumBase() setTrailingSide(trailingSide); // compute the double support duration - double doubleSupportDuration = getTransferDuration(trailingSide.getOppositeSide()); + double doubleSupportDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(trailingSide.getOppositeSide()); // compute the fraction through the double support state that we are at the current time double alpha = 1.0; @@ -562,6 +582,11 @@ public void getDesiredTouchdownPose(int footstepIndex, RobotSide robotSide, Fram touchdownPoseToPack.setMatchingFrame(desiredTouchdownPosesList.get(robotSide).getLast()); } + public double getStepDuration(RobotSide swingSide) + { + return getSwingDuration(swingSide) + getTransferDuration(swingSide); + } + public double getSwingDuration(RobotSide swingSide) { return parameters.getSwingDuration(swingSide).getDoubleValue(); @@ -569,10 +594,12 @@ public double getSwingDuration(RobotSide swingSide) public double getTransferDuration(RobotSide swingSide) { - if (useAlternateTransferDuration) - return alternateTransferDuration; - else - return parameters.getSwingDuration(swingSide).getDoubleValue() * parameters.getDoubleSupportFraction(swingSide).getDoubleValue(); + return parameters.getSwingDuration(swingSide).getDoubleValue() * parameters.getDoubleSupportFraction(swingSide).getDoubleValue(); + } + + public double getAlternateTransferDuration() + { + return alternateTransferDuration; } public void setAlternateTransferDuration(double alternateTransferDuration) @@ -585,11 +612,6 @@ public void setUseAlternateTransferDuration(boolean useAlternateTransferDuration this.useAlternateTransferDuration = useAlternateTransferDuration; } - public double getStepDuration(RobotSide swingSide) - { - return getSwingDuration(swingSide) + getTransferDuration(swingSide); - } - public double getSwingHeight(RobotSide swingSide) { return parameters.getSwingHeight(swingSide).getDoubleValue(); @@ -623,15 +645,17 @@ public void onEntry() pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + double transferDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(robotSide); if (robotSide == newestSupportSide.getEnumValue() && inDoubleSupport.getValue()) - calculateTouchdownPosition(robotSide, getTransferDuration(robotSide), true); + calculateTouchdownPosition(robotSide, transferDuration, true); } @Override public void doAction(double timeInState) { - double timeToReachGoal = getStepDuration(robotSide) - timeInState; - timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getTransferDuration(robotSide)); + double transferDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(robotSide); + double timeToReachGoal = transferDuration - timeInState; + timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, transferDuration); pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); @@ -649,7 +673,11 @@ public void onExit(double timeInState) @Override public boolean isDone(double timeInState) { - return inDoubleSupport.getValue() && (timeInState - getSwingDuration(robotSide)) > getTransferDuration(robotSide) && !robotSide.equals(newestSupportSide.getEnumValue()); + double transferDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(robotSide.getOppositeSide()); + + return inDoubleSupport.getValue() + && robotSide.getOppositeSide().equals(newestSupportSide.getEnumValue()) + && footStateMachines.get(robotSide.getOppositeSide()).getTimeInCurrentState() > transferDuration; } } @@ -669,7 +697,8 @@ public void onEntry() { footStates.get(robotSide).set(FootState.SWING); - calculateTouchdownPosition(robotSide, getStepDuration(robotSide), false); + double stepDuration = useAlternateTransferDuration ? getSwingDuration(robotSide) + getAlternateTransferDuration() : getStepDuration(robotSide); + calculateTouchdownPosition(robotSide, stepDuration, false); parameters.setParametersForUpcomingSwing(robotSide); } @@ -677,8 +706,9 @@ public void onEntry() @Override public void doAction(double timeInState) { - double timeToReachGoal = getStepDuration(robotSide) - timeInState; - timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getStepDuration(robotSide)); + double stepDuration = useAlternateTransferDuration ? getSwingDuration(robotSide) + getAlternateTransferDuration() : getStepDuration(robotSide); + double timeToReachGoal = stepDuration - timeInState; + timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, stepDuration); this.timeToReachGoal.set(timeToReachGoal); calculateTouchdownPosition(robotSide, timeToReachGoal, false); From bf134c2125a522091c8538e3dab068356fdecd02 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 2 Apr 2025 17:58:09 -0500 Subject: [PATCH 15/17] all qfp tests passing at control dt --- .../QuicksterFootstepProvider.java | 96 ++++++++----------- 1 file changed, 39 insertions(+), 57 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 9ad941b88a0..0742210d5ac 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -256,7 +256,7 @@ public void update(int numberOfFootstepsToPlan) private void updateEstimates() { initialize(); - desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * 10 * updateDT); + desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * 1 * updateDT); inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); @@ -292,11 +292,7 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRe desiredTouchdownPosesList.get(currentSwingSide).clear(); desiredTouchdownPosesList.get(currentSwingSide.getOppositeSide()).clear(); - int numberOfSteps = numberOfFootstepsToPlan; - if (inDoubleSupport) - numberOfSteps++; - - for (int i = 0; i < numberOfSteps; i++) + for (int i = 0; i < numberOfFootstepsToPlan; i++) { double timeToReachGoal; double stepDuration; @@ -308,7 +304,7 @@ public void calculateTouchdownPosition(RobotSide currentSwingSide, double timeRe { tempCurrentCoMPose.setFromReferenceFrame(centerOfMassControlZUpFrame); tempCurrentContactPointAngularMomentum.setMatchingFrame(estimates.getContactPointAngularMomentum()); - tempCurrentStanceFootPosition.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide()));//setMatchingFrame(netPendulumBase3DInWorld);// + tempCurrentStanceFootPosition.setMatchingFrame(netPendulumBase3DInWorld);//setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide.getOppositeSide()));// tempControlFrame.setTransformAndUpdate(centerOfMassControlZUpFrame.getTransformToDesiredFrame(tempControlFrame.getParent())); swingSide = currentSwingSide; @@ -337,28 +333,24 @@ else if (i % 2 == 0) FramePoint2D desiredTouchdownPositionToPack = desiredTouchdownPositionsList.get(swingSide).add(); FramePose2D desiredTouchdownPose = desiredTouchdownPosesList.get(swingSide).add(); - if (inDoubleSupport && i == 0) - desiredTouchdownPositionToPack.setFromReferenceFrame(referenceFrames.getSoleZUpFrame(currentSwingSide)); - else - alipCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempCurrentCoMPose, - tempCurrentContactPointAngularMomentum, - tempCurrentStanceFootPosition, - desiredTouchdownPositionToPack, - swingSide, - tempControlFrame, - desiredVelocity.getX(), - desiredVelocity.getY(), - desiredTurningVelocity.getDoubleValue(), - parameters.getStanceWidth(swingSide).getDoubleValue(), - timeToReachGoal, - stepDuration, - transferDuration, - robotModel.getTotalMass(), - parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), - parameters.getPole(swingSide).getDoubleValue(), - useFutureCoM, - updateDT); - + alipCalculatorTools.computeTouchdownPositionUsingRaibertHeuristicAndPolePlacement(tempCurrentCoMPose, + tempCurrentContactPointAngularMomentum, + tempCurrentStanceFootPosition, + desiredTouchdownPositionToPack, + swingSide, + tempControlFrame, + desiredVelocity.getX(), + desiredVelocity.getY(), + desiredTurningVelocity.getDoubleValue(), + parameters.getStanceWidth(swingSide).getDoubleValue(), + timeToReachGoal, + stepDuration, + transferDuration, + robotModel.getTotalMass(), + parameters.getDesiredCoMHeight(swingSide).getDoubleValue(), + parameters.getPole(swingSide).getDoubleValue(), + useFutureCoM, + updateDT); alipCalculatorTools.computeFutureStateUsingALIP(tempCurrentCoMPose, tempCurrentContactPointAngularMomentum, tempCurrentStanceFootPosition, @@ -381,22 +373,14 @@ else if (i % 2 == 0) if (i == 0 && !useFutureCoM) { - if (inDoubleSupport) - { - desiredTouchdownPositionsList.get(swingSide).remove(0); - desiredTouchdownPosesList.get(swingSide).remove(0); - } - else - { - tempCurrentStanceFootPosition.changeFrame(centerOfMassControlZUpFrame); - double stanceFootX = tempCurrentStanceFootPosition.getX(); - double stanceFootY = tempCurrentStanceFootPosition.getY(); - double stanceFootZ = tempCurrentStanceFootPosition.getZ(); - - tempCurrentStanceFootPosition.changeFrame(tempControlFrame); - tempCurrentStanceFootPosition.set(stanceFootX, stanceFootY, stanceFootZ); - tempCurrentStanceFootPosition.changeFrame(tempControlFrame.getParent()); - } + tempCurrentStanceFootPosition.changeFrame(centerOfMassControlZUpFrame); + double stanceFootX = tempCurrentStanceFootPosition.getX(); + double stanceFootY = tempCurrentStanceFootPosition.getY(); + double stanceFootZ = tempCurrentStanceFootPosition.getZ(); + + tempCurrentStanceFootPosition.changeFrame(tempControlFrame); + tempCurrentStanceFootPosition.set(stanceFootX, stanceFootY, stanceFootZ); + tempCurrentStanceFootPosition.changeFrame(tempControlFrame.getParent()); nextStanceFootPosition.setMatchingFrame(tempCurrentStanceFootPosition); nextCoMPosition.setMatchingFrame(tempCurrentCoMPose.getPosition()); @@ -645,22 +629,22 @@ public void onEntry() pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - double transferDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(robotSide); - if (robotSide == newestSupportSide.getEnumValue() && inDoubleSupport.getValue()) - calculateTouchdownPosition(robotSide, transferDuration, true); + double stepDuration = useAlternateTransferDuration ? getSwingDuration(robotSide) + getAlternateTransferDuration() : getStepDuration(robotSide); + if (robotSide != newestSupportSide.getEnumValue() && inDoubleSupport.getValue()) + calculateTouchdownPosition(robotSide, stepDuration, true); } @Override public void doAction(double timeInState) { - double transferDuration = useAlternateTransferDuration ? getAlternateTransferDuration() : getTransferDuration(robotSide); - double timeToReachGoal = transferDuration - timeInState; - timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, transferDuration); + double stepDuration = useAlternateTransferDuration ? getSwingDuration(robotSide) + getAlternateTransferDuration() : getStepDuration(robotSide); + double timeToReachGoal = stepDuration - footStateMachines.get(robotSide.getOppositeSide()).getTimeInCurrentState(); + timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, stepDuration); pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - if (robotSide == newestSupportSide.getEnumValue() && inDoubleSupport.getValue()) + if (robotSide != newestSupportSide.getEnumValue() && footStateMachines.get(robotSide.getOppositeSide()).getCurrentStateKey().equals(FootState.SUPPORT) && inDoubleSupport.getValue()) calculateTouchdownPosition(robotSide, timeToReachGoal, true); } @@ -697,8 +681,7 @@ public void onEntry() { footStates.get(robotSide).set(FootState.SWING); - double stepDuration = useAlternateTransferDuration ? getSwingDuration(robotSide) + getAlternateTransferDuration() : getStepDuration(robotSide); - calculateTouchdownPosition(robotSide, stepDuration, false); + calculateTouchdownPosition(robotSide, getSwingDuration(robotSide), false); parameters.setParametersForUpcomingSwing(robotSide); } @@ -706,9 +689,8 @@ public void onEntry() @Override public void doAction(double timeInState) { - double stepDuration = useAlternateTransferDuration ? getSwingDuration(robotSide) + getAlternateTransferDuration() : getStepDuration(robotSide); - double timeToReachGoal = stepDuration - timeInState; - timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, stepDuration); + double timeToReachGoal = getSwingDuration(robotSide) - timeInState; + timeToReachGoal = MathTools.clamp(timeToReachGoal, 0.0, getSwingDuration(robotSide)); this.timeToReachGoal.set(timeToReachGoal); calculateTouchdownPosition(robotSide, timeToReachGoal, false); From 7a4fc78938abd18059be40fa9b182feca5e3f30d Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 2 Apr 2025 18:45:23 -0500 Subject: [PATCH 16/17] increased min step width for elliptical reachability limiter --- .../EllipticalStepPositionLimiter.java | 4 +- .../ContinuousStepGenerator.java | 71 ------------------- .../QuicksterFootstepProvider.java | 2 +- 3 files changed, 3 insertions(+), 74 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java index 8752edc7a2a..71a8ced9c21 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/EllipticalStepPositionLimiter.java @@ -15,9 +15,9 @@ public class EllipticalStepPositionLimiter { public final static double NOMINAL_STANCE_WIDTH_DEFAULT = 0.25; public final static double MAX_STEP_FORWARD_DEFAULT = 0.85; - public final static double MIN_STANCE_WIDTH_DEFAULT = 0.075; + public final static double MIN_STANCE_WIDTH_DEFAULT = 0.1; public final static double MAX_STANCE_WIDTH_DEFAULT = 0.75; - public final static double MIN_DISTANCE_FROM_STANCE_FOOT_DEFAULT = 0.075; + public final static double MIN_DISTANCE_FROM_STANCE_FOOT_DEFAULT = 0.1; private final FramePoint2D stanceFootPosition = new FramePoint2D(); private final FramePoint2D constrainedTouchdownPosition2D = new FramePoint2D(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index 46f23507905..07f17b5f0b4 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java @@ -533,77 +533,6 @@ private void applyReachabilityConstraintToStep(FootstepDataMessage footstep) footstep.getLocation().set(constrainedFootstepPose.getPosition()); } - private void applyReachabilityConstraintToStep(List footsteps) - { - for (int i = 0; i < footsteps.size(); i++) - { - FootstepDataMessage footstepDataMessage = footsteps.get(i); - - unConstrainedFootstepPose.setMatchingFrame(ReferenceFrame.getWorldFrame(), footstepDataMessage.getLocation(), footstepDataMessage.getOrientation()); - - RobotSide swingSide = RobotSide.fromByte(footstepDataMessage.getRobotSide()); - - double offset = swingSide == RobotSide.LEFT ? EllipticalStepPositionLimiter.NOMINAL_STANCE_WIDTH_DEFAULT / 2.0 : -EllipticalStepPositionLimiter.NOMINAL_STANCE_WIDTH_DEFAULT / 2.0; - - if (i == 0) - { - stanceFootPose.setFromReferenceFrame(referenceFrames.get().getSoleZUpFrame(swingSide.getOppositeSide())); - - if (referenceFrames.hasValue()) - constraintFramePose.setFromReferenceFrame(referenceFrames.get().getPelvisZUpFrame()); - else - { - stanceFootFrame.setPoseAndUpdate(stanceFootPose); - - constraintFramePose.setFromReferenceFrame(stanceFootFrame); - constraintFramePose.getPosition().addY(offset); - } - } - else - { - constraintFramePose.changeFrame(stanceFootFrame); - - double x = constraintFramePose.getX(); - double y = constraintFramePose.getY(); - double z = constraintFramePose.getZ(); - - boolean stanceFootPoseHasBeenSet = false; - - // Make sure stance foot is set from latest footstep in plan that is opposite foot of swing foot - for (int k = 0 ; k < i; k++) - if (swingSide != RobotSide.fromByte(footsteps.get(k).getRobotSide())) - { - stanceFootPose.setMatchingFrame(ReferenceFrame.getWorldFrame(), footsteps.get(k).getLocation(), footsteps.get(k).getOrientation()); - stanceFootPoseHasBeenSet = true; - } - - // If all footsteps in plan are with the same foot, set stanceFootPose to robot's current stance foot - if (!stanceFootPoseHasBeenSet) - stanceFootPose.setFromReferenceFrame(referenceFrames.get().getSoleZUpFrame(swingSide.getOppositeSide())); - - stanceFootPose.changeFrame(stanceFootFrame.getParent()); - stanceFootFrame.setPoseAndUpdate(stanceFootPose); - - constraintFramePose.setFromReferenceFrame(stanceFootFrame); - constraintFramePose.getPosition().set(x, -y, z); - } - - stanceFootPose.changeFrame(stanceFootFrame.getParent()); - constraintFramePose.changeFrame(constraintFrame.getParent()); - - stanceFootFrame.setPoseAndUpdate(stanceFootPose); - constraintFrame.setPoseAndUpdate(constraintFramePose); - - stepPositionLimiter.enforceFootPositionConstraint(unConstrainedFootstepPose.getPosition(), - constrainedFootstepPose.getPosition(), - constraintFrame, - stanceFootFrame, - swingSide); - - footstepDataMessage.getLocation().set(constrainedFootstepPose.getPosition()); - } - } - private static void calculateNextFootstepPose2D(double stepTime, double desiredVelocityX, double desiredVelocityY, double desiredTurningVelocity, RobotSide swingSide, double maxStepLength, double maxStepWidth, double defaultStepWidth, double minStepWidth, double turnMaxAngleInward, double turnMaxAngleOutward, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index 0742210d5ac..267c43c9ec7 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -256,7 +256,7 @@ public void update(int numberOfFootstepsToPlan) private void updateEstimates() { initialize(); - desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * 1 * updateDT); + desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * 10 * updateDT); inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); From d888f7a7c322e053044f58c1b868a36b0c0256c9 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 2 Apr 2025 19:20:02 -0500 Subject: [PATCH 17/17] decreased ticks to interpolate cop feedback alpha --- .../capturePoint/controller/ICPController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index b75c055ce55..409bd6240f2 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -215,7 +215,7 @@ public ICPController(WalkingControllerParameters walkingControllerParameters, parameters.createFeedbackAlphaCalculator(registry, null); parameters.createFeedbackProjectionOperator(registry, null); - ticksToInterpolateAlpha.set(100); + ticksToInterpolateAlpha.set(75); if (yoGraphicsListRegistry != null) setupVisualizers(yoGraphicsListRegistry);