From 5d0dccca1529438b35fdba84f0ca6dda3f231a0b Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 4 Dec 2024 20:39:52 -0600 Subject: [PATCH 01/18] added necessary classes for QFP --- .../avatar/AvatarStepGeneratorThread.java | 2 +- .../QuicksterFootstepProvider.java | 536 ++++++++++++++++++ .../QuicksterFootstepProviderEstimates.java | 109 ++++ .../QuicksterFootstepProviderParameters.java | 164 ++++++ ...erFootstepProviderTouchdownCalculator.java | 518 +++++++++++++++++ 5 files changed, 1328 insertions(+), 1 deletion(-) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderParameters.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java index d7d56850d58..b65acfc9f4c 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java @@ -103,7 +103,7 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory, drcRobotModel.getWalkingControllerParameters(), walkingOutputManager, walkingCommandInputManager, - null, + csgGraphics, null, csgTime); csgRegistry.addChild(continuousStepGeneratorPlugin.getRegistry()); 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 new file mode 100644 index 00000000000..c6056ea0342 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java @@ -0,0 +1,536 @@ +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.communication.controllerAPI.StatusMessageOutputManager; +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.graphicsDescription.appearance.AppearanceDefinition; +import us.ihmc.graphicsDescription.appearance.YoAppearance; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPosition; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; +import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepStatus; +import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.robotics.screwTheory.MovingZUpFrame; +import us.ihmc.robotics.stateMachine.core.State; +import us.ihmc.robotics.stateMachine.core.StateMachine; +import us.ihmc.robotics.stateMachine.factories.StateMachineFactory; +import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; +import us.ihmc.yoVariables.euclid.YoVector2D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +public class QuicksterFootstepProvider implements Updatable +{ + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + private final QuicksterFootstepProviderParameters parameters; + + private final String variableNameSuffix = "QFP"; + + private final CommonHumanoidReferenceFrames referenceFrames; + + private final double updateDT; + private final double gravity = 9.81; + + // Estimates + private final QuicksterFootstepProviderEstimates estimates; + private final MovingZUpFrame centerOfMassControlZUpFrame; + + // Command/Desired output related stuff + private final SideDependentList touchdownCalculator = new SideDependentList<>(); + private final SideDependentList desiredTouchdownPositions = new SideDependentList<>(); + + // Desired inputs + private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); + private final YoVector2D desiredVelocity = new YoVector2D("desiredVelocity" + variableNameSuffix, registry); + private final YoBoolean ignoreWalkInputProvider = new YoBoolean("ignoreWalkInputProvider" + variableNameSuffix, registry); + private final YoBoolean walk = new YoBoolean("walk" + variableNameSuffix, registry); + private final YoBoolean walkPreviousValue = new YoBoolean("walkPreviousValue" + variableNameSuffix, registry); + private final YoFrameQuaternion desiredPelvisOrientation; + + // Foot state information + 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); + private final YoBoolean inDoubleSupport = new YoBoolean("inDoubleSupport" + variableNameSuffix, registry); + private RobotSide trailingSide = RobotSide.LEFT; + + // + 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 SideDependentList pendulumBaseViz = new SideDependentList<>(); + private final YoGraphicPosition netPendulumBaseViz; + + // Inputs + private final static Vector2DReadOnly zero2D = new Vector2D(); + private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; + private DesiredTurningVelocityProvider desiredTurningVelocityProvider = () -> 0.0; + + public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) + { + this.updateDT = updateDT; + this.referenceFrames = referenceFrames; + + desiredPelvisOrientation = new YoFrameQuaternion("pelvisDesiredOrientation" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + + // Parameters + parameters = new QuicksterFootstepProviderParameters(gravity, registry); + + + // Estimates + estimates = new QuicksterFootstepProviderEstimates(robotModel, + referenceFrames, + desiredPelvisOrientation, + updateDT, + variableNameSuffix, + registry, + yoGraphicsListRegistry); + + centerOfMassControlZUpFrame = estimates.getCenterOfMassControlZUpFrame(); + + + netPendulumBaseViz = new YoGraphicPosition("netPendulumBase" + variableNameSuffix, + netPendulumBase3DInWorld, + 0.015, + YoAppearance.Red(), + YoGraphicPosition.GraphicType.SQUARE); + + // Side-dependant stuff, most of the desired-related things + for (RobotSide robotSide : RobotSide.values) + { + footStates.put(robotSide, new YoEnum<>(robotSide.getLowerCaseName() + "FootStates" + variableNameSuffix, registry, FootState.class)); + + pendulumBase.put(robotSide, new FramePoint2D(centerOfMassControlZUpFrame)); + + pendulumBase3DInWorld.put(robotSide, new YoFramePoint3D(robotSide.getLowerCaseName() + "PendulumBase3DInWorld" + variableNameSuffix, + ReferenceFrame.getWorldFrame(), + registry)); + + AppearanceDefinition pendulumBaseVizColor = robotSide == RobotSide.LEFT ? YoAppearance.DodgerBlue() : YoAppearance.Orange(); + pendulumBaseViz.put(robotSide, new YoGraphicPosition(robotSide.getLowerCaseName() + "PendulumBase" + variableNameSuffix, + pendulumBase3DInWorld.get(robotSide), + 0.015, + pendulumBaseVizColor, + YoGraphicPosition.GraphicType.SQUARE)); + + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, pendulumBaseViz.get(robotSide)); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, pendulumBaseViz.get(robotSide).createArtifact()); + + touchdownCalculator.put(robotSide, new QuicksterFootstepProviderTouchdownCalculator(robotSide, + centerOfMassControlZUpFrame, + estimates.getCenterOfMassVelocity(), + estimates.getCenterOfMassAngularMomentum(), + robotModel, + parameters, + gravity, + desiredVelocity, + registry)); + + desiredTouchdownPositions.put(robotSide, new FramePoint2D()); + + StateMachineFactory stateMachineFactory = new StateMachineFactory<>(FootState.class); + stateMachineFactory.setRegistry(registry).setNamePrefix(robotSide.getLowerCaseName() + variableNameSuffix).buildYoClock(yoTime); + + stateMachineFactory.addState(FootState.SUPPORT, new SupportFootState(robotSide)); + stateMachineFactory.addState(FootState.SWING, new SwingFootState(robotSide)); + + footStateMachines.put(robotSide, stateMachineFactory.build(FootState.SUPPORT)); + footStateMachines.get(robotSide).resetToInitialState(); + } + + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, netPendulumBaseViz); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, netPendulumBaseViz.createArtifact()); + } + + @Override + public void update(double time) + { + updateEstimates(); + + updateDesireds(); + } + + private void updateEstimates() + { + // TODO maybe wrong? + desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); + desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); + + estimates.update(); + + inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && + footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); + + calculateNetPendulumBase(); + } + + private void updateDesireds() + { + handleDesiredsFromProviders(); + + for (RobotSide robotSide : RobotSide.values) + { + footStateMachines.get(robotSide).doActionAndTransition(); + pendulumBase3DInWorld.get(robotSide).setMatchingFrame(pendulumBase.get(robotSide), 0.0); + } + + netPendulumBase3DInWorld.setMatchingFrame(netPendulumBase, 0.0); + } + + public void calculateTouchdownPosition(RobotSide swingSide, + double timeToReachGoal, + FramePose2DReadOnly pendulumBase, + FramePose2DReadOnly netPendulumBase, + FramePose2DBasics touchdownPositionToPack) + { + calculateTouchdownPosition(touchdownCalculator.get(swingSide), + swingSide, timeToReachGoal, + pendulumBase.getPosition(), + netPendulumBase.getPosition(), + inDoubleSupport.getBooleanValue(), + touchdownPositionToPack.getPosition()); + } + + private final FramePoint2DBasics tempPendulumBase = new FramePoint2D(); + private final FramePoint2DBasics tempNetPendulumBase = new FramePoint2D(); + private final FramePoint2D tempTouchdownPosition = new FramePoint2D(); + + public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalculator touchdownCalculator, + RobotSide swingSide, + double timeToReachGoal, + FramePoint2DReadOnly pendulumBase, + FramePoint2DReadOnly netPendulumBase, + boolean isInDoubleSupport, + FixedFramePoint2DBasics 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); + } + + private void calculateNetPendulumBase() + { + // if only one foot is in contact, get the pendulum base directly from the support state + if (!inDoubleSupport.getBooleanValue()) + { + if (footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT) + { + netPendulumBase.setIncludingFrame(pendulumBase.get(RobotSide.LEFT)); + netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(RobotSide.LEFT)); + setTrailingSide(RobotSide.LEFT); + } + + else + { + netPendulumBase.setIncludingFrame(pendulumBase.get(RobotSide.RIGHT)); + netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(RobotSide.RIGHT)); + setTrailingSide(RobotSide.RIGHT); + } + + return; + } + + // Figure out the side that you're shifting the weight away from based on the clock + RobotSide trailingSide; + if (newestSupportSide.getEnumValue() == RobotSide.RIGHT) + trailingSide = RobotSide.LEFT; + else + trailingSide = RobotSide.RIGHT; + + setTrailingSide(trailingSide); + + // compute the double support duration + double doubleSupportDuration = getTransferDuration(trailingSide.getOppositeSide()); + + // compute the fraction through the double support state that we are at the current time + double alpha = 1.0; + if (doubleSupportDuration > 0.0) + 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)); + } + + private void handleDesiredsFromProviders() + { + Vector2DReadOnly desiredVelocity = desiredVelocityProvider.getDesiredVelocity(); + double desiredVelocityX = desiredVelocity.getX(); + double desiredVelocityY = desiredVelocity.getY(); + double turningVelocity = desiredTurningVelocityProvider.getTurningVelocity(); + + // if (desiredVelocityProvider.isUnitVelocity()) + // { + // double minMaxVelocityX = maxStepLength / stepTime.getValue(); + // double minMaxVelocityY = maxStepWidth / stepTime.getValue(); + // desiredVelocityX = minMaxVelocityX * MathTools.clamp(desiredVelocityX, 1.0); + // desiredVelocityY = minMaxVelocityY * MathTools.clamp(desiredVelocityY, 1.0); + // } + // + // if (desiredTurningVelocityProvider.isUnitVelocity()) + // { + // double minMaxVelocityTurn = (turnMaxAngleOutward - turnMaxAngleInward) / stepTime.getValue(); + // turningVelocity = minMaxVelocityTurn * MathTools.clamp(turningVelocity, 1.0); + // } + + // this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); + // this.desiredTurningVelocity.set(turningVelocity); + } + + /** + * Attaches a listener for {@code FootstepStatusMessage} to the manager. + *

+ * This listener will automatically call {@link #notifyFootstepStarted(RobotSide)} and + * {@link #notifyFootstepCompleted(RobotSide)}. + *

+ * + * @param statusMessageOutputManager the output API of the controller. + */ + public void setFootstepStatusListener(StatusMessageOutputManager statusMessageOutputManager) + { + statusMessageOutputManager.attachStatusMessageListener(FootstepStatusMessage.class, this::consumeFootstepStatus); + } + + /** + * Consumes a newly received message and calls {@link #notifyFootstepStarted(RobotSide)} or + * {@link #notifyFootstepCompleted(RobotSide)} according to the status. + * + * @param statusMessage the newly received footstep status. + */ + public void consumeFootstepStatus(FootstepStatusMessage statusMessage) + { + FootstepStatus status = FootstepStatus.fromByte(statusMessage.getFootstepStatus()); + if (status == FootstepStatus.COMPLETED) + notifyFootstepCompleted(RobotSide.fromByte(statusMessage.getRobotSide())); + else if (status == FootstepStatus.STARTED) + notifyFootstepStarted(RobotSide.fromByte(statusMessage.getRobotSide())); + } + + /** + * Notifies this generator that a footstep has been completed. + *

+ * It is used internally to keep track of the support foot from which footsteps should be generated. + *

+ * + * @param robotSide the side corresponding to the foot that just completed a footstep. + */ + public void notifyFootstepCompleted(RobotSide robotSide) + { + footStates.get(robotSide).set(FootState.SUPPORT); + footStateMachines.get(robotSide).performTransition(FootState.SUPPORT); + newestSupportSide.set(robotSide); + } + + /** + * Notifies this generator that a footstep has been started, i.e. the foot started swinging. + *

+ * It is used internally to mark the first generated footstep as unmodifiable so it does not change + * while the swing foot is targeting it. + *

+ */ + public void notifyFootstepStarted(RobotSide robotSide) + { + footStates.get(robotSide).set(FootState.SWING); + footStateMachines.get(robotSide).performTransition(FootState.SWING); + } + + /** + * Sets the provider to use for getting every tick the desired turning velocity. + * + * @param desiredTurningVelocityProvider provider for obtaining the desired turning velocity. + */ + public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider desiredTurningVelocityProvider) + { + this.desiredTurningVelocityProvider = desiredTurningVelocityProvider; + } + + /** + * Sets the provider to use for getting every tick the desired forward/lateral velocity. + * + * @param desiredVelocityProvider provider for obtaining the desired forward/lateral velocity. + */ + public void setDesiredVelocityProvider(DesiredVelocityProvider desiredVelocityProvider) + { + this.desiredVelocityProvider = desiredVelocityProvider; + } + + private void setTrailingSide(RobotSide robotSide) + { + trailingSide = robotSide; + } + + 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 double getSwingDuration(RobotSide swingSide) + { + return parameters.getSwingDuration(swingSide).getDoubleValue(); + } + + public double getTransferDuration(RobotSide swingSide) + { + return parameters.getSwingDuration(swingSide).getDoubleValue() * parameters.getDoubleSupportFraction(swingSide).getDoubleValue(); + } + + public double getStepDuration(RobotSide swingSide) + { + return getSwingDuration(swingSide) + getTransferDuration(swingSide); + } + + public YoRegistry getRegistry() + { + return registry; + } + + private class SupportFootState implements State + { + private final RobotSide robotSide; + + private SupportFootState(RobotSide robotSide) + { + this.robotSide = robotSide; + } + + @Override + 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 + 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 + public void onExit(double timeInState) + { + + } + + @Override + public boolean isDone(double timeInState) + { + return footStates.get(robotSide).getEnumValue() == FootState.SWING; + } + } + + private class SwingFootState implements State + { + private final RobotSide robotSide; + + private SwingFootState(RobotSide robotSide) + { + this.robotSide = robotSide; + } + + @Override + public void onEntry() + { + calculateTouchdownPosition(touchdownCalculator.get(robotSide), + robotSide, + getStepDuration(robotSide), + pendulumBase.get(robotSide.getOppositeSide()), + netPendulumBase, + inDoubleSupport.getBooleanValue(), + desiredTouchdownPositions.get(robotSide)); + } + + @Override + 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)); + } + + @Override + public void onExit(double timeInState) + { + } + + @Override + public boolean isDone(double timeInState) + { + return footStates.get(robotSide).getEnumValue() == FootState.SUPPORT; + } + } +} \ 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 new file mode 100644 index 00000000000..62b429c985c --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderEstimates.java @@ -0,0 +1,109 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotics.screwTheory.AngularExcursionCalculator; +import us.ihmc.robotics.screwTheory.MovingZUpFrame; +import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class QuicksterFootstepProviderEstimates +{ + // CoM State Estimates + private final YoFramePoint3D currentCoMPosition; + private final YoFrameVector3D currentCoMVelocity; + private final YoFrameVector3D currentCoMLinearMomentum; + private final YoFrameVector3D currentCoMAngularMomentum; + + // CoM Frames + private final MovingReferenceFrame centerOfMassFrame; + private final MovingReferenceFrame centerOfMassControlFrame; + private final MovingZUpFrame centerOfMassControlZUpFrame; + + // Calculator for CoM momentum info + private final AngularExcursionCalculator angularExcursionCalculator; + + public QuicksterFootstepProviderEstimates(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, FrameQuaternionReadOnly desiredPelvisOrientation, double updateDT, String variableNameSuffix, YoRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) + { + currentCoMPosition = new YoFramePoint3D("currentCoMPosition" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + currentCoMVelocity = new YoFrameVector3D("currentCoMVelocity" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + currentCoMLinearMomentum = new YoFrameVector3D("currentCoMLinearMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + currentCoMAngularMomentum = new YoFrameVector3D("currentCoMAngularMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + + centerOfMassFrame = (MovingReferenceFrame) referenceFrames.getCenterOfMassFrame(); + + centerOfMassControlFrame = new MovingReferenceFrame("centerOfMassControlFrame" + variableNameSuffix, ReferenceFrame.getWorldFrame()) + { + @Override + protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) + { + // pelvisFrame.getTwistRelativeToOther(ReferenceFrame.getWorldFrame(), pelvisTwist); + // pelvisTwist.changeFrame(centerOfMassFrame); // FIXME we really want the rotation about the center of mass, relative to the world. + + twistRelativeToParentToPack.getLinearPart().setMatchingFrame(getCenterOfMassVelocity()); + // twistRelativeToParentToPack.getAngularPart().setMatchingFrame(pelvisTwist.getAngularPart()); + } + + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParent) + { + transformToParent.getTranslation().set(currentCoMPosition); + transformToParent.getRotation().set(desiredPelvisOrientation); + } + }; + + centerOfMassControlZUpFrame = new MovingZUpFrame(centerOfMassControlFrame, "centerOfMassControlZUpFrame" + variableNameSuffix); + + angularExcursionCalculator = new AngularExcursionCalculator(centerOfMassFrame, robotModel.getElevator(), updateDT, registry, null); + } + + public void update() + { + // Update CoM position and velocity from CoM frame + currentCoMPosition.setFromReferenceFrame(centerOfMassFrame); + currentCoMVelocity.setMatchingFrame(centerOfMassFrame.getTwistOfFrame().getLinearPart()); + + // Update CoM momentum info + angularExcursionCalculator.compute(); + currentCoMLinearMomentum.setMatchingFrame(angularExcursionCalculator.getLinearMomentum()); + currentCoMAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); + + // Update CoM control frames + centerOfMassControlFrame.update(); + centerOfMassControlZUpFrame.update(); + } + + public FramePoint3DReadOnly getCenterOfMassPosition() + { + return currentCoMPosition; + } + + public FrameVector3DReadOnly getCenterOfMassVelocity() + { + return currentCoMVelocity; + } + + public FrameVector3DReadOnly getCenterOfMassLinearMomentum() + { + return currentCoMLinearMomentum; + } + + public FrameVector3DReadOnly getCenterOfMassAngularMomentum() + { + return currentCoMAngularMomentum; + } + + public MovingZUpFrame getCenterOfMassControlZUpFrame() + { + return centerOfMassControlZUpFrame; + } +} 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 new file mode 100644 index 00000000000..04f254a8d8d --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderParameters.java @@ -0,0 +1,164 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; + +import us.ihmc.commons.MathTools; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import java.util.ArrayList; +import java.util.List; + +public class QuicksterFootstepProviderParameters +{ + // Static variables + private static final double SWING_DURATION = 0.55; + private static final double DOUBLE_SUPPORT_FRACTION = 0.1; + private static final double STANCE_WIDTH = 0.2; + private static final double SWING_HEIGHT = 0.07; + private static final double COM_HEIGHT = 0.9; + private static final double POLE = 0.275; + + // YoVariables + private final YoDouble swingDuration; + private final YoDouble doubleSupportFraction; + private final YoDouble stanceWidth; + private final YoDouble swingHeight; + private final YoDouble comHeight; + private final YoDouble pole; + private final YoDouble omega; + + // Stageable YoVariables + private final SideDependentList swingDurationCurrentStep = new SideDependentList<>(); + private final SideDependentList doubleSupportFractionCurrentStep = new SideDependentList<>(); + private final SideDependentList stanceWidthCurrentStep = new SideDependentList<>(); + private final SideDependentList swingHeightCurrentStep = new SideDependentList<>(); + private final SideDependentList comHeightCurrentStep = new SideDependentList<>(); + private final SideDependentList poleCurrentStep = new SideDependentList<>(); + private final SideDependentList omegaCurrentStep = new SideDependentList<>(); + + private final SideDependentList> stageableYoDoubles = new SideDependentList<>(); + + public QuicksterFootstepProviderParameters(double gravityZ, YoRegistry parentRegistry) + { + YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + String suffix2 = "QFP"; + + swingDuration = new YoDouble("swingDuration" + suffix2, registry); + doubleSupportFraction = new YoDouble("doubleSupportFraction" + suffix2, registry); + stanceWidth = new YoDouble("stanceWidth" + suffix2, registry); + swingHeight = new YoDouble("desiredSwingHeight" + suffix2, registry); + comHeight = new YoDouble("desiredComHeight" + suffix2, registry); + pole = new YoDouble("pole" + suffix2, registry); + omega = new YoDouble("omega" + suffix2, registry); + + swingDuration.set(SWING_DURATION); + doubleSupportFraction.set(DOUBLE_SUPPORT_FRACTION); + stanceWidth.set(STANCE_WIDTH); + swingHeight.set(SWING_HEIGHT); + comHeight.set(COM_HEIGHT); + pole.set(POLE); + omega.set(Math.sqrt(Math.abs(gravityZ / comHeight.getDoubleValue()))); + + for (RobotSide robotSide : RobotSide.values) + { + String suffix = robotSide.getPascalCaseName() + "CurrentStep"; + stageableYoDoubles.put(robotSide, new ArrayList<>()); + + swingDurationCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "swingDuration", suffix + suffix2, swingDuration, registry)); + doubleSupportFractionCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "doubleSupportFraction", suffix + suffix2, doubleSupportFraction, registry)); + stanceWidthCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "stanceWidthCurrentStep", suffix + suffix2, stanceWidth, registry)); + swingHeightCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "desiredSwingHeight", suffix + suffix2, swingHeight, registry)); + comHeightCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "desiredComHeight", suffix + suffix2, comHeight, registry)); + poleCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "pole", suffix + suffix2, pole, registry)); + omegaCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "omega", suffix + suffix2, omega, registry)); + + } + + comHeight.addListener(change -> omega.set(Math.sqrt(Math.abs(gravityZ / comHeight.getDoubleValue())))); + + parentRegistry.addChild(registry); + } + + private StageableYoDouble createStageableYoDouble(RobotSide robotSide, String prefix, String suffix, YoDouble valueToWatch, YoRegistry registry) + { + StageableYoDouble stageableYoDouble = new StageableYoDouble(prefix, suffix, valueToWatch, registry); + stageableYoDoubles.get(robotSide).add(stageableYoDouble); + + return stageableYoDouble; + } + + public void setParametersForUpcomingSwing(RobotSide swingSide, double swingHeight, double swingDuration, double doubleSupportFraction) + { + for (int i = 0; i < stageableYoDoubles.get(swingSide).size(); i++) + stageableYoDoubles.get(swingSide).get(i).loadFromStage(); + + if (!Double.isNaN(swingHeight) && swingHeight >= 0.02) + this.swingHeightCurrentStep.get(swingSide).set(swingHeight); + if (!Double.isNaN(swingDuration) && MathTools.intervalContains(swingDuration, 0.2, 1.0)) + this.swingDurationCurrentStep.get(swingSide).set(swingDuration); + if (!Double.isNaN(doubleSupportFraction) && MathTools.intervalContains(doubleSupportFraction, 0.0, 0.5)) + this.doubleSupportFractionCurrentStep.get(swingSide).set(doubleSupportFraction); + } + + public YoDouble getSwingDuration(RobotSide robotSide) + { + return swingDurationCurrentStep.get(robotSide); + } + + public YoDouble getDoubleSupportFraction(RobotSide robotSide) + { + return doubleSupportFractionCurrentStep.get(robotSide); + } + + public YoDouble getStanceWidth(RobotSide robotSide) + { + return stanceWidthCurrentStep.get(robotSide); + } + + public YoDouble getDesiredSwingHeight(RobotSide robotSide) + { + return swingHeightCurrentStep.get(robotSide); + } + + public YoDouble getDesiredCoMHeight(RobotSide robotSide) + { + return comHeightCurrentStep.get(robotSide); + } + + public YoDouble getPole(RobotSide robotSide) + { + return poleCurrentStep.get(robotSide); + } + + public YoDouble getOmega(RobotSide robotSide) + { + return omegaCurrentStep.get(robotSide); + } + + private static class StageableYoDouble extends YoDouble + { + private double stagedValue; + private final YoDouble valueToWatch; + + public StageableYoDouble(String prefix, String suffix, YoDouble valueToWatch, YoRegistry registry) + { + super(prefix + suffix, registry); + this.valueToWatch = valueToWatch; + stagedValue = valueToWatch.getDoubleValue(); + loadFromStage(); + valueToWatch.addListener((v) -> stageValue()); + } + + private void stageValue() + { + if (stagedValue != valueToWatch.getDoubleValue()) + stagedValue = valueToWatch.getDoubleValue(); + } + + public void loadFromStage() + { + if (getDoubleValue() != stagedValue) + set(stagedValue); + } + } +} \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java new file mode 100644 index 00000000000..152b3ec0f6c --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java @@ -0,0 +1,518 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider; + +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class QuicksterFootstepProviderTouchdownCalculator +{ + private final MovingReferenceFrame centerOfMassControlZUpFrame; + + private final FrameVector3DReadOnly centerOfMassVelocity; + + private final FrameVector3DReadOnly centroidalAngularMomentum; + + // private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; + + private final YoBoolean isInitialStanceValid; + + // This is the stance foot at the beginning of the contact sequence. This is only used if the controller is using the TD;LO controller. + private final YoFramePoint3D stanceFootPositionAtBeginningOfState; + + // This is the desired touchdown position using the capture point, assuming no stance width and walking in place. + private final YoFramePoint2D desiredALIPTouchdownPositionWithoutDS; + private final YoFramePoint2D desiredALIPTouchdownPositionWithDS; + + // This is the desired touchdown position using the capture point, assuming no stance width and walking in place. + private final YoFrameVector2D predictedALIPVelocityAtTouchdownWithoutDS; + private final YoFrameVector2D predictedALIPVelocityAtTouchdownWithDS; + private final FrameVector2D predictedVelocityAtTouchdown2D = new FrameVector2D(); + private final YoFrameVector3D predictedVelocityAtTouchdown3D; + + // This is the desired touchdown position to be used by the robot + private final YoFramePoint3D desiredTouchdownPositionInWorld3D; + private final YoFramePoint2D desiredTouchdownPositionInCOM2D; + private final FramePoint2D desiredTouchdownPosition2D; + + // This is the desired lateral offset from the touchdown location returned by the TD;LO and capture point laws to achieve stable walking at the desired + // nominal width. + private final YoDouble touchdownPositionOffsetForDesiredStanceWidth; + // This is the desired lateral offset in the X and Y directions of the touchdown position to achieve walking at a desired speed. + private final YoFrameVector2D touchdownPositionOffsetForDesiredSpeed; + + private final SideDependentList soleFrames; + private final Vector2DReadOnly desiredVelocityProvider; + + // Temp value used for calculation + private final FrameVector2D tempVector = new FrameVector2D(); + + // + private final FrameVector2D angularMomentum = new FrameVector2D(); + + // Gains for Capture Point controllers + private final YoDouble capturePointTimeConstantX; + private final YoDouble capturePointTimeConstantY; + private final YoDouble capturePointTimeConstantXWithoutDS; + private final YoDouble capturePointTimeConstantYWithoutDS; + + // Poles for the Capture Point Controllers + private final YoDouble capturePointPole; + + // Natural frequency of system + // TODO make sure these are updating + private final YoDouble omegaX; + private final YoDouble omegaY; + + private final YoDouble desiredCoMHeight; + private final YoDouble swingDuration; + private final YoDouble doubleSupportFraction; + private final YoDouble stanceWidth; + + private final YoDouble timeToReachGoal; + private final RobotSide robotSide; + + private final double mass; + private final double gravity; + + public QuicksterFootstepProviderTouchdownCalculator(RobotSide robotSide, + MovingReferenceFrame centerOfMassControlZUpFrame, + FrameVector3DReadOnly centerOfMassVelocity, + FrameVector3DReadOnly centroidalAngularMomentum, + FullHumanoidRobotModel robotModel, + QuicksterFootstepProviderParameters parameters, + double gravity, + Vector2DReadOnly desiredVelocityProvider, + YoRegistry parentRegistry) + { + this.robotSide = robotSide; + this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; + this.centerOfMassVelocity = centerOfMassVelocity; + this.centroidalAngularMomentum = centroidalAngularMomentum; + // this.groundPlaneEstimator = groundPlaneEstimator; + this.desiredVelocityProvider = desiredVelocityProvider; + soleFrames = robotModel.getSoleFrames(); + mass = robotModel.getTotalMass(); + this.gravity = gravity; + + YoRegistry registry = new YoRegistry(robotSide.getCamelCaseName() + getClass().getSimpleName()); + + String prefix = robotSide.getCamelCaseName() + "TouchdownCalculator"; + + timeToReachGoal = new YoDouble(prefix + "TimeToReachGoal", registry); + + isInitialStanceValid = new YoBoolean(prefix + "IsInitialStancePositionValid", registry); + stanceFootPositionAtBeginningOfState = new YoFramePoint3D(prefix + "StanceFootPositionAtBeginningOfState", + centerOfMassControlZUpFrame, + registry); + + // Predicted CoM velocity as computed by each different touchdown calculator + predictedALIPVelocityAtTouchdownWithoutDS = new YoFrameVector2D(prefix + "PredictedALIPVelocityAtTouchdownWithoutDS", centerOfMassControlZUpFrame, registry); + predictedALIPVelocityAtTouchdownWithDS = new YoFrameVector2D(prefix + "PredictedALIPVelocityAtTouchdownWithDS", centerOfMassControlZUpFrame, registry); + + // Desired touchdown position as computed by each different touchdown calculator + desiredALIPTouchdownPositionWithoutDS = new YoFramePoint2D(prefix + "DesiredALIPTouchdownPositionWithoutDS", centerOfMassControlZUpFrame, registry); + desiredALIPTouchdownPositionWithDS = new YoFramePoint2D(prefix + "DesiredALIPTouchdownPositionWithDS", centerOfMassControlZUpFrame, registry); + + // Final touchdown position and predicted velocity. These are populated by the touchdown calculator currently in use + desiredTouchdownPositionInWorld3D = new YoFramePoint3D(prefix + "DesiredTouchdownPositionInWorld", ReferenceFrame.getWorldFrame(), registry); + desiredTouchdownPositionInCOM2D = new YoFramePoint2D(prefix + "DesiredTouchdownPositionInCOM2D", centerOfMassControlZUpFrame, registry); + desiredTouchdownPosition2D = new FramePoint2D(centerOfMassControlZUpFrame); + predictedVelocityAtTouchdown3D = new YoFrameVector3D(prefix + "PredictedVelocityAtTouchdownInWorld", ReferenceFrame.getWorldFrame(), registry); + + // Step position offsets for desired walking speed and stance width + touchdownPositionOffsetForDesiredStanceWidth = new YoDouble(prefix + "TouchdownPositionOffsetForDesiredStanceWidth", registry); + touchdownPositionOffsetForDesiredSpeed = new YoFrameVector2D(prefix + "TouchdownPositionOffsetForDesiredSpeed", + centerOfMassControlZUpFrame, + registry); + + capturePointTimeConstantX = new YoDouble(prefix + "CapturePointTimeConstantX", registry); + capturePointTimeConstantY = new YoDouble(prefix + "CapturePointTimeConstantY", registry); + capturePointTimeConstantXWithoutDS = new YoDouble(prefix + "CapturePointTimeConstantXWithoutDS", registry); + capturePointTimeConstantYWithoutDS = new YoDouble(prefix + "CapturePointTimeConstantYWithoutDS", registry); + + omegaX = parameters.getOmega(robotSide); + omegaY = parameters.getOmega(robotSide); + + capturePointPole = parameters.getPole(robotSide); + + desiredCoMHeight = parameters.getDesiredCoMHeight(robotSide); + swingDuration = parameters.getSwingDuration(robotSide); + doubleSupportFraction = parameters.getDoubleSupportFraction(robotSide); + stanceWidth = parameters.getStanceWidth(robotSide); + + omegaX.addListener(v -> computeCapturePointTimeConstant()); + omegaY.addListener(v -> computeCapturePointTimeConstant()); + capturePointPole.addListener(v -> computeCapturePointTimeConstant()); + + desiredCoMHeight.addListener(v -> computeCapturePointTimeConstant()); + swingDuration.addListener(v -> computeCapturePointTimeConstant()); + doubleSupportFraction.addListener(v -> computeCapturePointTimeConstant()); + + computeCapturePointTimeConstant(); + + parentRegistry.addChild(registry); + } + + public void reset() + { + desiredALIPTouchdownPositionWithoutDS.setToNaN(); + desiredALIPTouchdownPositionWithDS.setToNaN(); + desiredTouchdownPositionInWorld3D.setToNaN(); + + isInitialStanceValid.set(false); + } + + public void initializeForContactChange(RobotSide supportSide) + { + isInitialStanceValid.set(true); + stanceFootPositionAtBeginningOfState.setFromReferenceFrame(soleFrames.get(supportSide)); + } + + public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeToReachGoal, FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean isInDoubleSupport) + { + if (pendulumBase != null) + pendulumBase.checkReferenceFrameMatch(centerOfMassControlZUpFrame); + + this.timeToReachGoal.set(timeToReachGoal); + + // compute all the touchdown positions and their offset values + computeDesiredALIPTouchdownPosition(timeToReachGoal, pendulumBase); + + desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithoutDS); + predictedVelocityAtTouchdown2D.setIncludingFrame(predictedALIPVelocityAtTouchdownWithoutDS); + predictedVelocityAtTouchdown2D.changeFrameAndProjectToXYPlane(predictedVelocityAtTouchdown3D.getReferenceFrame()); + predictedVelocityAtTouchdown3D.set(predictedVelocityAtTouchdown2D); + + // compute offsets for desired walking speed and stance width + computeDesiredTouchdownOffsetForVelocity(supportSide); + computeDesiredTouchdownOffsetForStanceWidth(supportSide); + + // offset the touchdown position based on the desired velocity + desiredTouchdownPosition2D.add(touchdownPositionOffsetForDesiredSpeed.getX(), touchdownPositionOffsetForDesiredSpeed.getY()); + + // offset the touchdown position based on the stance width + desiredTouchdownPosition2D.addY(touchdownPositionOffsetForDesiredStanceWidth.getValue()); + + // add extra offsets for fixing drifting + // desiredTouchdownPosition2D.addX(fastWalkingParameters.getFixedForwardOffset()); + // desiredTouchdownPosition2D.addY(supportSide.negateIfLeftSide(fastWalkingParameters.getFixedWidthOffset())); + + // determine touchdown z position from x position, y position, and ground plane + desiredTouchdownPositionInWorld3D.setMatchingFrame(desiredTouchdownPosition2D, 0.0); + desiredTouchdownPositionInCOM2D.setMatchingFrame(desiredTouchdownPosition2D); + } + + public FramePoint3DReadOnly getDesiredTouchdownPositionInWorld3D() + { + return desiredTouchdownPositionInWorld3D; + } + + public FramePoint2DReadOnly getDesiredTouchdownPosition2D() + { + return desiredTouchdownPosition2D; + } + + public FrameVector3DReadOnly getPredictedVelocityAtTouchdown3D() + { + return predictedVelocityAtTouchdown3D; + } + + private void computeDesiredALIPTouchdownPosition(double timeToReachGoal, FramePoint2DReadOnly pendulumBase) + { + + // compute the current center of mass velocity + tempVector.setIncludingFrame(centerOfMassVelocity); + tempVector.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + + angularMomentum.setIncludingFrame(centroidalAngularMomentum); + angularMomentum.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + + // the capture point touchdown position is just the capture point. Which is the scaled velocity based on the time constant. + if (pendulumBase != null && Double.isFinite(timeToReachGoal)) + { + double omegaTX = omegaX.getDoubleValue() * timeToReachGoal; + double omegaTY = omegaY.getDoubleValue() * timeToReachGoal; + double coshX = Math.cosh(omegaTX); + double coshY = Math.cosh(omegaTY); + double heightX = gravity / (omegaX.getDoubleValue() * omegaX.getDoubleValue()); + double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); + double mhX = mass * heightX; + double mhY = mass * heightY; + + predictedALIPVelocityAtTouchdownWithoutDS.set(coshX * tempVector.getX(), coshY * tempVector.getY()); + predictedALIPVelocityAtTouchdownWithoutDS.add(coshX / mhX * angularMomentum.getY(), -coshY / mhY * angularMomentum.getX()); + predictedALIPVelocityAtTouchdownWithoutDS.add(-omegaX.getDoubleValue() * Math.sinh(omegaTX) * pendulumBase.getX(), + -omegaY.getDoubleValue() * Math.sinh(omegaTY) * pendulumBase.getY()); + } + else + { + double heightX = gravity / (omegaX.getDoubleValue() * omegaX.getDoubleValue()); + double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); + double mhX = mass * heightX; + double mhY = mass * heightY; + + predictedALIPVelocityAtTouchdownWithoutDS.set(tempVector); + predictedALIPVelocityAtTouchdownWithoutDS.addX(angularMomentum.getY()/ mhX); + predictedALIPVelocityAtTouchdownWithoutDS.addY(-angularMomentum.getX() / mhY); + } + + desiredALIPTouchdownPositionWithoutDS.set(capturePointTimeConstantXWithoutDS.getDoubleValue() * predictedALIPVelocityAtTouchdownWithoutDS.getX(), + capturePointTimeConstantYWithoutDS.getDoubleValue() * predictedALIPVelocityAtTouchdownWithoutDS.getY()); + } + + private void computeDesiredALIPTouchdownPositionWithDS(double timeToReachGoal, FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean isInDoubleSupport) + { + + // Get CoM velocity and change frame to CoM control frame + tempVector.setIncludingFrame(centerOfMassVelocity); + tempVector.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + + // Get CoM angular momentum and change frame to CoM control frame + angularMomentum.setIncludingFrame(centroidalAngularMomentum); + angularMomentum.changeFrame(centerOfMassControlZUpFrame); + + // The capture point touchdown position is just the capture point. Which is the scaled velocity based on the time constant + if (pendulumBase != null && netPendulumBase != null && Double.isFinite(timeToReachGoal)) + { + double heightX = gravity / (omegaX.getDoubleValue() * omegaX.getDoubleValue()); + double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); + double mhX = mass * heightX; + double mhY = mass * heightY; + double doubleSupportDuration = swingDuration.getDoubleValue() * doubleSupportFraction.getDoubleValue(); + double B2X; + double B2Y; + double C2X; + double C2Y; + + // Prediction horizons to project/predict state at end of single and double support + double singleSupportHorizon; + double doubleSupportHorizon; + if (isInDoubleSupport) + { + singleSupportHorizon = 0.0; + doubleSupportHorizon = timeToReachGoal; + } + else + { + singleSupportHorizon = timeToReachGoal - doubleSupportDuration; + doubleSupportHorizon = doubleSupportDuration; + } + + double coshXSingleSupport = Math.cosh(omegaX.getDoubleValue() * singleSupportHorizon); + double sinhXSingleSupport = Math.sinh(omegaX.getDoubleValue() * singleSupportHorizon); + double coshYSingleSupport = Math.cosh(omegaY.getDoubleValue() * singleSupportHorizon); + double sinhYSingleSupport = Math.sinh(omegaY.getDoubleValue() * singleSupportHorizon); + + double coshXDoubleSupport = Math.cosh(omegaX.getDoubleValue() * doubleSupportHorizon); + double sinhXDoubleSupport = Math.sinh(omegaX.getDoubleValue() * doubleSupportHorizon); + double coshYDoubleSupport = Math.cosh(omegaY.getDoubleValue() * doubleSupportHorizon); + double sinhYDoubleSupport = Math.sinh(omegaY.getDoubleValue() * doubleSupportHorizon); + + // Current state variables + double comPositionX = -pendulumBase.getX(); + double comPositionY = -pendulumBase.getY(); + double comVelocityX = tempVector.getX(); + double comVelocityY = tempVector.getY(); + double Ly = mhX * comVelocityX + angularMomentum.getY(); + double Lx = -mhY * comVelocityY + angularMomentum.getX(); + + // State variables at end of single support + double comXAtEndOfSingleSupport = comPositionX * coshXSingleSupport + Ly * sinhXSingleSupport / (omegaX.getDoubleValue() * mhX); + double LyAtEndOfSingleSupport = comPositionX * mhX * omegaX.getDoubleValue() * sinhXSingleSupport + Ly * coshXSingleSupport; + + double comYAtEndOfSingleSupport = comPositionY * coshYSingleSupport - Lx * sinhYSingleSupport / (omegaY.getDoubleValue() * mhY); + double LxAtEndOfSingleSupport = -comPositionY * mhY * omegaY.getDoubleValue() * sinhYSingleSupport + Lx * coshYSingleSupport; + + // Arbitrary constant resulting from double support dynamics + if (doubleSupportDuration > 0) + { + B2X = mhX * (1-coshXDoubleSupport)/doubleSupportDuration; + B2Y = -mhY * (1-coshYDoubleSupport)/doubleSupportDuration; + C2X = -mhX * omegaX.getDoubleValue()*sinhXDoubleSupport; + C2Y = mhY * omegaY.getDoubleValue()*sinhYDoubleSupport; + } + else + { + B2X = 0.0; + B2Y = 0.0; + C2X = 0.0; + C2Y = 0.0; + } + + // More arbitrary constants + double DX = 1.0 / (1.0 - B2X * capturePointTimeConstantX.getDoubleValue() / mhX); + double DY = 1.0 / (1.0 + B2Y * capturePointTimeConstantY.getDoubleValue() / mhY); + double EX = C2X * netPendulumBase.getX() / mhX; + double EY = -C2Y * netPendulumBase.getY() / mhY; + + double xDotAtEndOfDoubleSupport = DX * (comXAtEndOfSingleSupport * omegaX.getDoubleValue() * sinhXDoubleSupport + LyAtEndOfSingleSupport * coshXDoubleSupport / mhX + EX); + double yDotAtEndOfDoubleSupport = DY * (comYAtEndOfSingleSupport * omegaY.getDoubleValue() * sinhYDoubleSupport - LxAtEndOfSingleSupport * coshYDoubleSupport / mhY + EY); + + predictedALIPVelocityAtTouchdownWithDS.setX(xDotAtEndOfDoubleSupport); + predictedALIPVelocityAtTouchdownWithDS.setY(yDotAtEndOfDoubleSupport); + } + else + { + double heightX = gravity / (omegaX.getDoubleValue() * omegaX.getDoubleValue()); + double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); + double mhX = mass * heightX; + double mhY = mass * heightY; + + predictedALIPVelocityAtTouchdownWithDS.set(tempVector); + predictedALIPVelocityAtTouchdownWithDS.addX(angularMomentum.getY() / mhX); + predictedALIPVelocityAtTouchdownWithDS.addY(-angularMomentum.getX() / mhY); + } + + desiredALIPTouchdownPositionWithDS.set(capturePointTimeConstantX.getDoubleValue() * predictedALIPVelocityAtTouchdownWithDS.getX(), + capturePointTimeConstantY.getDoubleValue() * predictedALIPVelocityAtTouchdownWithDS.getY()); + } + + private void computeCapturePointTimeConstant() + { + double omegaX = this.omegaX.getDoubleValue(); + double omegaY = this.omegaY.getDoubleValue(); + double heightX = gravity / (omegaX * omegaX); + double heightY = gravity / (omegaY * omegaY); + double m = mass; + double lambda = capturePointPole.getDoubleValue(); + double singleSupportDuration = swingDuration.getDoubleValue(); + double doubleSupportDuration = singleSupportDuration * doubleSupportFraction.getDoubleValue(); + + double coshXDS = Math.cosh(omegaX * doubleSupportDuration); + double coshYDS = Math.cosh(omegaY * doubleSupportDuration); + double sinhXDS = Math.sinh(omegaX * doubleSupportDuration); + double sinhYDS = Math.sinh(omegaY * doubleSupportDuration); + double coshXSS = Math.cosh(omegaX * singleSupportDuration); + double coshYSS = Math.cosh(omegaY * singleSupportDuration); + double sinhXSS = Math.sinh(omegaX * singleSupportDuration); + double sinhYSS = Math.sinh(omegaY * singleSupportDuration); + + double timeConstantXWithDS; + double timeConstantYWithDS; + double timeConstantXNoDS = (coshXSS - lambda) / (omegaX * sinhXSS); + double timeConstantYNoDS = (coshYSS - lambda) / (omegaY * sinhYSS); + capturePointTimeConstantXWithoutDS.set(timeConstantXNoDS); + capturePointTimeConstantYWithoutDS.set(timeConstantYNoDS); + + if (doubleSupportDuration > 0) + { + timeConstantXWithDS = (doubleSupportDuration * (coshXDS * coshXSS - lambda + sinhXDS * sinhXSS)) / + (coshXDS * lambda - lambda + doubleSupportDuration * coshXDS * omegaX * sinhXSS + doubleSupportDuration * coshXSS * omegaX * sinhXDS); + + timeConstantYWithDS = (doubleSupportDuration * (coshYDS * coshYSS - lambda + sinhYDS * sinhYSS)) / + (coshYDS * lambda - lambda + doubleSupportDuration * coshYDS * omegaY * sinhYSS + doubleSupportDuration * coshYSS * omegaY * sinhYDS); + + capturePointTimeConstantX.set(timeConstantXWithDS); + capturePointTimeConstantY.set(timeConstantYWithDS); + } + else + { + capturePointTimeConstantX.set(timeConstantXNoDS); + capturePointTimeConstantY.set(timeConstantYNoDS); + } + } + + private void computeDesiredTouchdownOffsetForVelocity(RobotSide supportSide) + { + // this is the desired offset distance to make the robot stably walk at the desired velocity + computeDesiredTouchdownOffsetForVelocity(swingDuration.getDoubleValue(), + doubleSupportFraction.getDoubleValue() * swingDuration.getDoubleValue(), + supportSide, + 1/capturePointTimeConstantX.getDoubleValue(), + 1/capturePointTimeConstantY.getDoubleValue(), + desiredVelocityProvider, + touchdownPositionOffsetForDesiredSpeed); + } + + public static void computeDesiredTouchdownOffsetForVelocity(double swingDuration, + double doubleSupportDuration, + RobotSide supportSide, + double omegaX, + double omegaY, + Vector2DReadOnly desiredVelocity, + FixedFrameVector2DBasics touchdownPositionOffsetToPack) + { + // this is the desired offset distance to make the robot stably walk at the desired velocity + + double forwardOffset = computeForwardTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, omegaX, desiredVelocity.getX()); + + double lateralOffset = computeLateralTouchdownOffsetForVelocity(swingDuration, doubleSupportDuration, supportSide, omegaY, desiredVelocity.getY()); + + touchdownPositionOffsetToPack.set(forwardOffset, lateralOffset); + } + + private static double computeForwardTouchdownOffsetForVelocity(double swingDuration, double doubleSupportDuration, double omega, double desiredVelocity) + { + 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); + } + + private static double computeLateralTouchdownOffsetForVelocity(double swingDuration, + double doubleSupportDuration, + RobotSide supportSide, + double omega, + double desiredVelocity) + { + 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; + } + + private void computeDesiredTouchdownOffsetForStanceWidth(RobotSide supportSide) + { + // this is the desired offset distance to make the robot stably walk in place. + touchdownPositionOffsetForDesiredStanceWidth.set(computeDesiredTouchdownOffsetForStanceWidth(swingDuration.getDoubleValue(), + doubleSupportFraction.getDoubleValue() * swingDuration.getDoubleValue(), + stanceWidth.getDoubleValue(), + supportSide, + 1/capturePointTimeConstantY.getDoubleValue())); + } + + public static double computeDesiredTouchdownOffsetForStanceWidth(double swingDuration, + double doubleSupportDuration, + double stanceWidth, + RobotSide supportSide, + double omega) + { + 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); + } +} \ No newline at end of file From ef60a8d8f0c52f13a934049facdcf83a512d357d Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 4 Dec 2024 22:08:32 -0600 Subject: [PATCH 02/18] added QFP to CSG --- .../avatar/AvatarStepGeneratorThread.java | 3 ++- .../scs2/SCS2AvatarSimulationFactory.java | 2 +- .../ContinuousStepGenerator.java | 27 +++++++++++++++++-- .../QuicksterFootstepProvider.java | 22 ++++----------- ...edFootstepDataMessageGeneratorFactory.java | 11 +++++++- .../plugin/HumanoidSteppingPluginFactory.java | 7 +++-- .../JoystickBasedSteppingPluginFactory.java | 11 +++++--- .../VelocityBasedSteppingPluginFactory.java | 4 ++- 8 files changed, 59 insertions(+), 28 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java index b65acfc9f4c..649b5b57e34 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java @@ -98,7 +98,8 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory, pluginFactory.createStepGeneratorNetworkSubscriber(drcRobotModel.getSimpleRobotName(), ros2Node); humanoidReferenceFrames = new HumanoidReferenceFrames(fullRobotModel); - continuousStepGeneratorPlugin = pluginFactory.buildPlugin(humanoidReferenceFrames, + continuousStepGeneratorPlugin = pluginFactory.buildPlugin(fullRobotModel, + humanoidReferenceFrames, drcRobotModel.getStepGeneratorDT(), drcRobotModel.getWalkingControllerParameters(), walkingOutputManager, diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java index 6c7f7dcb8c5..ab255e43f2b 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java @@ -460,7 +460,7 @@ private void setupStepGeneratorThread() else { JoystickBasedSteppingPluginFactory joystickPluginFactory = new JoystickBasedSteppingPluginFactory(); - if (heightMapForFootstepZ.hasValue()) + if (heightMapForFootstepZ.hasValue() && heightMapForFootstepZ.get() != null) joystickPluginFactory.setFootStepAdjustment(new HeightMapBasedFootstepAdjustment(heightMapForFootstepZ.get())); else { 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 fe87f5efaaf..069fd3c7619 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 @@ -14,6 +14,7 @@ import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.commonWalkingControlModules.controllers.Updatable; import us.ihmc.commonWalkingControlModules.desiredFootStep.FootstepVisualizer; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider.QuicksterFootstepProvider; import us.ihmc.commons.MathTools; import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; @@ -36,6 +37,7 @@ import us.ihmc.robotics.time.ExecutionTimer; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition; +import us.ihmc.tools.factories.OptionalFactoryField; import us.ihmc.yoVariables.euclid.YoVector2D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; import us.ihmc.yoVariables.providers.BooleanProvider; @@ -157,6 +159,11 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder private final MutableObject latestStatusReceived = new MutableObject<>(null); private final MutableObject footstepCompletionSide = new MutableObject<>(null); + // All things QFP + public enum CSGMode {STANDARD, QFP} + private final YoEnum csgMode = new YoEnum<>("csgMode", registry, CSGMode.class); + private final OptionalFactoryField quicksterFootstepProvider = new OptionalFactoryField<>("QuicksterFootstepProviderField"); + /** * Creates a new step generator, its {@code YoVariable}s will not be attached to any registry. */ @@ -176,7 +183,7 @@ public ContinuousStepGenerator(YoRegistry parentRegistry) parentRegistry.addChild(registry); parameters.clear(); - numberOfTicksBeforeSubmittingFootsteps.set(2); + numberOfTicksBeforeSubmittingFootsteps.set(0); currentFootstepDataListCommandID.set(new Random().nextLong(0, Long.MAX_VALUE / 2)); // To make this command ID unique setSupportFootBasedFootstepAdjustment(true); @@ -230,7 +237,8 @@ else if (startWalkingMessenger != null && walk.getValue() != walkPreviousValue.g counter = numberOfTicksBeforeSubmittingFootsteps.getValue(); // To make footsteps being sent right away. } - { // Processing footstep status + // Processing footstep status + { FootstepStatus statusToProcess = latestStatusReceived.getValue(); if (statusToProcess != null) @@ -253,6 +261,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) footstepCompletionSide.setValue(null); } + // Determine swing side RobotSide swingSide; if (footsteps.isEmpty()) @@ -510,6 +519,9 @@ public void setSwingHeight(double swingHeight) public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider desiredTurningVelocityProvider) { this.desiredTurningVelocityProvider = desiredTurningVelocityProvider; + + if (quicksterFootstepProvider.hasValue()) + quicksterFootstepProvider.get().setDesiredTurningVelocityProvider(desiredTurningVelocityProvider); } /** @@ -520,6 +532,9 @@ public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider des public void setDesiredVelocityProvider(DesiredVelocityProvider desiredVelocityProvider) { this.desiredVelocityProvider = desiredVelocityProvider; + + if (quicksterFootstepProvider.hasValue()) + quicksterFootstepProvider.get().setDesiredVelocityProvider(desiredVelocityProvider); } /** @@ -601,6 +616,9 @@ public void notifyFootstepStarted() public void setFootstepStatusListener(StatusMessageOutputManager statusMessageOutputManager) { statusMessageOutputManager.attachStatusMessageListener(FootstepStatusMessage.class, this::consumeFootstepStatus); + + if (quicksterFootstepProvider.hasValue()) + quicksterFootstepProvider.get().setFootstepStatusListener(statusMessageOutputManager); } /** @@ -756,6 +774,11 @@ public void setAlternateStepChooser(AlternateStepChooser alternateStepChooser) this.alternateStepChooser = alternateStepChooser; } + public void setQuicksterFootstepProvider(QuicksterFootstepProvider quicksterFootstepProvider) + { + this.quicksterFootstepProvider.set(quicksterFootstepProvider); + } + /** * 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/QuicksterFootstepProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProvider.java index c6056ea0342..ea94fc34eb0 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 @@ -82,7 +82,7 @@ public enum FootState {SUPPORT, SWING} private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; private DesiredTurningVelocityProvider desiredTurningVelocityProvider = () -> 0.0; - public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) + public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) { this.updateDT = updateDT; this.referenceFrames = referenceFrames; @@ -156,6 +156,8 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, netPendulumBaseViz); yoGraphicsListRegistry.registerArtifact(variableNameSuffix, netPendulumBaseViz.createArtifact()); + + parentRegistry.addChild(registry); } @Override @@ -283,22 +285,8 @@ private void handleDesiredsFromProviders() double desiredVelocityY = desiredVelocity.getY(); double turningVelocity = desiredTurningVelocityProvider.getTurningVelocity(); - // if (desiredVelocityProvider.isUnitVelocity()) - // { - // double minMaxVelocityX = maxStepLength / stepTime.getValue(); - // double minMaxVelocityY = maxStepWidth / stepTime.getValue(); - // desiredVelocityX = minMaxVelocityX * MathTools.clamp(desiredVelocityX, 1.0); - // desiredVelocityY = minMaxVelocityY * MathTools.clamp(desiredVelocityY, 1.0); - // } - // - // if (desiredTurningVelocityProvider.isUnitVelocity()) - // { - // double minMaxVelocityTurn = (turnMaxAngleOutward - turnMaxAngleInward) / stepTime.getValue(); - // turningVelocity = minMaxVelocityTurn * MathTools.clamp(turningVelocity, 1.0); - // } - - // this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); - // this.desiredTurningVelocity.set(turningVelocity); + this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); + this.desiredTurningVelocity.set(turningVelocity); } /** 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 4f1ccb859cb..442f44f4138 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 @@ -8,11 +8,13 @@ import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.commonWalkingControlModules.controllers.Updatable; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.*; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.quicksterFootstepProvider.QuicksterFootstepProvider; import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -121,7 +123,8 @@ public StepGeneratorCommandInputManager getStepGeneratorCommandInputManager() } @Override - public ComponentBasedFootstepDataMessageGenerator buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + public ComponentBasedFootstepDataMessageGenerator buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, double updateDT, WalkingControllerParameters walkingControllerParameters, StatusMessageOutputManager walkingStatusMessageOutputManager, @@ -136,6 +139,12 @@ public ComponentBasedFootstepDataMessageGenerator buildPlugin(CommonHumanoidRefe FactoryTools.checkAllFactoryFieldsAreSet(this); ContinuousStepGenerator continuousStepGenerator = new ContinuousStepGenerator(registryField.get()); + continuousStepGenerator.setQuicksterFootstepProvider(new QuicksterFootstepProvider(robotModel, + referenceFrames, + updateDT, + registryField.get(), + yoGraphicsListRegistry, + timeProvider)); if (createSupportFootBasedFootstepAdjustment.hasValue() && createSupportFootBasedFootstepAdjustment.get()) continuousStepGenerator.setSupportFootBasedFootstepAdjustment(adjustPitchAndRoll.hasValue() && adjustPitchAndRoll.get()); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java index e9b365897e9..3c79f4701ff 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java @@ -15,6 +15,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.ros2.ROS2Topic; @@ -51,7 +52,8 @@ default HumanoidSteppingPlugin buildPlugin(HighLevelControllerFactoryHelper cont { HighLevelHumanoidControllerToolbox controllerToolbox = controllerFactoryHelper.getHighLevelHumanoidControllerToolbox(); - return buildPlugin(controllerToolbox.getReferenceFrames(), + return buildPlugin(controllerToolbox.getFullRobotModel(), + controllerToolbox.getReferenceFrames(), controllerToolbox.getControlDT(), controllerFactoryHelper.getWalkingControllerParameters(), controllerFactoryHelper.getStatusMessageOutputManager(), @@ -61,7 +63,8 @@ default HumanoidSteppingPlugin buildPlugin(HighLevelControllerFactoryHelper cont controllerToolbox.getYoTime()); } - HumanoidSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + HumanoidSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, double updateDT, WalkingControllerParameters walkingControllerParameters, StatusMessageOutputManager walkingStatusMessageOutputManager, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java index 5edab54dc19..aca34efea39 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java @@ -12,6 +12,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -84,7 +85,8 @@ public StepGeneratorCommandInputManager getStepGeneratorCommandInputManager() } @Override - public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + public JoystickBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, double updateDT, WalkingControllerParameters walkingControllerParameters, StatusMessageOutputManager walkingStatusMessageOutputManager, @@ -93,7 +95,8 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref SideDependentList contactableFeet, DoubleProvider timeProvider) { - ComponentBasedFootstepDataMessageGenerator csgFootstepGenerator = csgPluginFactory.buildPlugin(referenceFrames, + ComponentBasedFootstepDataMessageGenerator csgFootstepGenerator = csgPluginFactory.buildPlugin(robotModel, + referenceFrames, updateDT, walkingControllerParameters, walkingStatusMessageOutputManager, @@ -101,7 +104,9 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref yoGraphicsListRegistry, contactableFeet, timeProvider); - VelocityBasedSteppingPlugin fastWalkingPlugin = velocityPluginFactory.buildPlugin(referenceFrames, + + VelocityBasedSteppingPlugin fastWalkingPlugin = velocityPluginFactory.buildPlugin(robotModel, + referenceFrames, updateDT, walkingControllerParameters, walkingStatusMessageOutputManager, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java index 8a402926057..688f7e5c63e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java @@ -10,6 +10,7 @@ import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -93,7 +94,8 @@ public StepGeneratorCommandInputManager getStepGeneratorCommandInputManager() } @Override - public VelocityBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + public VelocityBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, double updateDT, WalkingControllerParameters walkingControllerParameters, StatusMessageOutputManager walkingStatusMessageOutputManager, From 340f1abfe2914d546b1b592abb8428f1a0fa803b Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 4 Dec 2024 22:21:59 -0600 Subject: [PATCH 03/18] QFP steps added to CSG FootstepDataList --- .../ContinuousStepGenerator.java | 107 ++++++++++++++---- 1 file changed, 84 insertions(+), 23 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 069fd3c7619..8167e427aff 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 @@ -205,12 +205,6 @@ public void update(double time) { stepGeneratorTimer.startMeasurement(); - footstepDataListMessage.setDefaultSwingDuration(parameters.getSwingDuration()); - footstepDataListMessage.setDefaultTransferDuration(parameters.getTransferDuration()); - footstepDataListMessage.setFinalTransferDuration(parameters.getTransferDuration()); - footstepDataListMessage.setAreFootstepsAdjustable(parameters.getStepsAreAdjustable()); - footstepDataListMessage.setOffsetFootstepsWithExecutionError(parameters.getShiftUpcomingStepsWithTouchdown()); - if (!ignoreWalkInputProvider.getBooleanValue() && walkInputProvider != null) walk.set(walkInputProvider.getValue()); @@ -283,6 +277,24 @@ else if (statusToProcess == FootstepStatus.COMPLETED) previousFootstepPose.set(previousFootstep.getLocation(), previousFootstep.getOrientation()); } + // Set footstep parameters + if (csgMode.getEnumValue() == CSGMode.QFP && quicksterFootstepProvider.hasValue()) + { + footstepDataListMessage.setDefaultSwingDuration(quicksterFootstepProvider.get().getSwingDuration(swingSide)); + footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide)); + footstepDataListMessage.setAreFootstepsAdjustable(false); + footstepDataListMessage.setOffsetFootstepsWithExecutionError(false); + } + else + { + footstepDataListMessage.setDefaultSwingDuration(parameters.getSwingDuration()); + footstepDataListMessage.setDefaultTransferDuration(parameters.getTransferDuration()); + footstepDataListMessage.setFinalTransferDuration(parameters.getTransferDuration()); + footstepDataListMessage.setAreFootstepsAdjustable(parameters.getStepsAreAdjustable()); + footstepDataListMessage.setOffsetFootstepsWithExecutionError(parameters.getShiftUpcomingStepsWithTouchdown()); + } + double maxStepLength = parameters.getMaxStepLength(); double maxStepWidth = parameters.getMaxStepWidth(); double minStepWidth = parameters.getMinStepWidth(); @@ -314,32 +326,51 @@ else if (statusToProcess == FootstepStatus.COMPLETED) int startingIndexToAdjust = footsteps.size(); + quicksterFootstepProvider.get().update(time); + for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++) { - double xDisplacement = MathTools.clamp(stepTime.getValue() * desiredVelocityX, maxStepLength); - double yDisplacement = stepTime.getValue() * desiredVelocityY + swingSide.negateIfRightSide(defaultStepWidth); - double headingDisplacement = stepTime.getValue() * turningVelocity; - if (swingSide == RobotSide.LEFT) + if (csgMode.getEnumValue() == CSGMode.QFP && quicksterFootstepProvider.hasValue()) { - yDisplacement = MathTools.clamp(yDisplacement, minStepWidth, maxStepWidth); - headingDisplacement = MathTools.clamp(headingDisplacement, turnMaxAngleInward, turnMaxAngleOutward); + if (i == startingIndexToAdjust) + { + quicksterFootstepProvider.get().getDesiredTouchdownPosition2D(swingSide, nextFootstepPose2D.getPosition()); + } + else + { + calculateNextFootstepPose2D(stepTime.getValue(), + desiredVelocityX, + desiredVelocityY, + desiredTurningVelocity.getDoubleValue(), + swingSide, + maxStepLength, + maxStepWidth, + defaultStepWidth, + minStepWidth, + turnMaxAngleInward, + turnMaxAngleOutward, + footstepPose2D, + nextFootstepPose2D); + } } else { - yDisplacement = MathTools.clamp(yDisplacement, -maxStepWidth, -minStepWidth); - headingDisplacement = MathTools.clamp(headingDisplacement, -turnMaxAngleOutward, -turnMaxAngleInward); + calculateNextFootstepPose2D(stepTime.getValue(), + desiredVelocityX, + desiredVelocityY, + desiredTurningVelocity.getDoubleValue(), + swingSide, + maxStepLength, + maxStepWidth, + defaultStepWidth, + minStepWidth, + turnMaxAngleInward, + turnMaxAngleOutward, + footstepPose2D, + nextFootstepPose2D); } - double halfInPlaceWidth = 0.5 * swingSide.negateIfRightSide(defaultStepWidth); - nextFootstepPose2D.set(footstepPose2D); - // Applying the translation before the rotation allows the rotation to be centered in between the feet. - // This ordering seems to provide the most natural footsteps. - nextFootstepPose2D.appendTranslation(0.0, halfInPlaceWidth); - nextFootstepPose2D.appendRotation(headingDisplacement); - nextFootstepPose2D.appendTranslation(0.0, -halfInPlaceWidth); - nextFootstepPose2D.appendTranslation(xDisplacement, yDisplacement); - nextFootstepPose3D.set(nextFootstepPose2D); FootstepDataMessage footstep = footsteps.add(); @@ -450,6 +481,36 @@ else if (statusToProcess == FootstepStatus.COMPLETED) stepGeneratorTimer.stopMeasurement(); } + 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, + FramePose2D stanceFootPose2D, FramePose2D nextFootstepPose2DToPack) + { + double xDisplacement = MathTools.clamp(stepTime * desiredVelocityX, maxStepLength); + double yDisplacement = stepTime * desiredVelocityY + swingSide.negateIfRightSide(defaultStepWidth); + double headingDisplacement = stepTime * desiredTurningVelocity; + + if (swingSide == RobotSide.LEFT) + { + yDisplacement = MathTools.clamp(yDisplacement, minStepWidth, maxStepWidth); + headingDisplacement = MathTools.clamp(headingDisplacement, turnMaxAngleInward, turnMaxAngleOutward); + } + else + { + yDisplacement = MathTools.clamp(yDisplacement, -maxStepWidth, -minStepWidth); + headingDisplacement = MathTools.clamp(headingDisplacement, -turnMaxAngleOutward, -turnMaxAngleInward); + } + + double halfInPlaceWidth = 0.5 * swingSide.negateIfRightSide(defaultStepWidth); + nextFootstepPose2DToPack.set(stanceFootPose2D); + // Applying the translation before the rotation allows the rotation to be centered in between the feet. + // This ordering seems to provide the most natural footsteps. + nextFootstepPose2DToPack.appendTranslation(0.0, halfInPlaceWidth); + nextFootstepPose2DToPack.appendRotation(headingDisplacement); + nextFootstepPose2DToPack.appendTranslation(0.0, -halfInPlaceWidth); + nextFootstepPose2DToPack.appendTranslation(xDisplacement, yDisplacement); + } + /** * Sets the number of footsteps that are to be planned every tick. * From 92955ed80f49607b75cfc7ba2aebc2082f15e91e Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 5 Dec 2024 16:14:04 -0600 Subject: [PATCH 04/18] footsteps now continuously update throughout swing --- .../ContinuousStepGenerator.java | 16 ++- .../QuicksterFootstepProvider.java | 2 +- .../states/WalkingSingleSupportState.java | 106 ++++++++++++++---- .../WalkingMessageHandler.java | 18 +++ 4 files changed, 116 insertions(+), 26 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 8167e427aff..7f4f12c9daf 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,8 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder public enum CSGMode {STANDARD, QFP} private final YoEnum csgMode = new YoEnum<>("csgMode", registry, CSGMode.class); private final OptionalFactoryField quicksterFootstepProvider = new OptionalFactoryField<>("QuicksterFootstepProviderField"); + private final YoBoolean updateFootstepContinuouslyThroughoutSwing = new YoBoolean("updateFootstepContinuouslyThroughoutSwingCSG", registry); + private final boolean updateFootstepContinuouslyThroughoutSwingDefault = true; /** * Creates a new step generator, its {@code YoVariable}s will not be attached to any registry. @@ -184,7 +186,16 @@ public ContinuousStepGenerator(YoRegistry parentRegistry) parameters.clear(); numberOfTicksBeforeSubmittingFootsteps.set(0); - currentFootstepDataListCommandID.set(new Random().nextLong(0, Long.MAX_VALUE / 2)); // To make this command ID unique + currentFootstepDataListCommandID.set(new Random().nextLong(0, Long.MAX_VALUE / 2)); // To make this command ID unique + + updateFootstepContinuouslyThroughoutSwing.set(updateFootstepContinuouslyThroughoutSwingDefault); + csgMode.addListener(change -> + { + if (csgMode.getEnumValue() == CSGMode.QFP) + updateFootstepContinuouslyThroughoutSwing.set(true); + else + updateFootstepContinuouslyThroughoutSwing.set(updateFootstepContinuouslyThroughoutSwingDefault); + }); setSupportFootBasedFootstepAdjustment(true); } @@ -258,6 +269,9 @@ else if (statusToProcess == FootstepStatus.COMPLETED) // Determine swing side RobotSide swingSide; + if (updateFootstepContinuouslyThroughoutSwing.getBooleanValue()) + footsteps.clear(); + if (footsteps.isEmpty()) { footstepPose2D.set(currentSupportFootPose); 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 ea94fc34eb0..c87f488c686 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 @@ -122,7 +122,7 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano ReferenceFrame.getWorldFrame(), registry)); - AppearanceDefinition pendulumBaseVizColor = robotSide == RobotSide.LEFT ? YoAppearance.DodgerBlue() : YoAppearance.Orange(); + AppearanceDefinition pendulumBaseVizColor = robotSide == RobotSide.LEFT ? YoAppearance.Magenta() : YoAppearance.Green(); pendulumBaseViz.put(robotSide, new YoGraphicPosition(robotSide.getLowerCaseName() + "PendulumBase" + variableNameSuffix, pendulumBase3DInWorld.get(robotSide), 0.015, 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 2ff6888b387..23c92ed7b79 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 @@ -81,6 +81,8 @@ public class WalkingSingleSupportState extends SingleSupportState private final DoubleProvider swingFootCoPWeight; private final CenterOfPressureCommand copCommand = new CenterOfPressureCommand(); + private final WalkingMessageHandler.Listener listener = this::handleNewFootstep; + public WalkingSingleSupportState(WalkingStateEnum stateEnum, WalkingMessageHandler walkingMessageHandler, TouchdownErrorCompensator touchdownErrorCompensator, @@ -132,6 +134,8 @@ public WalkingSingleSupportState(WalkingStateEnum stateEnum, copCommand.setContactingRigidBody(contactableSwingFoot.getRigidBody()); copCommand.getDesiredCoP().setToZero(contactableSwingFoot.getSoleFrame()); swingFootCoPWeight = ParameterProvider.getOrCreateParameter(parentRegistry.getName(), getClass().getSimpleName(), "swingFootCoPWeight", registry, Double.NaN); + + walkingMessageHandler.addFoostepConsumptionListener(listener); } int stepsToAdd; @@ -188,7 +192,7 @@ else if (resubmitStepsInSwingEveryTick.getBooleanValue()) if (!hasSwingFootTouchedDown.getValue()) balanceManager.setSwingFootTrajectory(swingSide, feetManager.getSwingTrajectory(swingSide)); - balanceManager.computeICPPlan(isFootInContact); + balanceManager.computeICPPlan(); updateWalkingTrajectoryPath(); // call this here, too, so that the time in state is updated properly for all the swing speed up stuff, so it doesn't get out of sequence. This is @@ -273,36 +277,49 @@ public boolean isDone(double timeInState) return finishWhenICPPlannerIsDone.getValue() && balanceManager.isICPPlanDone(); } - @Override - public void onEntry() + private void handleNewFootstep() { - super.onEntry(); + handleNewFootstep(false); + } - hasSwingFootTouchedDown.set(false); - hasTriggeredTouchdown.set(false); + private void handleNewFootstep(boolean firstTick) + { + if (!haveWeEntered) + return; - double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); + if (walkingMessageHandler.hasUpcomingFootsteps()) + { + walkingMessageHandler.peekFootstep(0, nextFootstep); + if(nextFootstep.getRobotSide() != swingSide) + return; + } + else + { + return; + } + // + double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); swingTime = walkingMessageHandler.getNextSwingTime(); walkingMessageHandler.poll(nextFootstep, footstepTiming); if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) walkingMessageHandler.peekFootstep(0, nextNextFootstep); + // desiredFootPoseInWorld.set(nextFootstep.getFootstepPose()); desiredFootPoseInWorld.changeFrame(worldFrame); - /** - * 1/08/2018 RJG this has to be done before calling #updateFootstepParameters() to make sure the - * contact points are up to date - */ - feetManager.setContactStateForSwing(swingSide); - + // updateFootstepParameters(); + // balanceManager.minimizeAngularMomentumRateZ(minimizeAngularMomentumRateZDuringSwing.getValue()); balanceManager.setFinalTransferTime(finalTransferTime); + balanceManager.clearICPPlan(); + balanceManager.clearSwingFootTrajectory(); balanceManager.addFootstepToPlan(nextFootstep, footstepTiming); + // int stepsToAdd = Math.min(additionalFootstepsToConsider, walkingMessageHandler.getCurrentNumberOfFootsteps()); for (int i = 0; i < stepsToAdd; i++) { @@ -311,28 +328,67 @@ public void onEntry() balanceManager.addFootstepToPlan(footsteps[i], footstepTimings[i]); } - balanceManager.setICPPlanSupportSide(supportSide); - balanceManager.initializeICPPlanForSingleSupport(); - /** This has to be called after calling initialize ICP Plan, as that resets the step constraints **/ - walkingMessageHandler.pollStepConstraints(stepConstraints); - balanceManager.setCurrentStepConstraints(stepConstraints); + // updateHeightManager(); - feetManager.requestSwing(swingSide, - nextFootstep, - swingTime, - balanceManager.getFinalDesiredCoMVelocity(), - balanceManager.getFinalDesiredCoMAcceleration()); + // + if (firstTick) + { + feetManager.requestSwing(swingSide, + nextFootstep, + swingTime, + balanceManager.getFinalDesiredCoMVelocity(), + balanceManager.getFinalDesiredCoMAcceleration()); + + } + else + feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); + + // if (feetManager.adjustHeightIfNeeded(nextFootstep)) { walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); } + // + controllerToolbox.updateBipedSupportPolygons(); + balanceManager.setSwingFootTrajectory(swingSide, feetManager.getSwingTrajectory(swingSide)); + balanceManager.adjustFootstepInCoPPlan(nextFootstep); + balanceManager.computeICPPlan(); + } + + private boolean haveWeEntered = false; + + @Override + public void onEntry() + { + super.onEntry(); + + haveWeEntered = true; + + hasSwingFootTouchedDown.set(false); + hasTriggeredTouchdown.set(false); + + /** + * 1/08/2018 RJG this has to be done before calling #updateFootstepParameters() to make sure the + * contact points are up to date + */ + feetManager.setContactStateForSwing(swingSide); + balanceManager.setICPPlanSupportSide(supportSide); + + handleNewFootstep(true); + + // + balanceManager.initializeICPPlanForSingleSupport(); + + /** This has to be called after calling initialize ICP Plan, as that resets the step constraints **/ + walkingMessageHandler.pollStepConstraints(stepConstraints); + balanceManager.setCurrentStepConstraints(stepConstraints); pelvisOrientationManager.initializeSwing(); @@ -346,6 +402,8 @@ public void onExit(double timeInState) { super.onExit(timeInState); + haveWeEntered = false; + triggerTouchdown(); } @@ -479,7 +537,7 @@ public void handleChangeInContactState() return; controllerToolbox.updateBipedSupportPolygons(); - balanceManager.computeICPPlan(isFootInContact); + balanceManager.computeICPPlan(); } @Override 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 63521e71e4f..14c3dd1b5ee 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 @@ -1,5 +1,6 @@ package us.ihmc.commonWalkingControlModules.messageHandlers; +import java.util.ArrayList; import java.util.List; import controller_msgs.msg.dds.*; @@ -14,6 +15,7 @@ import us.ihmc.communication.packets.ExecutionMode; import us.ihmc.communication.packets.ExecutionTiming; import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; +import us.ihmc.euclid.interfaces.Settable; import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.FrameVector3D; @@ -120,6 +122,8 @@ public class WalkingMessageHandler implements SCS2YoGraphicHolder private final DoubleProvider maxStepHeightChange = new DoubleParameter("MaxStepHeightChange", registry, Double.POSITIVE_INFINITY); private final DoubleProvider maxSwingDistance = new DoubleParameter("MaxSwingDistance", registry, Double.POSITIVE_INFINITY); + private final List> footstepConsumptionListenerList = new ArrayList<>(); + public WalkingMessageHandler(double defaultTransferTime, double defaultSwingTime, double defaultInitialTransferTime, @@ -313,6 +317,10 @@ else if (upcomingFootsteps.isEmpty()) checkForPause(); + if (hasUpcomingFootsteps()) + for (int i = 0; i < footstepConsumptionListenerList.size(); i++) + footstepConsumptionListenerList.get(i).doListenerAction(); + updateVisualization(); } @@ -434,6 +442,16 @@ private static void packQueuedFootstepStatus(QueuedFootstepStatusMessage messasg // StepConstraintRegionsList.getAsMessage(stepConstraints, messasgeToPack.getStepConstraints()); } + public void addFoostepConsumptionListener(Listener listener) + { + footstepConsumptionListenerList.add(listener); + } + + public interface Listener> + { + void doListenerAction(); + } + public MomentumTrajectoryHandler getMomentumTrajectoryHandler() { return momentumTrajectoryHandler; From 33793fcf6d64ec45a18823334ccbad0f26391bba Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 5 Dec 2024 17:30:02 -0600 Subject: [PATCH 05/18] fixed stageable yovariable params. Did some tuning of double support fraction and pole --- .../QuicksterFootstepProvider.java | 2 ++ .../QuicksterFootstepProviderParameters.java | 10 ++++++++-- 2 files changed, 10 insertions(+), 2 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 c87f488c686..edfcb33574e 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 @@ -493,6 +493,8 @@ public void onEntry() netPendulumBase, inDoubleSupport.getBooleanValue(), desiredTouchdownPositions.get(robotSide)); + + parameters.setParametersForUpcomingSwing(robotSide); } @Override 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 04f254a8d8d..b608c5ae17d 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 @@ -12,11 +12,11 @@ public class QuicksterFootstepProviderParameters { // Static variables private static final double SWING_DURATION = 0.55; - private static final double DOUBLE_SUPPORT_FRACTION = 0.1; + private static final double DOUBLE_SUPPORT_FRACTION = 0.05; private static final double STANCE_WIDTH = 0.2; private static final double SWING_HEIGHT = 0.07; private static final double COM_HEIGHT = 0.9; - private static final double POLE = 0.275; + private static final double POLE = 0.0; // YoVariables private final YoDouble swingDuration; @@ -87,6 +87,12 @@ private StageableYoDouble createStageableYoDouble(RobotSide robotSide, String pr return stageableYoDouble; } + public void setParametersForUpcomingSwing(RobotSide swingSide) + { + for (int i = 0; i < stageableYoDoubles.get(swingSide).size(); i++) + stageableYoDoubles.get(swingSide).get(i).loadFromStage(); + } + public void setParametersForUpcomingSwing(RobotSide swingSide, double swingHeight, double swingDuration, double doubleSupportFraction) { for (int i = 0; i < stageableYoDoubles.get(swingSide).size(); i++) From 83413225d086661eda974046d664c548b6f3966f Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 6 Dec 2024 11:27:40 -0600 Subject: [PATCH 06/18] added ICP param to disable feedback CoP control --- .../capturePoint/controller/ICPController.java | 6 +++++- .../capturePoint/controller/ICPControllerParameters.java | 8 ++++++++ 2 files changed, 13 insertions(+), 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 577fda9b4ad..3fba644d8f6 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 @@ -54,6 +54,7 @@ public class ICPController implements ICPControllerInterface private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); private final BooleanProvider useCMPFeedback; + private final BooleanProvider useCoPFeedback; private final BooleanProvider useAngularMomentum; private final BooleanProvider scaleFeedbackWeightWithGain; @@ -160,6 +161,7 @@ public ICPController(WalkingControllerParameters walkingControllerParameters, this.controlDTSquare = controlDT * controlDT; useCMPFeedback = new BooleanParameter(yoNamePrefix + "UseCMPFeedback", registry, icpOptimizationParameters.useCMPFeedback()); + useCoPFeedback = new BooleanParameter(yoNamePrefix + "UseCoPFeedback", registry, icpOptimizationParameters.useCoPFeedback()); useAngularMomentum = new BooleanParameter(yoNamePrefix + "UseAngularMomentum", registry, icpOptimizationParameters.useAngularMomentum()); scaleFeedbackWeightWithGain = new BooleanParameter(yoNamePrefix + "ScaleFeedbackWeightWithGain", @@ -449,7 +451,9 @@ private void extractSolutionsFromSolver(boolean converged) private void computeFeedForwardAndFeedBackAlphas() { - if (parameters.getFeedbackAlphaCalculator() != null) + if (!useCoPFeedback.getValue()) + feedbackAlpha.set(1.0); + else if (parameters.getFeedbackAlphaCalculator() != null) feedbackAlpha.set(parameters.getFeedbackAlphaCalculator().computeAlpha(currentICP, copConstraintHandler.getCoPConstraint())); else feedbackAlpha.set(0.0); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerParameters.java index 4fcd7aa7c07..798d3513405 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerParameters.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerParameters.java @@ -96,6 +96,14 @@ public boolean useCMPFeedback() return true; } + /** + * Enabling this boolean allows feedback control of CoP (feedback alpha < 1). + */ + public boolean useCoPFeedback() + { + return true; + } + /** * Enabling this boolean allows the CMP to exit the support polygon. The CoP will still be * constrained to lie inside the support polygon, however. From 2b83ce21df9e6048bbd38264d778c06903ab1593 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Mon, 9 Dec 2024 11:32:33 -0600 Subject: [PATCH 07/18] fixed turn walking bug by setting QFP footstep position and orientation --- .../ContinuousStepGenerator.java | 2 +- .../QuicksterFootstepProvider.java | 63 ++++++++++++------- .../QuicksterFootstepProviderEstimates.java | 14 ++++- 3 files changed, 55 insertions(+), 24 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 7f4f12c9daf..adb837befb8 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 @@ -349,7 +349,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) { if (i == startingIndexToAdjust) { - quicksterFootstepProvider.get().getDesiredTouchdownPosition2D(swingSide, nextFootstepPose2D.getPosition()); + quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D); } else { 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 edfcb33574e..e342605e539 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 @@ -6,6 +6,8 @@ import us.ihmc.commons.MathTools; 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.interfaces.*; import us.ihmc.euclid.tuple2D.Vector2D; @@ -40,6 +42,7 @@ public class QuicksterFootstepProvider implements Updatable private final String variableNameSuffix = "QFP"; + private final FullHumanoidRobotModel robotModel; private final CommonHumanoidReferenceFrames referenceFrames; private final double updateDT; @@ -52,6 +55,7 @@ public class QuicksterFootstepProvider implements Updatable // Command/Desired output related stuff private final SideDependentList touchdownCalculator = new SideDependentList<>(); private final SideDependentList desiredTouchdownPositions = new SideDependentList<>(); + private final SideDependentList desiredTouchdownPoses = new SideDependentList<>(); // Desired inputs private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); @@ -60,6 +64,7 @@ public class QuicksterFootstepProvider implements Updatable private final YoBoolean walk = new YoBoolean("walk" + variableNameSuffix, registry); private final YoBoolean walkPreviousValue = new YoBoolean("walkPreviousValue" + variableNameSuffix, registry); private final YoFrameQuaternion desiredPelvisOrientation; + private final FrameQuaternion chestOrientation = new FrameQuaternion(); // Foot state information public enum FootState {SUPPORT, SWING} @@ -69,7 +74,7 @@ public enum FootState {SUPPORT, SWING} private final YoBoolean inDoubleSupport = new YoBoolean("inDoubleSupport" + variableNameSuffix, registry); private RobotSide trailingSide = RobotSide.LEFT; - // + // Pendulum base information private final SideDependentList pendulumBase = new SideDependentList<>(); private final FramePoint2D netPendulumBase = new FramePoint2D(); private final SideDependentList pendulumBase3DInWorld = new SideDependentList<>(); @@ -77,6 +82,11 @@ public enum FootState {SUPPORT, SWING} private final SideDependentList pendulumBaseViz = new SideDependentList<>(); private final YoGraphicPosition netPendulumBaseViz; + // Temp variables for changing frames and stuff + private final FramePoint2DBasics tempPendulumBase = new FramePoint2D(); + private final FramePoint2DBasics tempNetPendulumBase = new FramePoint2D(); + private final FramePoint2D tempTouchdownPosition = new FramePoint2D(); + // Inputs private final static Vector2DReadOnly zero2D = new Vector2D(); private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; @@ -84,8 +94,9 @@ public enum FootState {SUPPORT, SWING} public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) { - this.updateDT = updateDT; + this.robotModel = robotModel; this.referenceFrames = referenceFrames; + this.updateDT = updateDT; desiredPelvisOrientation = new YoFrameQuaternion("pelvisDesiredOrientation" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); @@ -143,6 +154,7 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano registry)); desiredTouchdownPositions.put(robotSide, new FramePoint2D()); + desiredTouchdownPoses.put(robotSide, new FramePose2D()); StateMachineFactory stateMachineFactory = new StateMachineFactory<>(FootState.class); stateMachineFactory.setRegistry(registry).setNamePrefix(robotSide.getLowerCaseName() + variableNameSuffix).buildYoClock(yoTime); @@ -160,9 +172,25 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano parentRegistry.addChild(registry); } + private boolean firstTick = true; + + private void initialize() + { + chestOrientation.setToZero(robotModel.getChest().getBodyFixedFrame()); + chestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); + + desiredPelvisOrientation.setToZero(); + desiredPelvisOrientation.setToYawOrientation(chestOrientation.getYaw()); + + firstTick = false; + } + @Override public void update(double time) { + if (firstTick) + initialize(); + updateEstimates(); updateDesireds(); @@ -170,8 +198,8 @@ public void update(double time) private void updateEstimates() { - // TODO maybe wrong? - desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); + // TODO should we set to chest yaw each tick? + // desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); estimates.update(); @@ -190,29 +218,14 @@ private void updateDesireds() { footStateMachines.get(robotSide).doActionAndTransition(); pendulumBase3DInWorld.get(robotSide).setMatchingFrame(pendulumBase.get(robotSide), 0.0); + + desiredTouchdownPoses.get(robotSide).getPosition().set(desiredTouchdownPositions.get(robotSide)); + desiredTouchdownPoses.get(robotSide).getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); } netPendulumBase3DInWorld.setMatchingFrame(netPendulumBase, 0.0); } - public void calculateTouchdownPosition(RobotSide swingSide, - double timeToReachGoal, - FramePose2DReadOnly pendulumBase, - FramePose2DReadOnly netPendulumBase, - FramePose2DBasics touchdownPositionToPack) - { - calculateTouchdownPosition(touchdownCalculator.get(swingSide), - swingSide, timeToReachGoal, - pendulumBase.getPosition(), - netPendulumBase.getPosition(), - inDoubleSupport.getBooleanValue(), - touchdownPositionToPack.getPosition()); - } - - private final FramePoint2DBasics tempPendulumBase = new FramePoint2D(); - private final FramePoint2DBasics tempNetPendulumBase = new FramePoint2D(); - private final FramePoint2D tempTouchdownPosition = new FramePoint2D(); - public void calculateTouchdownPosition(QuicksterFootstepProviderTouchdownCalculator touchdownCalculator, RobotSide swingSide, double timeToReachGoal, @@ -395,6 +408,12 @@ public FramePoint2DReadOnly getDesiredTouchdownPosition2D(RobotSide robotSide) return desiredTouchdownPositions.get(robotSide); } + public void getDesiredTouchdownPose(RobotSide robotSide, FramePose2D touchdownPoseToPack) + { + // TODO 3D transform between two 2D poses? + touchdownPoseToPack.setMatchingFrame(desiredTouchdownPoses.get(robotSide)); + } + 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 62b429c985c..b4bb1e19a29 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 @@ -5,6 +5,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicReferenceFrame; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.mecano.spatial.Twist; @@ -28,11 +29,18 @@ public class QuicksterFootstepProviderEstimates private final MovingReferenceFrame centerOfMassFrame; private final MovingReferenceFrame centerOfMassControlFrame; private final MovingZUpFrame centerOfMassControlZUpFrame; + private final YoGraphicReferenceFrame centerOfMassControlZUpFrameGraphic; // Calculator for CoM momentum info private final AngularExcursionCalculator angularExcursionCalculator; - public QuicksterFootstepProviderEstimates(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, FrameQuaternionReadOnly desiredPelvisOrientation, double updateDT, String variableNameSuffix, YoRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) + public QuicksterFootstepProviderEstimates(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, + FrameQuaternionReadOnly desiredPelvisOrientation, + double updateDT, + String variableNameSuffix, + YoRegistry registry, + YoGraphicsListRegistry yoGraphicsListRegistry) { currentCoMPosition = new YoFramePoint3D("currentCoMPosition" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); currentCoMVelocity = new YoFrameVector3D("currentCoMVelocity" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); @@ -63,6 +71,9 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) centerOfMassControlZUpFrame = new MovingZUpFrame(centerOfMassControlFrame, "centerOfMassControlZUpFrame" + variableNameSuffix); + centerOfMassControlZUpFrameGraphic = new YoGraphicReferenceFrame(centerOfMassControlZUpFrame, registry, false, 2.0); + yoGraphicsListRegistry.registerYoGraphic("QFP", centerOfMassControlZUpFrameGraphic); + angularExcursionCalculator = new AngularExcursionCalculator(centerOfMassFrame, robotModel.getElevator(), updateDT, registry, null); } @@ -80,6 +91,7 @@ public void update() // Update CoM control frames centerOfMassControlFrame.update(); centerOfMassControlZUpFrame.update(); + centerOfMassControlZUpFrameGraphic.update(); } public FramePoint3DReadOnly getCenterOfMassPosition() From e89e446851f1e2ac95a960bbe98cb91736caa815 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Mon, 9 Dec 2024 15:17:34 -0600 Subject: [PATCH 08/18] added support for joystick control in QFP Sim --- .../ContinuousStepGenerator.java | 4 ++- .../QuicksterFootstepProvider.java | 5 ++++ .../QuicksterFootstepProviderParameters.java | 26 +++++++++++++++++-- .../JoystickBasedSteppingPluginFactory.java | 2 +- 4 files changed, 33 insertions(+), 4 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 adb837befb8..bb5bebad2ef 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 @@ -398,8 +398,10 @@ else if (statusToProcess == FootstepStatus.COMPLETED) // } footstep.setRobotSide(swingSide.toByte()); - if (swingHeightInputProvider == null) + if (swingHeightInputProvider == null && csgMode.getEnumValue() == CSGMode.STANDARD) footstep.setSwingHeight(parameters.getSwingHeight()); + else if (swingHeightInputProvider == null && csgMode.getEnumValue() == CSGMode.QFP) + footstep.setSwingHeight(quicksterFootstepProvider.get().getSwingHeight(swingSide)); else footstep.setSwingHeight(swingHeightInputProvider.getValue()); 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 e342605e539..f4e6a7cdc59 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 @@ -429,6 +429,11 @@ public double getStepDuration(RobotSide swingSide) return getSwingDuration(swingSide) + getTransferDuration(swingSide); } + public double getSwingHeight(RobotSide swingSide) + { + return parameters.getSwingHeight(swingSide).getDoubleValue(); + } + public YoRegistry getRegistry() { return registry; 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 b608c5ae17d..898bbf7709c 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 @@ -14,9 +14,11 @@ public class QuicksterFootstepProviderParameters private static final double SWING_DURATION = 0.55; private static final double DOUBLE_SUPPORT_FRACTION = 0.05; private static final double STANCE_WIDTH = 0.2; - private static final double SWING_HEIGHT = 0.07; + private static final double SWING_HEIGHT = 0.09; private static final double COM_HEIGHT = 0.9; private static final double POLE = 0.0; + private static final double MAX_ACCELERATION_PER_STEP = 0.075; + private static final double MAX_DECELERATION_PER_STEP = -0.2; // YoVariables private final YoDouble swingDuration; @@ -25,6 +27,8 @@ public class QuicksterFootstepProviderParameters private final YoDouble swingHeight; private final YoDouble comHeight; private final YoDouble pole; + private final YoDouble maxAccelerationPerStep; + private final YoDouble maxDecelerationPerStep; private final YoDouble omega; // Stageable YoVariables @@ -34,6 +38,8 @@ public class QuicksterFootstepProviderParameters private final SideDependentList swingHeightCurrentStep = new SideDependentList<>(); private final SideDependentList comHeightCurrentStep = new SideDependentList<>(); private final SideDependentList poleCurrentStep = new SideDependentList<>(); + private final SideDependentList maxAccelerationPerStepCurrentStep = new SideDependentList<>(); + private final SideDependentList maxDecelerationPerStepCurrentStep = new SideDependentList<>(); private final SideDependentList omegaCurrentStep = new SideDependentList<>(); private final SideDependentList> stageableYoDoubles = new SideDependentList<>(); @@ -49,6 +55,8 @@ public QuicksterFootstepProviderParameters(double gravityZ, YoRegistry parentReg swingHeight = new YoDouble("desiredSwingHeight" + suffix2, registry); comHeight = new YoDouble("desiredComHeight" + suffix2, registry); pole = new YoDouble("pole" + suffix2, registry); + maxAccelerationPerStep = new YoDouble("maxAccelerationPerStep" + suffix2, registry); + maxDecelerationPerStep = new YoDouble("maxDecelerationPerStep" + suffix2, registry); omega = new YoDouble("omega" + suffix2, registry); swingDuration.set(SWING_DURATION); @@ -57,6 +65,8 @@ public QuicksterFootstepProviderParameters(double gravityZ, YoRegistry parentReg swingHeight.set(SWING_HEIGHT); comHeight.set(COM_HEIGHT); pole.set(POLE); + maxAccelerationPerStep.set(MAX_ACCELERATION_PER_STEP); + maxDecelerationPerStep.set(MAX_DECELERATION_PER_STEP); omega.set(Math.sqrt(Math.abs(gravityZ / comHeight.getDoubleValue()))); for (RobotSide robotSide : RobotSide.values) @@ -70,6 +80,8 @@ public QuicksterFootstepProviderParameters(double gravityZ, YoRegistry parentReg swingHeightCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "desiredSwingHeight", suffix + suffix2, swingHeight, registry)); comHeightCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "desiredComHeight", suffix + suffix2, comHeight, registry)); poleCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "pole", suffix + suffix2, pole, registry)); + maxAccelerationPerStepCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "maxAccelerationPerStep", suffix + suffix2, maxAccelerationPerStep, registry)); + maxDecelerationPerStepCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "maxDecelerationPerStep", suffix + suffix2, maxDecelerationPerStep, registry)); omegaCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "omega", suffix + suffix2, omega, registry)); } @@ -121,7 +133,7 @@ public YoDouble getStanceWidth(RobotSide robotSide) return stanceWidthCurrentStep.get(robotSide); } - public YoDouble getDesiredSwingHeight(RobotSide robotSide) + public YoDouble getSwingHeight(RobotSide robotSide) { return swingHeightCurrentStep.get(robotSide); } @@ -136,6 +148,16 @@ public YoDouble getPole(RobotSide robotSide) return poleCurrentStep.get(robotSide); } + public YoDouble getMaxAccelerationPerStepCurrentStep(RobotSide robotSide) + { + return maxAccelerationPerStepCurrentStep.get(robotSide); + } + + public YoDouble getMaxDecelerationPerStepCurrentStep(RobotSide robotSide) + { + return maxDecelerationPerStepCurrentStep.get(robotSide); + } + public YoDouble getOmega(RobotSide robotSide) { return omegaCurrentStep.get(robotSide); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java index aca34efea39..c19314b507f 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java @@ -118,7 +118,7 @@ public JoystickBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel csgFootstepGenerator.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); csgFootstepGenerator.setDesiredTurningVelocityProvider(commandInputManager.createDesiredTurningVelocityProvider()); csgFootstepGenerator.setWalkInputProvider(commandInputManager.createWalkInputProvider()); - csgFootstepGenerator.setSwingHeightInputProvider(commandInputManager.createSwingHeightProvider()); +// csgFootstepGenerator.setSwingHeightInputProvider(commandInputManager.createSwingHeightProvider()); fastWalkingPlugin.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); From a09a8aeeff3585578e8455dda356b6483d0b75e4 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 10 Dec 2024 14:01:49 -0600 Subject: [PATCH 09/18] fixed csg wrong support side bug --- .../ContinuousStepGenerator.java | 19 +++++++++---------- .../states/WalkingSingleSupportState.java | 10 +--------- 2 files changed, 10 insertions(+), 19 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 bb5bebad2ef..bf5fc6b34ea 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 @@ -157,7 +157,6 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder private final SideDependentList> footstepSideDependentVisualizers = new SideDependentList<>(new ArrayList<>(), new ArrayList<>()); private final MutableObject latestStatusReceived = new MutableObject<>(null); - private final MutableObject footstepCompletionSide = new MutableObject<>(null); // All things QFP public enum CSGMode {STANDARD, QFP} @@ -248,6 +247,8 @@ else if (startWalkingMessenger != null && walk.getValue() != walkPreviousValue.g if (statusToProcess != null) { + currentSupportFootPose.setMatchingFrame(footPoseProvider.getCurrentFootPose(currentSupportSide.getValue())); + if (statusToProcess == FootstepStatus.STARTED) { if (!footsteps.isEmpty()) @@ -255,15 +256,13 @@ else if (startWalkingMessenger != null && walk.getValue() != walkPreviousValue.g } else if (statusToProcess == FootstepStatus.COMPLETED) { - currentSupportSide.set(footstepCompletionSide.getValue()); - currentSupportFootPose.setMatchingFrame(footPoseProvider.getCurrentFootPose(currentSupportSide.getValue())); + if (parameters.getNumberOfFixedFootsteps() == 0) footsteps.clear(); } } latestStatusReceived.setValue(null); - footstepCompletionSide.setValue(null); } // Determine swing side @@ -665,7 +664,7 @@ public FramePose3DReadOnly getCurrentFootPose(RobotSide robotSide) public void notifyFootstepCompleted(RobotSide robotSide) { latestStatusReceived.setValue(FootstepStatus.COMPLETED); - footstepCompletionSide.setValue(robotSide); + currentSupportSide.set(robotSide); } /** @@ -675,16 +674,16 @@ public void notifyFootstepCompleted(RobotSide robotSide) * while the swing foot is targeting it. *

*/ - public void notifyFootstepStarted() + public void notifyFootstepStarted(RobotSide robotSide) { latestStatusReceived.setValue(FootstepStatus.STARTED); - footstepCompletionSide.setValue(null); + currentSupportSide.set(robotSide.getOppositeSide()); } /** * Attaches a listener for {@code FootstepStatusMessage} to the manager. *

- * This listener will automatically call {@link #notifyFootstepStarted()} and + * This listener will automatically call {@link #notifyFootstepStarted(RobotSide robotSide)} and * {@link #notifyFootstepCompleted(RobotSide)}. *

* @@ -699,7 +698,7 @@ public void setFootstepStatusListener(StatusMessageOutputManager statusMessageOu } /** - * Consumes a newly received message and calls {@link #notifyFootstepStarted()} or + * Consumes a newly received message and calls {@link #notifyFootstepStarted(RobotSide robotSide)} or * {@link #notifyFootstepCompleted(RobotSide)} according to the status. * * @param statusMessage the newly received footstep status. @@ -710,7 +709,7 @@ public void consumeFootstepStatus(FootstepStatusMessage statusMessage) if (status == FootstepStatus.COMPLETED) notifyFootstepCompleted(RobotSide.fromByte(statusMessage.getRobotSide())); else if (status == FootstepStatus.STARTED) - notifyFootstepStarted(); + notifyFootstepStarted(RobotSide.fromByte(statusMessage.getRobotSide())); } /** 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 23c92ed7b79..1688d8437d1 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 @@ -287,16 +287,8 @@ private void handleNewFootstep(boolean firstTick) if (!haveWeEntered) return; - if (walkingMessageHandler.hasUpcomingFootsteps()) - { - walkingMessageHandler.peekFootstep(0, nextFootstep); - if(nextFootstep.getRobotSide() != swingSide) - return; - } - else - { + if (!walkingMessageHandler.isNextFootstepFor(swingSide)) return; - } // double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); From 022c0e75134366e2cdbc6e7daeaa9f6140c45f74 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 12 Dec 2024 10:26:09 -0600 Subject: [PATCH 10/18] added boolean to not continously update touchdown location throughout swing --- .../states/WalkingSingleSupportState.java | 14 +++++++++++++- .../messageHandlers/WalkingMessageHandler.java | 7 ++++++- 2 files changed, 19 insertions(+), 2 deletions(-) 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 1688d8437d1..5f71a772136 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 @@ -81,6 +81,7 @@ public class WalkingSingleSupportState extends SingleSupportState private final DoubleProvider swingFootCoPWeight; private final CenterOfPressureCommand copCommand = new CenterOfPressureCommand(); + private final YoBoolean updateFootstepContinuouslyThroughoutSwing; private final WalkingMessageHandler.Listener listener = this::handleNewFootstep; public WalkingSingleSupportState(WalkingStateEnum stateEnum, @@ -135,7 +136,18 @@ public WalkingSingleSupportState(WalkingStateEnum stateEnum, copCommand.getDesiredCoP().setToZero(contactableSwingFoot.getSoleFrame()); swingFootCoPWeight = ParameterProvider.getOrCreateParameter(parentRegistry.getName(), getClass().getSimpleName(), "swingFootCoPWeight", registry, Double.NaN); - walkingMessageHandler.addFoostepConsumptionListener(listener); + updateFootstepContinuouslyThroughoutSwing = new YoBoolean("updateFootstepContinuouslyThroughoutSwing", registry); + + updateFootstepContinuouslyThroughoutSwing.addListener(value -> + { + if (updateFootstepContinuouslyThroughoutSwing.getBooleanValue()) + walkingMessageHandler.addFootstepConsumptionListener(listener); + else + walkingMessageHandler.removeFootstepConsumptionListener(listener); + + }); + + updateFootstepContinuouslyThroughoutSwing.set(true); } int stepsToAdd; 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 14c3dd1b5ee..01c5cbb93ac 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 @@ -442,11 +442,16 @@ private static void packQueuedFootstepStatus(QueuedFootstepStatusMessage messasg // StepConstraintRegionsList.getAsMessage(stepConstraints, messasgeToPack.getStepConstraints()); } - public void addFoostepConsumptionListener(Listener listener) + public void addFootstepConsumptionListener(Listener listener) { footstepConsumptionListenerList.add(listener); } + public void removeFootstepConsumptionListener(Listener listener) + { + footstepConsumptionListenerList.remove(listener); + } + public interface Listener> { void doListenerAction(); From 4b5cc4c668d1cdb5111a80cadcbb8c6b0013cc33 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 12 Dec 2024 12:26:59 -0600 Subject: [PATCH 11/18] yovariableized StepGeneratorCommandInputManager desired input variables --- .../avatar/AvatarStepGeneratorThread.java | 1 + .../ContinuousStepGenerator.java | 3 ++ .../QuicksterFootstepProvider.java | 16 +++++++- .../StepGeneratorCommandInputManager.java | 39 +++++++++++++------ 4 files changed, 45 insertions(+), 14 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java index 649b5b57e34..577e9559bc3 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java @@ -78,6 +78,7 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory, humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData(); csgCommandInputManager = pluginFactory.getStepGeneratorCommandInputManager(); + csgRegistry.addChild(csgCommandInputManager.getRegistry()); if (environmentalConstraints != 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 bf5fc6b34ea..93437801de5 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 @@ -904,6 +904,9 @@ public boolean adjustFootstep(FramePose3DReadOnly supportFootPose, public void setWalkInputProvider(BooleanProvider walkInputProvider) { this.walkInputProvider = walkInputProvider; + + if (quicksterFootstepProvider.hasValue()) + quicksterFootstepProvider.get().setWalkInputProvider(walkInputProvider); } /** 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 f4e6a7cdc59..4cfe4885641 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 @@ -28,6 +28,7 @@ import us.ihmc.yoVariables.euclid.YoVector2D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; @@ -60,9 +61,7 @@ public class QuicksterFootstepProvider implements Updatable // Desired inputs private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); private final YoVector2D desiredVelocity = new YoVector2D("desiredVelocity" + variableNameSuffix, registry); - private final YoBoolean ignoreWalkInputProvider = new YoBoolean("ignoreWalkInputProvider" + variableNameSuffix, registry); private final YoBoolean walk = new YoBoolean("walk" + variableNameSuffix, registry); - private final YoBoolean walkPreviousValue = new YoBoolean("walkPreviousValue" + variableNameSuffix, registry); private final YoFrameQuaternion desiredPelvisOrientation; private final FrameQuaternion chestOrientation = new FrameQuaternion(); @@ -89,6 +88,7 @@ public enum FootState {SUPPORT, SWING} // Inputs private final static Vector2DReadOnly zero2D = new Vector2D(); + private BooleanProvider walkInputProvider; private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; private DesiredTurningVelocityProvider desiredTurningVelocityProvider = () -> 0.0; @@ -298,6 +298,7 @@ private void handleDesiredsFromProviders() double desiredVelocityY = desiredVelocity.getY(); double turningVelocity = desiredTurningVelocityProvider.getTurningVelocity(); + this.walk.set(walkInputProvider.getValue()); this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); this.desiredTurningVelocity.set(turningVelocity); } @@ -379,6 +380,17 @@ public void setDesiredVelocityProvider(DesiredVelocityProvider desiredVelocityPr this.desiredVelocityProvider = desiredVelocityProvider; } + /** + * Sets a provider that is to be used to update the state of {@link #walk} internally on each call + * to {@link #update(double)}. + * + * @param walkInputProvider the provider used to determine whether to walk or not walk. + */ + public void setWalkInputProvider(BooleanProvider walkInputProvider) + { + this.walkInputProvider = walkInputProvider; + } + private void setTrailingSide(RobotSide robotSide) { trailingSide = robotSide; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java index 32e7652a206..aa7dd3adcc9 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java @@ -16,8 +16,12 @@ import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepStatus; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.yoVariables.euclid.YoVector2D; import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; import java.util.ArrayList; import java.util.List; @@ -28,14 +32,15 @@ public class StepGeneratorCommandInputManager implements Updatable { + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); private final CommandInputManager commandInputManager = new CommandInputManager(supportedCommands()); private boolean isOpen = false; - private boolean walk = false; + private final YoBoolean walk; + private final YoVector2D desiredVelocity; + private final YoDouble turningVelocity; + private final YoDouble swingHeight; private boolean isUnitVelocities = false; - private double swingHeight = 0.1; - private final Vector2D desiredVelocity = new Vector2D(); - private double turningVelocity = 0.0; private int ticksToUpdateTheEnvironment = Integer.MAX_VALUE; private HighLevelControllerName currentController; private ContinuousStepGenerator continuousStepGenerator; @@ -51,6 +56,11 @@ public class StepGeneratorCommandInputManager implements Updatable public StepGeneratorCommandInputManager() { + String suffix = "StepGeneratorCommandInputManager"; + walk = new YoBoolean("walk_" + suffix, registry); + desiredVelocity = new YoVector2D("desiredVelocity_" + suffix, registry); + turningVelocity = new YoDouble("desiredTurningVelocity_" + suffix, registry); + swingHeight = new YoDouble("desiredSwingHeight_" + suffix, registry); } public void setCSG(ContinuousStepGenerator continuousStepGenerator) @@ -125,9 +135,9 @@ public void update(double time) ContinuousStepGeneratorInputCommand command = commandInputManager.pollNewestCommand(ContinuousStepGeneratorInputCommand.class); desiredVelocity.setX(command.getForwardVelocity()); desiredVelocity.setY(command.getLateralVelocity()); - turningVelocity = command.getTurnVelocity(); + turningVelocity.set(command.getTurnVelocity()); isUnitVelocities = command.isUnitVelocities(); - walk = command.isWalk(); + walk.set(command.isWalk()); } commandInputManager.clearCommands(ContinuousStepGeneratorInputCommand.class); @@ -137,12 +147,12 @@ public void update(double time) ContinuousStepGeneratorParameters parameters = command.getParameters(); ticksToUpdateTheEnvironment = parameters.getTicksToUpdateTheEnvironment(); - swingHeight = parameters.getSwingHeight(); + swingHeight.set(parameters.getSwingHeight()); if (continuousStepGenerator != null) { continuousStepGenerator.setFootstepTiming(parameters.getSwingDuration(), parameters.getTransferDuration()); - continuousStepGenerator.setSwingHeight(swingHeight); + continuousStepGenerator.setSwingHeight(swingHeight.getDoubleValue()); continuousStepGenerator.setFootstepsAreAdjustable(parameters.getStepsAreAdjustable()); continuousStepGenerator.setStepWidths(parameters.getDefaultStepWidth(), parameters.getMinStepWidth(), parameters.getMaxStepWidth()); } @@ -188,7 +198,7 @@ public void update(double time) previousFootstepStatusReceived.set(latestFootstepStatusReceived.get()); if (!isOpen) - walk = false; + walk.set(false); } public boolean isOpen() @@ -221,7 +231,7 @@ public DesiredTurningVelocityProvider createDesiredTurningVelocityProvider() @Override public double getTurningVelocity() { - return turningVelocity; + return turningVelocity.getDoubleValue(); } @Override @@ -234,11 +244,16 @@ public boolean isUnitVelocity() public BooleanProvider createWalkInputProvider() { - return () -> walk; + return walk::getBooleanValue; } public DoubleProvider createSwingHeightProvider() { - return () -> swingHeight; + return swingHeight::getDoubleValue; + } + + public YoRegistry getRegistry() + { + return registry; } } From 3532006229c7487f805d9d64d300ca4765025d43 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 13 Dec 2024 10:28:10 -0600 Subject: [PATCH 12/18] fixed bug in which footsteps weren't polled at right time causing state machine to go to TO_STANDING --- .../footstepGenerator/ContinuousStepGenerator.java | 1 + .../messageHandlers/WalkingMessageHandler.java | 4 ++++ 2 files changed, 5 insertions(+) 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 93437801de5..ce71f111f80 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 @@ -665,6 +665,7 @@ public void notifyFootstepCompleted(RobotSide robotSide) { latestStatusReceived.setValue(FootstepStatus.COMPLETED); currentSupportSide.set(robotSide); +// update(0.0); } /** 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 01c5cbb93ac..0d6dc6532d0 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 @@ -283,6 +283,10 @@ else if (upcomingFootsteps.isEmpty()) boolean areFootstepsAdjustable = command.areFootstepsAdjustable(); boolean shouldCheckPlanForReachability = command.getShouldCheckForReachability(); + // TODO make this only for QFP mode once messages for that are made + if (footstepStatus.getRobotSide() == command.getFootstep(0).getRobotSide().toByte() && footstepStatus.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_COMPLETED) + command.removeFootstep(0) + ; for (int i = 0; i < command.getNumberOfFootsteps(); i++) { boolean shouldCheckStepForReachability = shouldCheckPlanForReachability || command.getFootstep(i).getShouldCheckForReachability(); From a846e1b1fb677504e8571f112bb4b197e3d5706f Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 13 Dec 2024 10:54:19 -0600 Subject: [PATCH 13/18] added visualizer of desired touchdown position from QFP --- .../QuicksterFootstepProvider.java | 19 +++++++++++++++++++ 1 file changed, 19 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 4cfe4885641..d95ba63624e 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 @@ -57,6 +57,8 @@ public class QuicksterFootstepProvider implements Updatable private final SideDependentList touchdownCalculator = new SideDependentList<>(); private final SideDependentList desiredTouchdownPositions = new SideDependentList<>(); private final SideDependentList desiredTouchdownPoses = new SideDependentList<>(); + private final SideDependentList desiredTouchdownPosition3DInWorld = new SideDependentList<>(); + private final SideDependentList desiredTouchdownPositionViz = new SideDependentList<>(); // Desired inputs private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); @@ -156,6 +158,20 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano desiredTouchdownPositions.put(robotSide, new FramePoint2D()); desiredTouchdownPoses.put(robotSide, new FramePose2D()); + desiredTouchdownPosition3DInWorld.put(robotSide, new YoFramePoint3D(robotSide.getLowerCaseName() + "DesiredTouchdownPosition3DInWorld" + variableNameSuffix, + ReferenceFrame.getWorldFrame(), + registry)); + + AppearanceDefinition touchdownVizColor = robotSide == RobotSide.LEFT ? YoAppearance.Magenta() : YoAppearance.Green(); + desiredTouchdownPositionViz.put(robotSide, new YoGraphicPosition(robotSide.getLowerCaseName() + "DesiredTouchdownPosition" + variableNameSuffix, + desiredTouchdownPosition3DInWorld.get(robotSide), + 0.015, + touchdownVizColor, + YoGraphicPosition.GraphicType.DIAMOND_WITH_CROSS)); + + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, desiredTouchdownPositionViz.get(robotSide)); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, desiredTouchdownPositionViz.get(robotSide).createArtifact()); + StateMachineFactory stateMachineFactory = new StateMachineFactory<>(FootState.class); stateMachineFactory.setRegistry(registry).setNamePrefix(robotSide.getLowerCaseName() + variableNameSuffix).buildYoClock(yoTime); @@ -217,8 +233,11 @@ private void updateDesireds() for (RobotSide robotSide : RobotSide.values) { footStateMachines.get(robotSide).doActionAndTransition(); + pendulumBase3DInWorld.get(robotSide).setMatchingFrame(pendulumBase.get(robotSide), 0.0); + desiredTouchdownPosition3DInWorld.get(robotSide).setMatchingFrame(desiredTouchdownPositions.get(robotSide), 0.0); + desiredTouchdownPoses.get(robotSide).getPosition().set(desiredTouchdownPositions.get(robotSide)); desiredTouchdownPoses.get(robotSide).getOrientation().setFromReferenceFrame(centerOfMassControlZUpFrame); } From 5a0ee77640f83265e8138e0aaa46ddf4558796dc Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Mon, 16 Dec 2024 16:28:21 -0600 Subject: [PATCH 14/18] switched to ALIP+DS model --- .../QuicksterFootstepProviderTouchdownCalculator.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java index 152b3ec0f6c..13680b6b99c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/quicksterFootstepProvider/QuicksterFootstepProviderTouchdownCalculator.java @@ -190,10 +190,10 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo this.timeToReachGoal.set(timeToReachGoal); // compute all the touchdown positions and their offset values - computeDesiredALIPTouchdownPosition(timeToReachGoal, pendulumBase); + computeDesiredALIPTouchdownPositionWithDS(timeToReachGoal, pendulumBase, netPendulumBase, isInDoubleSupport); - desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithoutDS); - predictedVelocityAtTouchdown2D.setIncludingFrame(predictedALIPVelocityAtTouchdownWithoutDS); + desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithDS); + predictedVelocityAtTouchdown2D.setIncludingFrame(predictedALIPVelocityAtTouchdownWithDS); predictedVelocityAtTouchdown2D.changeFrameAndProjectToXYPlane(predictedVelocityAtTouchdown3D.getReferenceFrame()); predictedVelocityAtTouchdown3D.set(predictedVelocityAtTouchdown2D); From c5bcb14d3555014b71c5c3e1ae81d7144b541a63 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 17 Dec 2024 13:59:15 -0600 Subject: [PATCH 15/18] disabled continouous touchdown position update by default --- .../footstepGenerator/ContinuousStepGenerator.java | 6 +++--- .../walkingController/states/WalkingSingleSupportState.java | 2 +- 2 files changed, 4 insertions(+), 4 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 ce71f111f80..5f20717747e 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,7 +163,7 @@ public enum CSGMode {STANDARD, QFP} private final YoEnum csgMode = new YoEnum<>("csgMode", registry, CSGMode.class); private final OptionalFactoryField quicksterFootstepProvider = new OptionalFactoryField<>("QuicksterFootstepProviderField"); private final YoBoolean updateFootstepContinuouslyThroughoutSwing = new YoBoolean("updateFootstepContinuouslyThroughoutSwingCSG", registry); - private final boolean updateFootstepContinuouslyThroughoutSwingDefault = true; + private final boolean updateFootstepContinuouslyThroughoutSwingDefault = false; /** * Creates a new step generator, its {@code YoVariable}s will not be attached to any registry. @@ -192,8 +192,8 @@ public ContinuousStepGenerator(YoRegistry parentRegistry) { if (csgMode.getEnumValue() == CSGMode.QFP) updateFootstepContinuouslyThroughoutSwing.set(true); - else - updateFootstepContinuouslyThroughoutSwing.set(updateFootstepContinuouslyThroughoutSwingDefault); +// else +// updateFootstepContinuouslyThroughoutSwing.set(updateFootstepContinuouslyThroughoutSwingDefault); }); setSupportFootBasedFootstepAdjustment(true); 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 5f71a772136..30fa0aebb1e 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 @@ -147,7 +147,7 @@ public WalkingSingleSupportState(WalkingStateEnum stateEnum, }); - updateFootstepContinuouslyThroughoutSwing.set(true); + updateFootstepContinuouslyThroughoutSwing.set(false); } int stepsToAdd; From c0e9d4e113aa83a540a13bbeee4d6c6db02ea296 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 18 Dec 2024 14:56:09 -0600 Subject: [PATCH 16/18] fixed control frame yaw discontinuity bug --- .../footstepGenerator/ContinuousStepGenerator.java | 14 +++++++++++++- .../QuicksterFootstepProvider.java | 6 ++---- .../QuicksterFootstepProviderEstimates.java | 2 +- 3 files changed, 16 insertions(+), 6 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 5f20717747e..2db9eb62248 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 @@ -191,7 +191,11 @@ public ContinuousStepGenerator(YoRegistry parentRegistry) csgMode.addListener(change -> { if (csgMode.getEnumValue() == CSGMode.QFP) + { updateFootstepContinuouslyThroughoutSwing.set(true); + if (quicksterFootstepProvider.hasValue()) + quicksterFootstepProvider.get().initialize(); + } // else // updateFootstepContinuouslyThroughoutSwing.set(updateFootstepContinuouslyThroughoutSwingDefault); }); @@ -339,7 +343,15 @@ else if (statusToProcess == FootstepStatus.COMPLETED) int startingIndexToAdjust = footsteps.size(); - quicksterFootstepProvider.get().update(time); + // 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 (csgMode.getEnumValue() == CSGMode.STANDARD) + quicksterFootstepProvider.get().initialize(); + + quicksterFootstepProvider.get().update(time); + } for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++) { 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 d95ba63624e..86fe9b09dc0 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 @@ -190,9 +190,9 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano private boolean firstTick = true; - private void initialize() + public void initialize() { - chestOrientation.setToZero(robotModel.getChest().getBodyFixedFrame()); + chestOrientation.setToZero(robotModel.getPelvis().getBodyFixedFrame()); chestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); desiredPelvisOrientation.setToZero(); @@ -214,8 +214,6 @@ public void update(double time) private void updateEstimates() { - // TODO should we set to chest yaw each tick? - // desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); estimates.update(); 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 b4bb1e19a29..f8d89f51389 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 @@ -71,7 +71,7 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) centerOfMassControlZUpFrame = new MovingZUpFrame(centerOfMassControlFrame, "centerOfMassControlZUpFrame" + variableNameSuffix); - centerOfMassControlZUpFrameGraphic = new YoGraphicReferenceFrame(centerOfMassControlZUpFrame, registry, false, 2.0); + centerOfMassControlZUpFrameGraphic = new YoGraphicReferenceFrame(centerOfMassControlZUpFrame, registry, false, 1.25); yoGraphicsListRegistry.registerYoGraphic("QFP", centerOfMassControlZUpFrameGraphic); angularExcursionCalculator = new AngularExcursionCalculator(centerOfMassFrame, robotModel.getElevator(), updateDT, registry, null); From b7dc5963f5452d49e632b08c16bfbeb442dff329 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 18 Dec 2024 17:01:19 -0600 Subject: [PATCH 17/18] some parameter tuning for hardware --- .../QuicksterFootstepProviderParameters.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 898bbf7709c..fef828adb1e 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 @@ -11,11 +11,11 @@ public class QuicksterFootstepProviderParameters { // Static variables - private static final double SWING_DURATION = 0.55; - private static final double DOUBLE_SUPPORT_FRACTION = 0.05; - private static final double STANCE_WIDTH = 0.2; + private static final double SWING_DURATION = 0.7;//0.55; + private static final double DOUBLE_SUPPORT_FRACTION = 0.1;//0.05; + private static final double STANCE_WIDTH = 0.275; private static final double SWING_HEIGHT = 0.09; - private static final double COM_HEIGHT = 0.9; + private static final double COM_HEIGHT = 0.95; private static final double POLE = 0.0; private static final double MAX_ACCELERATION_PER_STEP = 0.075; private static final double MAX_DECELERATION_PER_STEP = -0.2; From 5647e3c8c532102e986e72ba107ac4bfe4bf4931 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 8 Jan 2025 16:43:10 -0600 Subject: [PATCH 18/18] added acp viz --- .../QuicksterFootstepProvider.java | 6 ++-- .../QuicksterFootstepProviderEstimates.java | 28 ++++++++++++++++++- 2 files changed, 30 insertions(+), 4 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 86fe9b09dc0..a05f4f4ee3d 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 @@ -110,6 +110,7 @@ public QuicksterFootstepProvider(FullHumanoidRobotModel robotModel, CommonHumano estimates = new QuicksterFootstepProviderEstimates(robotModel, referenceFrames, desiredPelvisOrientation, + parameters, updateDT, variableNameSuffix, registry, @@ -216,12 +217,12 @@ private void updateEstimates() { desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); - estimates.update(); - inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); calculateNetPendulumBase(); + + estimates.update(getTrailingSide().getOppositeSide()); } private void updateDesireds() @@ -274,7 +275,6 @@ private void calculateNetPendulumBase() netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(RobotSide.LEFT)); setTrailingSide(RobotSide.LEFT); } - else { netPendulumBase.setIncludingFrame(pendulumBase.get(RobotSide.RIGHT)); 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 f8d89f51389..87e351982bb 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 @@ -5,11 +5,14 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.graphicsDescription.appearance.YoAppearance; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPosition; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicReferenceFrame; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.mecano.spatial.Twist; import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.screwTheory.AngularExcursionCalculator; import us.ihmc.robotics.screwTheory.MovingZUpFrame; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -34,14 +37,23 @@ public class QuicksterFootstepProviderEstimates // Calculator for CoM momentum info private final AngularExcursionCalculator angularExcursionCalculator; + // + private final double mass; + private final YoFramePoint3D acp; + private final QuicksterFootstepProviderParameters parameters; + public QuicksterFootstepProviderEstimates(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, FrameQuaternionReadOnly desiredPelvisOrientation, + QuicksterFootstepProviderParameters parameters, double updateDT, String variableNameSuffix, YoRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { + this.parameters = parameters; + mass = robotModel.getTotalMass(); + currentCoMPosition = new YoFramePoint3D("currentCoMPosition" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); currentCoMVelocity = new YoFrameVector3D("currentCoMVelocity" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); currentCoMLinearMomentum = new YoFrameVector3D("currentCoMLinearMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); @@ -75,9 +87,13 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) yoGraphicsListRegistry.registerYoGraphic("QFP", centerOfMassControlZUpFrameGraphic); angularExcursionCalculator = new AngularExcursionCalculator(centerOfMassFrame, robotModel.getElevator(), updateDT, registry, null); + + acp = new YoFramePoint3D("ACP_" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + YoGraphicPosition acpViz = new YoGraphicPosition("ACP", acp, 0.01, YoAppearance.Red(), YoGraphicPosition.GraphicType.BALL_WITH_ROTATED_CROSS); + yoGraphicsListRegistry.registerArtifact("QFP", acpViz.createArtifact()); } - public void update() + public void update(RobotSide swingSide) { // Update CoM position and velocity from CoM frame currentCoMPosition.setFromReferenceFrame(centerOfMassFrame); @@ -92,6 +108,16 @@ public void update() centerOfMassControlFrame.update(); centerOfMassControlZUpFrame.update(); centerOfMassControlZUpFrameGraphic.update(); + + // Update Measured ACP + double wmh = parameters.getOmega(swingSide).getDoubleValue() * mass * parameters.getDesiredCoMHeight(swingSide).getDoubleValue(); + + acp.setFromReferenceFrame(centerOfMassFrame); + acp.scaleAdd(1.0 / parameters.getOmega(swingSide).getDoubleValue(), + getCenterOfMassVelocity(), + getCenterOfMassPosition()); + acp.addX(1.0 / wmh * getCenterOfMassAngularMomentum().getY()); + acp.addY(-1.0 / wmh * getCenterOfMassAngularMomentum().getX()); } public FramePoint3DReadOnly getCenterOfMassPosition()