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 d7d56850d58b..649b5b57e340 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,12 +98,13 @@ 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, walkingCommandInputManager, - null, + csgGraphics, null, csgTime); csgRegistry.addChild(continuousStepGeneratorPlugin.getRegistry()); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java index 78714226871f..efce2b90e84f 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java @@ -312,6 +312,7 @@ private HumanoidKinematicsSimulation(DRCRobotModel robotModel, HumanoidKinematic controllerToolbox.getContactableFeet(), fullRobotModel.getElevator(), walkingControllerParameters, + controllerToolbox.getWalkingMessageHandler(), controllerToolbox.getTotalMassProvider(), GRAVITY_Z, controllerToolbox.getControlDT(), diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java index c074a75c41e6..fd77326b4914 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java @@ -183,6 +183,7 @@ public WalkingControllerPreviewToolboxController(DRCRobotModel robotModel, contactableFeet, elevator, walkingControllerParameters, + controllerToolbox.getWalkingMessageHandler(), controllerToolbox.getTotalMassProvider(), gravityZ, controlDT, 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 6c7f7dcb8c58..ab255e43f2b3 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/capturePoint/BalanceManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/BalanceManager.java index d0ff0cf05db3..58653c32037d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/BalanceManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/BalanceManager.java @@ -19,6 +19,7 @@ import us.ihmc.commonWalkingControlModules.dynamicPlanning.bipedPlanning.*; import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CoMContinuousContinuityCalculator; import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CoMTrajectoryPlanner; +import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CornerPointViewer; import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.SettableContactStateProvider; import us.ihmc.commonWalkingControlModules.messageHandlers.CenterOfMassTrajectoryHandler; import us.ihmc.commonWalkingControlModules.messageHandlers.MomentumTrajectoryHandler; @@ -299,6 +300,7 @@ public BalanceManager(HighLevelHumanoidControllerToolbox controllerToolbox, maintainInitialCoMVelocityContinuitySingleSupport = new BooleanParameter("maintainInitialCoMVelocityContinuitySingleSupport", registry, true); maintainInitialCoMVelocityContinuityTransfer = new BooleanParameter("maintainInitialCoMVelocityContinuityTransfer", registry, true); comTrajectoryPlanner = new CoMTrajectoryPlanner(controllerToolbox.getGravityZ(), controllerToolbox.getOmega0Provider(), registry); + comTrajectoryPlanner.setCornerPointViewer(new CornerPointViewer(registry, yoGraphicsListRegistry)); comTrajectoryPlanner.setComContinuityCalculator(new CoMContinuousContinuityCalculator(controllerToolbox.getGravityZ(), controllerToolbox.getOmega0Provider(), registry)); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java index 7ee8f8c8fd35..1f02e4ba1f11 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java @@ -13,6 +13,7 @@ import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.CenterOfPressureCommand; import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.MomentumRateCommand; import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.PlaneContactStateCommand; +import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler; import us.ihmc.commonWalkingControlModules.momentumBasedController.CapturePointCalculator; import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.MomentumOptimizationSettings; @@ -20,6 +21,7 @@ import us.ihmc.commonWalkingControlModules.momentumControlCore.HeightController; import us.ihmc.commonWalkingControlModules.momentumControlCore.PelvisHeightController; import us.ihmc.commonWalkingControlModules.wrenchDistribution.WrenchDistributorTools; +import us.ihmc.commons.MathTools; import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FrameVector2D; @@ -59,6 +61,7 @@ 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 static us.ihmc.graphicsDescription.appearance.YoAppearance.*; import static us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory.newYoGraphicPoint2D; @@ -110,6 +113,7 @@ public class LinearMomentumRateControlModule implements SCS2YoGraphicHolder private final FixedFramePoint2DBasics perfectCoP = new FramePoint2D(); private final FixedFramePoint2DBasics desiredCMP = new FramePoint2D(); private final FixedFramePoint2DBasics desiredCoP = new FramePoint2D(); + private final FramePoint3D desiredPendulumBase = new FramePoint3D(); private final FixedFramePoint2DBasics achievedCMP = new FramePoint2D(); private final FrameVector3D achievedLinearMomentumRate = new FrameVector3D(); @@ -155,6 +159,15 @@ public class LinearMomentumRateControlModule implements SCS2YoGraphicHolder private final LinearMomentumRateControlModuleOutput output = new LinearMomentumRateControlModuleOutput(); + //// + private final SideDependentList contactableFeet; + private final WalkingControllerParameters walkingControllerParameters; + private final double controlDt; + private final FramePoint3D leadingPendulumBase = new FramePoint3D(); + private final FramePoint3D trailingPendulumBase = new FramePoint3D(); + private final FramePoint3D alternateCoP = new FramePoint3D(); + private final WalkingMessageHandler walkingMessageHandler; + public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox controllerToolbox, WalkingControllerParameters walkingControllerParameters, YoRegistry parentRegistry) @@ -164,6 +177,7 @@ public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox contro controllerToolbox.getContactableFeet(), controllerToolbox.getFullRobotModel().getElevator(), walkingControllerParameters, + controllerToolbox.getWalkingMessageHandler(), controllerToolbox.getTotalMassProvider(), controllerToolbox.getGravityZ(), controllerToolbox.getControlDT(), @@ -176,6 +190,7 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta SideDependentList contactableFeet, RigidBodyBasics elevator, WalkingControllerParameters walkingControllerParameters, + WalkingMessageHandler walkingMessageHandler, DoubleProvider totalMassProvider, double gravityZ, double controlDT, @@ -184,6 +199,10 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta { this.totalMassProvider = totalMassProvider; this.gravityZ = gravityZ; + this.contactableFeet = contactableFeet; + this.walkingControllerParameters = walkingControllerParameters; + this.walkingMessageHandler = walkingMessageHandler; + this.controlDt = controlDT; MomentumOptimizationSettings momentumOptimizationSettings = walkingControllerParameters.getMomentumOptimizationSettings(); linearMomentumRateWeight = new ParameterVector3D("LinearMomentumRateWeight", momentumOptimizationSettings.getLinearMomentumWeight(), registry); @@ -256,9 +275,74 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta icpController = new ICPController(walkingControllerParameters, icpControlPolygons, contactableFeet, controlDT, registry, yoGraphicsListRegistry); } + useAlternateCoP = new YoBoolean("useAlternateCoP", registry); + parentRegistry.addChild(registry); } + private double leftTimeInContact = 0.0; + private double rightTimeInContact = 0.0; + private double leadingTimeInContact = 0.0; + private final YoBoolean useAlternateCoP; + + private void computeAlternateCoP() + { + boolean leftInContact = contactStateCommands.get(RobotSide.LEFT).getNumberOfContactPoints() > 0; + boolean rightInContact = contactStateCommands.get(RobotSide.RIGHT).getNumberOfContactPoints() > 0; + + RobotSide leadingSide = RobotSide.RIGHT; + RobotSide trailingSide = RobotSide.LEFT; + + if (leftInContact && !rightInContact) + { + leftTimeInContact += controlDt; + rightTimeInContact = 0.0; + leadingTimeInContact = leftTimeInContact; + leadingSide = RobotSide.LEFT; + trailingSide = RobotSide.LEFT; + } + else if (rightInContact && !leftInContact) + { + rightTimeInContact += controlDt; + leftTimeInContact = 0.0; + leadingTimeInContact = rightTimeInContact; + leadingSide = RobotSide.RIGHT; + trailingSide = RobotSide.RIGHT; + } + else if (leftInContact && rightInContact) + { + leftTimeInContact += controlDt; + rightTimeInContact += controlDt; + + if (leftTimeInContact > rightTimeInContact) + { + leadingSide = RobotSide.RIGHT; + trailingSide = RobotSide.LEFT; + leadingTimeInContact = rightTimeInContact; + } + else + { + leadingSide = RobotSide.LEFT; + trailingSide = RobotSide.RIGHT; + leadingTimeInContact = leftTimeInContact; + } + } + + leadingPendulumBase.setToZero(contactableFeet.get(leadingSide).getSoleFrame()); + trailingPendulumBase.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); + + + double transferDuration = walkingMessageHandler.getNextTransferTime(); + double alpha = 1.0; + if (transferDuration > 0.0) + alpha = MathTools.clamp(leadingTimeInContact / transferDuration, 0.0, 1.0); + + leadingPendulumBase.changeFrame(worldFrame); + trailingPendulumBase.changeFrame(worldFrame); + alternateCoP.changeFrame(worldFrame); + alternateCoP.interpolate(trailingPendulumBase, leadingPendulumBase, alpha); + } + public void reset() { desiredLinearMomentumRateWeight.set(linearMomentumRateWeight); @@ -379,13 +463,22 @@ public boolean computeControllerCoreCommands() else desiredLinearMomentumRateWeight.update(linearMomentumRateWeight); + computeAlternateCoP(); + FixedFramePoint2DBasics desiredCoPToUse = desiredCoP; + + if (useAlternateCoP.getBooleanValue()) + desiredCoPToUse.set(alternateCoP); + yoDesiredCMP.set(desiredCMP); - yoDesiredCoP.set(desiredCoP); + yoDesiredCoP.set(desiredCoPToUse); yoCenterOfMass.setFromReferenceFrame(centerOfMassFrame); yoCenterOfMassVelocity.set(capturePointCalculator.getCenterOfMassVelocity()); yoCapturePoint.set(capturePoint); - success = success && computeDesiredLinearMomentumRateOfChange(); + if (useAlternateCoP.getBooleanValue()) + success = success && computeDesiredLinearMomentumRateOfChangeNew(); + else + success = success && computeDesiredLinearMomentumRateOfChange(); selectionMatrix.setToLinearSelectionOnly(); selectionMatrix.selectLinearX(!useCenterOfPressureCommandOnly.getValue()); @@ -396,7 +489,7 @@ public boolean computeControllerCoreCommands() momentumRateCommand.setSelectionMatrix(selectionMatrix); momentumRateCommand.setWeights(angularMomentumRateWeight, desiredLinearMomentumRateWeight); - centerOfPressureCommandCalculator.computeCenterOfPressureCommand(desiredCoP, contactStateCommands, bipedSupportPolygons.getFootPolygonsInSoleFrame()); + centerOfPressureCommandCalculator.computeCenterOfPressureCommand(desiredCoPToUse, contactStateCommands, bipedSupportPolygons.getFootPolygonsInSoleFrame()); return success; } @@ -563,6 +656,25 @@ private boolean computeDesiredLinearMomentumRateOfChange() return success; } + private boolean computeDesiredLinearMomentumRateOfChangeNew() + { + double totalMass = totalMassProvider.getValue(); + double desiredVerticalRateOfChange = totalMass * desiredCoMHeightAcceleration; + + desiredPendulumBase.setIncludingFrame(alternateCoP); + desiredPendulumBase.changeFrame(centerOfMassFrame); + + linearMomentumRateOfChange.setToZero(); + linearMomentumRateOfChange.setMatchingFrame(desiredPendulumBase); + double weight = totalMass * Math.abs(gravityZ); + double expectedVerticalForce = desiredVerticalRateOfChange + weight; + linearMomentumRateOfChange.scale(expectedVerticalForce / linearMomentumRateOfChange.getZ()); + linearMomentumRateOfChange.subZ(weight); +// linearMomentumRateOfChange.changeFrame(worldFrame); + + return true; + } + private static boolean checkInputs(FramePoint2DReadOnly capturePoint, FixedFramePoint2DBasics desiredCapturePoint, FixedFrameVector2DBasics desiredCapturePointVelocity, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/foot/SwingState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/foot/SwingState.java index b92de320c972..e66b2aba547b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/foot/SwingState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/foot/SwingState.java @@ -375,18 +375,18 @@ public void doSpecificAction(double timeInState) computeAndPackTrajectory(timeInState); - if (workspaceLimiterControlModule != null) - { - desiredPose.setIncludingFrame(desiredPosition, desiredOrientation); - changeDesiredPoseBodyFrame(controlFrame, ankleFrame, desiredPose); - desiredAnklePosition.setIncludingFrame(desiredPose.getPosition()); - - workspaceLimiterControlModule.correctSwingFootTrajectory(desiredAnklePosition, desiredLinearVelocity, desiredLinearAcceleration); - - desiredPose.getPosition().set(desiredAnklePosition); - changeDesiredPoseBodyFrame(ankleFrame, controlFrame, desiredPose); - desiredPosition.setIncludingFrame(desiredPose.getPosition()); - } +// if (workspaceLimiterControlModule != null) +// { +// desiredPose.setIncludingFrame(desiredPosition, desiredOrientation); +// changeDesiredPoseBodyFrame(controlFrame, ankleFrame, desiredPose); +// desiredAnklePosition.setIncludingFrame(desiredPose.getPosition()); +// +// workspaceLimiterControlModule.correctSwingFootTrajectory(desiredAnklePosition, desiredLinearVelocity, desiredLinearAcceleration); +// +// desiredPose.getPosition().set(desiredAnklePosition); +// changeDesiredPoseBodyFrame(ankleFrame, controlFrame, desiredPose); +// desiredPosition.setIncludingFrame(desiredPose.getPosition()); +// } if (yoSetDesiredVelocityToZero.getBooleanValue()) { @@ -479,11 +479,11 @@ else if (replanTrajectory.getBooleanValue()) // need to update the swing traject yoReferenceSolePosition.setMatchingFrame(desiredPosition); yoReferenceSoleLinearVelocity.setMatchingFrame(desiredLinearVelocity); - if (!isInTouchdown) - { // we're still in swing, so update the desired setpoints using the smoother - swingTrajectorySmoother.compute(time); - swingTrajectorySmoother.getLinearData(desiredPosition, desiredLinearVelocity, desiredLinearAcceleration); - } +// if (!isInTouchdown) +// { // we're still in swing, so update the desired setpoints using the smoother +// swingTrajectorySmoother.compute(time); +// swingTrajectorySmoother.getLinearData(desiredPosition, desiredLinearVelocity, desiredLinearAcceleration); +// } if (isSwingSpeedUpEnabled.getBooleanValue() && !currentTimeWithSwingSpeedUp.isNaN()) { 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 fe87f5efaaf4..23bb14aa3b71 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.dyanmicsBasedFootstepGenerator.DynamicsBasedFootstepPlugin; 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; @@ -66,7 +68,7 @@ *
  • This generator needs to be informed when footsteps are started/completed:
    * The can be handle automatically by setting it up with * {@link #setFootstepStatusListener(StatusMessageOutputManager)}. Otherwise, the notifications - * should be done every step via {@link #notifyFootstepStarted()} and + * should be done every step via {@link #notifyFootstepStarted(RobotSide side)} and * {@link #notifyFootstepCompleted(RobotSide)}, or * {@link #consumeFootstepStatus(FootstepStatusMessage)}. *
  • Protocol for submitting footsteps to the controller:
    @@ -119,12 +121,20 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder private final String variableNameSuffix = "CSG"; + ///////////////////////////////////////////////// + public enum CSGMode {STANDARD, DYNAMIC} + private final YoEnum csgMode = new YoEnum<>("csgMode", registry, CSGMode.class); + + private final OptionalFactoryField dynamicsBasedFootstepPlugin = new OptionalFactoryField<>("CSGDynamicsBasedFootstepProvider"); + ///////////////// + private BooleanProvider walkInputProvider; private DoubleProvider swingHeightInputProvider; 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 YoEnum currentSwingSide = new YoEnum<>("currentSwingSide" + variableNameSuffix, registry, RobotSide.class); private final YoEnum currentSupportSide = new YoEnum<>("currentSupportSide" + variableNameSuffix, registry, RobotSide.class); private final YoFramePose3D currentSupportFootPose = new YoFramePose3D("currentSupportFootPose" + variableNameSuffix, worldFrame, registry); @@ -176,7 +186,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); @@ -198,12 +208,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()); @@ -230,7 +234,8 @@ else if (startWalkingMessenger != null && walk.getValue() != walkPreviousValue.g counter = numberOfTicksBeforeSubmittingFootsteps.getValue(); // To make footsteps being sent right away. } - { // Processing footstep status + // Process footstep status + { FootstepStatus statusToProcess = latestStatusReceived.getValue(); if (statusToProcess != null) @@ -253,6 +258,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) footstepCompletionSide.setValue(null); } + // Determine swing side RobotSide swingSide; if (footsteps.isEmpty()) @@ -263,15 +269,37 @@ else if (statusToProcess == FootstepStatus.COMPLETED) } else { - while (footsteps.size() > Math.max(1, parameters.getNumberOfFixedFootsteps())) - footsteps.fastRemove(footsteps.size() - 1); +// while (footsteps.size() > Math.max(1, parameters.getNumberOfFixedFootsteps())) +// footsteps.fastRemove(footsteps.size() - 1); +// +// FootstepDataMessage previousFootstep = footsteps.get(footsteps.size() - 1); +// footstepPose2D.getPosition().set(previousFootstep.getLocation()); +// footstepPose2D.getOrientation().set(previousFootstep.getOrientation()); +// swingSide = RobotSide.fromByte(previousFootstep.getRobotSide()).getOppositeSide(); +// +// previousFootstepPose.set(previousFootstep.getLocation(), previousFootstep.getOrientation()); - FootstepDataMessage previousFootstep = footsteps.get(footsteps.size() - 1); - footstepPose2D.getPosition().set(previousFootstep.getLocation()); - footstepPose2D.getOrientation().set(previousFootstep.getOrientation()); - swingSide = RobotSide.fromByte(previousFootstep.getRobotSide()).getOppositeSide(); + footsteps.clear(); + swingSide = currentSwingSide.getEnumValue(); + previousFootstepPose.set(currentSupportFootPose); + } - previousFootstepPose.set(previousFootstep.getLocation(), previousFootstep.getOrientation()); + // Set parameters for FootstepDataListMessage + if (csgMode.getEnumValue() == CSGMode.DYNAMIC && dynamicsBasedFootstepPlugin.hasValue()) + { + footstepDataListMessage.setDefaultSwingDuration(dynamicsBasedFootstepPlugin.get().getSwingDuration(swingSide)); + footstepDataListMessage.setDefaultTransferDuration(dynamicsBasedFootstepPlugin.get().getTransferDuration(swingSide)); + footstepDataListMessage.setFinalTransferDuration(dynamicsBasedFootstepPlugin.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(); @@ -281,7 +309,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) double turnMaxAngleInward = parameters.getTurnMaxAngleInward(); double turnMaxAngleOutward = parameters.getTurnMaxAngleOutward(); - Vector2DReadOnly desiredVelocity = desiredVelocityProvider.getDesiredVelocity(); + Vector2DReadOnly desiredVelocity = this.desiredVelocity;//desiredVelocityProvider.getDesiredVelocity(); double desiredVelocityX = desiredVelocity.getX(); double desiredVelocityY = desiredVelocity.getY(); double turningVelocity = desiredTurningVelocityProvider.getTurningVelocity(); @@ -300,39 +328,67 @@ else if (statusToProcess == FootstepStatus.COMPLETED) turningVelocity = minMaxVelocityTurn * MathTools.clamp(turningVelocity, 1.0); } - this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); +// this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); this.desiredTurningVelocity.set(turningVelocity); + dynamicsBasedFootstepPlugin.get().update(time); + int startingIndexToAdjust = footsteps.size(); 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; + FootstepDataMessage footstep = footsteps.add(); +// RobotSide footstepSide = RobotSide.RIGHT;//!(RobotSide.fromByte(footstep.getRobotSide()) == null) ? RobotSide.fromByte(footstep.getRobotSide()) : RobotSide.LEFT; +// footstep.setRobotSide(footstepSide.toByte()); - if (swingSide == RobotSide.LEFT) + ////// TODO new stuff here + if (csgMode.getEnumValue() == CSGMode.DYNAMIC && dynamicsBasedFootstepPlugin.hasValue()) { - yDisplacement = MathTools.clamp(yDisplacement, minStepWidth, maxStepWidth); - headingDisplacement = MathTools.clamp(headingDisplacement, turnMaxAngleInward, turnMaxAngleOutward); + if (i == startingIndexToAdjust) + { + dynamicsBasedFootstepPlugin.get().getDesiredTouchdownPosition2D(swingSide, nextFootstepPose2D.getPosition()); + } + else + { + calculateNextFootstepPose2D(stepTime.getValue(), + desiredVelocityX, + desiredVelocityY, + desiredTurningVelocity.getDoubleValue(), + swingSide, + maxStepLength, + maxStepWidth, + defaultStepWidth, + minStepWidth, + turnMaxAngleInward, + turnMaxAngleOutward, + footstepPose2D, + nextFootstepPose2D); + +// dynamicsBasedFootstepPlugin.get().calculate(footstepSide, +// dynamicsBasedFootstepPlugin.get().getStepDuration(footstepSide), +// footstepPose2D, +// 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(); for (int adjustorIndex = 0; adjustorIndex < footstepAdjustments.size(); adjustorIndex++) footstepAdjustments.get(adjustorIndex).adjustFootstep(currentSupportFootPose, nextFootstepPose2D, swingSide, footstep); @@ -441,6 +497,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. * @@ -510,6 +596,7 @@ public void setSwingHeight(double swingHeight) public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider desiredTurningVelocityProvider) { this.desiredTurningVelocityProvider = desiredTurningVelocityProvider; + dynamicsBasedFootstepPlugin.get().setDesiredTurningVelocityProvider(desiredTurningVelocityProvider); } /** @@ -520,6 +607,7 @@ public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider des public void setDesiredVelocityProvider(DesiredVelocityProvider desiredVelocityProvider) { this.desiredVelocityProvider = desiredVelocityProvider; + dynamicsBasedFootstepPlugin.get().setDesiredVelocityProvider(desiredVelocityProvider); } /** @@ -574,6 +662,7 @@ public void notifyFootstepCompleted(RobotSide robotSide) { latestStatusReceived.setValue(FootstepStatus.COMPLETED); footstepCompletionSide.setValue(robotSide); + currentSwingSide.set(robotSide.getOppositeSide()); } /** @@ -583,16 +672,17 @@ 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); + currentSwingSide.set(robotSide); } /** * 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)}. *

    * @@ -601,10 +691,11 @@ public void notifyFootstepStarted() public void setFootstepStatusListener(StatusMessageOutputManager statusMessageOutputManager) { statusMessageOutputManager.attachStatusMessageListener(FootstepStatusMessage.class, this::consumeFootstepStatus); + dynamicsBasedFootstepPlugin.get().setFootstepStatusListener(statusMessageOutputManager); } /** - * 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. @@ -615,7 +706,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())); } /** @@ -756,6 +847,11 @@ public void setAlternateStepChooser(AlternateStepChooser alternateStepChooser) this.alternateStepChooser = alternateStepChooser; } + public void setDynamicsBasedFootstepPlugin(DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin) + { + this.dynamicsBasedFootstepPlugin.set(dynamicsBasedFootstepPlugin); + } + /** * 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/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepParameters.java new file mode 100644 index 000000000000..0f0378deeddb --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepParameters.java @@ -0,0 +1,164 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +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 DynamicsBasedFootstepParameters +{ + // 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 DynamicsBasedFootstepParameters(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); + } + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java new file mode 100644 index 000000000000..b8799714ae25 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java @@ -0,0 +1,644 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +import controller_msgs.msg.dds.FootstepStatusMessage; +import org.apache.commons.lang3.mutable.MutableObject; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.*; +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPlugin; +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.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +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.mecano.frames.MovingReferenceFrame; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.robotics.screwTheory.AngularExcursionCalculator; +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.euclid.referenceFrame.YoFrameVector3D; +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 us.ihmc.yoVariables.variable.YoEnum; + +public class DynamicsBasedFootstepPlugin implements HumanoidSteppingPlugin +{ + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + private final DynamicsBasedFootstepParameters parameters; + + private final String variableNameSuffix = "QFP"; + + private final CommonHumanoidReferenceFrames referenceFrames; + + private final double updateDT; + private final double gravity = 9.81; + + // Estimates + private final YoFramePoint3D currentCoMPosition = new YoFramePoint3D("currentCoMPosition" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + private final YoFrameVector3D currentCoMVelocity = new YoFrameVector3D("currentCoMVelocity" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + private final YoFrameVector3D currentCentroidalLinearMomentum = new YoFrameVector3D("currentCentroidalLinearMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + private final YoFrameVector3D currentCentroidalAngularMomentum = new YoFrameVector3D("currentCentroidalAngularMomentum" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + private final AngularExcursionCalculator angularExcursionCalculator; + private final MovingReferenceFrame centerOfMassControlFrame; + private final MovingZUpFrame centerOfMassControlZUpFrame; + private final MovingReferenceFrame centerOfMassFrame; +// private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; + + // 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 MutableObject latestFootstepStatusReceived = new MutableObject<>(null); + private final MutableObject footstepCompletionSide = new MutableObject<>(null); + 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; + + + // + private final static Vector2DReadOnly zero2D = new Vector2D(); + private BooleanProvider walkInputProvider; + private DoubleProvider swingHeightInputProvider; + private DesiredVelocityProvider desiredVelocityProvider = () -> zero2D; + private DesiredTurningVelocityProvider desiredTurningVelocityProvider = () -> 0.0; + private DirectionalControlMessenger directionalControlMessenger; + private StopWalkingMessenger stopWalkingMessenger; + private StartWalkingMessenger startWalkingMessenger; + private FootstepMessenger footstepMessenger; + + public DynamicsBasedFootstepPlugin(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) + { + this.updateDT = updateDT; + this.referenceFrames = referenceFrames; + + // Parameters + parameters = new DynamicsBasedFootstepParameters(gravity, registry); + + // All things estimates + centerOfMassFrame = (MovingReferenceFrame) referenceFrames.getCenterOfMassFrame(); + + angularExcursionCalculator = new AngularExcursionCalculator(centerOfMassFrame, robotModel.getElevator(), updateDT, registry, null); + + desiredPelvisOrientation = new YoFrameQuaternion("pelvisDesiredOrientation" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + + 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); + + netPendulumBaseViz = new YoGraphicPosition("netPendulumBase" + variableNameSuffix, + netPendulumBase3DInWorld, + 0.015, + YoAppearance.Red(), + YoGraphicPosition.GraphicType.SQUARE); + + + //groundPlaneEstimator = new FootSoleBasedGroundPlaneEstimator(centerOfMassControlZUpFrame, referenceFrames, yoGraphicsListRegistry, registry); + + // 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)); + + pendulumBaseViz.put(robotSide, new YoGraphicPosition(robotSide.getLowerCaseName() + "PendulumBase" + variableNameSuffix, + pendulumBase3DInWorld.get(robotSide), + 0.015, + YoAppearance.Black(), + YoGraphicPosition.GraphicType.SQUARE)); + + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, pendulumBaseViz.get(robotSide)); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, pendulumBaseViz.get(robotSide).createArtifact()); + + touchdownCalculator.put(robotSide, new DynamicsBasedFootstepTouchdownCalculator(robotSide, + centerOfMassControlZUpFrame, + getCenterOfMassVelocity(), + currentCentroidalAngularMomentum, + 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)); + + stateMachineFactory.addDoneTransition(FootState.SUPPORT, FootState.SWING); + stateMachineFactory.addDoneTransition(FootState.SWING, FootState.SUPPORT); + + footStateMachines.put(robotSide, stateMachineFactory.build(FootState.SUPPORT)); + footStateMachines.get(robotSide).resetToInitialState(); + } + + yoGraphicsListRegistry.registerYoGraphic(variableNameSuffix, netPendulumBaseViz); + yoGraphicsListRegistry.registerArtifact(variableNameSuffix, netPendulumBaseViz.createArtifact()); + } + + 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() + { + calculate(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)); + + 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.SUPPORT; + } + } + + @Override + public void update(double time) + { +// groundPlaneEstimator.update(); + + currentCoMPosition.setFromReferenceFrame(centerOfMassFrame); + + angularExcursionCalculator.compute(); + currentCentroidalLinearMomentum.setMatchingFrame(angularExcursionCalculator.getLinearMomentum()); + currentCentroidalAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); + + // TODO maybe wrong? + desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); + desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); + + centerOfMassControlFrame.update(); + centerOfMassControlZUpFrame.update(); + + currentCoMVelocity.setMatchingFrame(centerOfMassFrame.getTwistOfFrame().getLinearPart()); + + handleDesiredsFromProviders(); + + inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && + footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); + + calculateNetPendulumBase(); + + 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 calculate(RobotSide swingSide, + double timeToReachGoal, + FramePose2DReadOnly pendulumBase, + FramePose2DReadOnly netPendulumBase, + FramePose2DBasics touchdownPositionToPack) + { + calculate(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 calculate(DynamicsBasedFootstepTouchdownCalculator 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 (footStateMachines.get(RobotSide.LEFT).getTimeInCurrentState() > footStateMachines.get(RobotSide.RIGHT).getTimeInCurrentState()) + 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); + } + + /** + * Sets the protocol for sending footsteps to the controller. + * + * @param footstepMessenger the callback used to send footsteps. + */ + public void setFootstepMessenger(FootstepMessenger footstepMessenger) + { + this.footstepMessenger = footstepMessenger; + } + + /** + * 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) + { + latestFootstepStatusReceived.setValue(FootstepStatus.COMPLETED); + footstepCompletionSide.setValue(robotSide); + footStates.get(robotSide).set(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) + { + latestFootstepStatusReceived.setValue(FootstepStatus.STARTED); + footstepCompletionSide.setValue(null); + footStates.get(robotSide).set(FootState.SWING); + } + + /** + * 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; + } + + /** + * Sets a provider that is to be used to update the desired swing height of each foot internally + * on each call to {@link #update(double)} + * + * @param swingHeightInputProvider the provider used to set the swing height + */ + public void setSwingHeightInputProvider(DoubleProvider swingHeightInputProvider) + { + this.swingHeightInputProvider = swingHeightInputProvider; + } + + /** + * 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; + } + + /** + * Sets the protocol for stop walking requests to the controller. + * + * @param stopWalkingMessenger the callback used to send requests. + */ + public void setStopWalkingMessenger(StopWalkingMessenger stopWalkingMessenger) + { + this.stopWalkingMessenger = stopWalkingMessenger; + } + + /** + * Sets the protocol for start walking requests to the controller. + * + * @param startWalkingMessenger the callback used to send requests. + */ + public void setStartWalkingMessenger(StartWalkingMessenger startWalkingMessenger) + { + this.startWalkingMessenger = startWalkingMessenger; + } + + @Override + public void setFootstepAdjustment(FootstepAdjustment footstepAdjustment) + { + + } + + 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 FrameVector3DReadOnly getCenterOfMassVelocity() + { + return currentCoMVelocity; + } + + 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); + } + + @Override + public YoRegistry getRegistry() + { + return registry; + } +} \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPluginFactory.java new file mode 100644 index 000000000000..b865a7eb7e6f --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPluginFactory.java @@ -0,0 +1,121 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +import controller_msgs.msg.dds.PauseWalkingMessage; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.commonWalkingControlModules.controllers.Updatable; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.*; +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPlugin; +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPluginFactory; +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.StepGeneratorCommandInputManager; +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; +import us.ihmc.tools.factories.OptionalFactoryField; +import us.ihmc.yoVariables.providers.DoubleProvider; + +import java.util.function.Consumer; + +public class DynamicsBasedFootstepPluginFactory implements HumanoidSteppingPluginFactory +{ + private final OptionalFactoryField csgCommandInputManagerField = new OptionalFactoryField<>("csgCommandInputManagerField"); + + public StepGeneratorCommandInputManager setStepGeneratorCommandInputManager() + { + StepGeneratorCommandInputManager csgCommandInputManager = new StepGeneratorCommandInputManager(); + setStepGeneratorCommandInputManager(csgCommandInputManager); + return csgCommandInputManager; + } + + public void setStepGeneratorCommandInputManager(StepGeneratorCommandInputManager commandInputManager) + { + this.csgCommandInputManagerField.set(commandInputManager); + } + + @Override + public StepGeneratorCommandInputManager getStepGeneratorCommandInputManager() + { + if (csgCommandInputManagerField.hasValue()) + return csgCommandInputManagerField.get(); + else + return setStepGeneratorCommandInputManager(); + } + + @Override + public void setFootStepAdjustment(FootstepAdjustment footstepAdjustment) + { + + } + + @Override + public void setFootStepPlanAdjustment(FootstepPlanAdjustment footstepAdjustment) + { + + } + + @Override + public void addFootstepValidityIndicator(FootstepValidityIndicator footstepValidityIndicator) + { + + } + + @Override + public void addPlanarRegionsListCommandConsumer(Consumer planarRegionsListCommandConsumer) + { + + } + + @Override + public void addUpdatable(Updatable updatable) + { + + } + + @Override + public DynamicsBasedFootstepPlugin buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, + double updateDT, + WalkingControllerParameters walkingControllerParameters, + StatusMessageOutputManager walkingStatusMessageOutputManager, + CommandInputManager walkingCommandInputManager, + YoGraphicsListRegistry yoGraphicsListRegistry, + SideDependentList contactableFeet, + DoubleProvider timeProvider) + { + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = new DynamicsBasedFootstepPlugin(robotModel, referenceFrames, updateDT, yoGraphicsListRegistry, timeProvider); + + dynamicsBasedFootstepPlugin.setStopWalkingMessenger(new StopWalkingMessenger() + { + private final PauseWalkingMessage message = HumanoidMessageTools.createPauseWalkingMessage(true); + + @Override + public void submitStopWalkingRequest() + { + message.setClearRemainingFootstepQueue(true); + walkingCommandInputManager.submitMessage(message); + } + }); + + dynamicsBasedFootstepPlugin.setStartWalkingMessenger(new StartWalkingMessenger() + { + private final PauseWalkingMessage message = HumanoidMessageTools.createPauseWalkingMessage(false); + + @Override + public void submitStartWalkingRequest() + { + walkingCommandInputManager.submitMessage(message); + } + }); + + dynamicsBasedFootstepPlugin.setFootstepStatusListener(walkingStatusMessageOutputManager); + + dynamicsBasedFootstepPlugin.setFootstepMessenger(walkingCommandInputManager::submitMessage); + + return dynamicsBasedFootstepPlugin; + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepTouchdownCalculator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepTouchdownCalculator.java new file mode 100644 index 000000000000..c53fbcc0f183 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepTouchdownCalculator.java @@ -0,0 +1,555 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +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 DynamicsBasedFootstepTouchdownCalculator +{ + 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; + private final YoFrameVector3D angularMomentumAboutFootInWorld; + + // 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 DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, + MovingReferenceFrame centerOfMassControlZUpFrame, + FrameVector3DReadOnly centerOfMassVelocity, + FrameVector3DReadOnly centroidalAngularMomentum, + FullHumanoidRobotModel robotModel, + DynamicsBasedFootstepParameters 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); + angularMomentumAboutFootInWorld = new YoFrameVector3D(robotSide.getOppositeSide().getLowerCaseName() + "AngularMomentumAboutFootInWorld_QFP", 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); + } + + private final FramePoint3D tempVelocity = new FramePoint3D(); + private final FramePoint3D tempCentroidalMomentum = new FramePoint3D(); + private final FramePoint2D tempCoP2D = new FramePoint2D(); + private final FramePoint3D tempCoP = new FramePoint3D(); + private final FramePoint3D tempCoM = new FramePoint3D(); + private final FramePoint3D tempAngularMomentumAboutFoot = new FramePoint3D(); + private void calculateCurrentAngularMomentum(FramePoint2DReadOnly pendulumBase, RobotSide supportSide) + { + tempCoP2D.setIncludingFrame(pendulumBase); + tempCoP2D.changeFrameAndProjectToXYPlane(soleFrames.get(supportSide)); + tempCoP.setIncludingFrame(tempCoP2D, 0.0); + + tempCoM.setToZero(centerOfMassControlZUpFrame); + tempCoM.changeFrame(soleFrames.get(supportSide)); + + double height = tempCoM.getZ() - tempCoP.getZ(); + + tempVelocity.setIncludingFrame(centerOfMassVelocity); + tempVelocity.changeFrame(soleFrames.get(supportSide)); + + tempAngularMomentumAboutFoot.setIncludingFrame(tempVelocity); + tempAngularMomentumAboutFoot.scale(height * mass, height * mass, 1.0); + tempAngularMomentumAboutFoot.changeFrame(ReferenceFrame.getWorldFrame()); + + tempCentroidalMomentum.setIncludingFrame(centroidalAngularMomentum); + tempCentroidalMomentum.changeFrame(ReferenceFrame.getWorldFrame()); + + angularMomentumAboutFootInWorld.setToZero(); + angularMomentumAboutFootInWorld.add(tempAngularMomentumAboutFoot); + angularMomentumAboutFootInWorld.add(tempCentroidalMomentum); + + } + + 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); + + calculateCurrentAngularMomentum(pendulumBase, supportSide); + + 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); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/FootSoleBasedGroundPlaneEstimator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/FootSoleBasedGroundPlaneEstimator.java new file mode 100644 index 000000000000..8d6ea0a26f78 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/FootSoleBasedGroundPlaneEstimator.java @@ -0,0 +1,191 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +import us.ihmc.commons.lists.RecyclingArrayList; +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.graphicsDescription.appearance.YoAppearance; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotics.geometry.GroundPlaneEstimator; +import us.ihmc.robotics.geometry.YoGroundPlaneEstimator; +import us.ihmc.robotics.referenceFrames.ZUpFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; +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; + +public class FootSoleBasedGroundPlaneEstimator +{ + private final CommonHumanoidReferenceFrames referenceFrames; + + private final MovingReferenceFrame centerOfMassControlZUpFrame; + + private final YoGroundPlaneEstimator groundPlaneEstimator; + private final List contactPoints = new ArrayList<>(); + private final RecyclingArrayList frameContactPoints = new RecyclingArrayList<>(FramePoint3D.class); + private RobotSide supportSide = RobotSide.LEFT; + + private final FramePoint3D groundPlanePoint; + + private final YoDouble groundPlaneVerticalOffset; + private final YoBoolean accountForGroundOrientation; + + + private final ZUpFrame groundPlaneZUpFrame; + + /** + * Determines the position and orientation of the steppable region either by assuming flat ground, + * in which case the location and orientation of the ground matches that of the stance foot, or by + * using planar regions from a {@code PlanarRegionsListCommand} + * + * @param parentRegistry the {@code YoRegistry} used to register YoVariables constructed within this class + */ + public FootSoleBasedGroundPlaneEstimator(MovingReferenceFrame centerOfMassControlZUpFrame, CommonHumanoidReferenceFrames referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry, YoRegistry parentRegistry) + { + this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; + + this.referenceFrames = referenceFrames; + + groundPlaneEstimator = new YoGroundPlaneEstimator("FootSoleBased", + parentRegistry, + yoGraphicsListRegistry, + YoAppearance.Red()); + + groundPlaneVerticalOffset = new YoDouble("groundPlaneVerticalOffset", parentRegistry); + accountForGroundOrientation = new YoBoolean("accountForGroundOrientation", parentRegistry); + + groundPlanePoint = new FramePoint3D(); + + groundPlaneZUpFrame = new ZUpFrame(getGroundPlaneFrame(), "groundPlaneZUpFrame"); + + contactPoints.add(new Point3D(-0.1, -0.1, 0.0)); + contactPoints.add(new Point3D(-0.1, 0.1, 0.0)); + contactPoints.add(new Point3D(0.1, 0.1, 0.0)); + contactPoints.add(new Point3D(0.1, -0.1, 0.0)); + + for (int i = 0; i < contactPoints.size(); i++) + frameContactPoints.add(); + } + + public void reset() + { + // TODO detect the sides that are on the ground, in case we call reset on entering the state where one foot isn't on the ground, or the support side is + // wrong +// supportSide = controllerToolbox.getCommonVariables().getSwingside().getEnumValue().getOppositeSide(); + + updateFrameContactPoints(false); + computeGroundPlaneEstimator(); + } + + public void update() + { + groundPlaneZUpFrame.update(); + + update(false, false, false); + } + + public void update(boolean isToeingOff, boolean isFootSlipping, boolean isFootRocking) + { + groundPlaneZUpFrame.update(); + + if (!isFootSlipping && !isFootRocking) + { + updateFrameContactPoints(isToeingOff); + computeGroundPlaneEstimator(); + } + } + + private void updateFrameContactPoints(boolean isInToeOff) + { + FramePoint3D lowestContactPoint = frameContactPoints.get(0); + + for (int i = 0; i < frameContactPoints.size(); i++) + { + FramePoint3D frameContactPoint = frameContactPoints.get(i); + + if (!isInToeOff) + { + frameContactPoint.setMatchingFrame(referenceFrames.getSoleFrame(supportSide), contactPoints.get(i)); + frameContactPoint.addZ(groundPlaneVerticalOffset.getDoubleValue()); + } + else if (i > 1) + { +// frameContactPoint.setMatchingFrame(controllerToolbox.getEstimates().getToePosition(supportSide)); + frameContactPoint.addZ(groundPlaneVerticalOffset.getDoubleValue()); + } + + if (frameContactPoint.getZ() < lowestContactPoint.getZ()) + lowestContactPoint = frameContactPoint; + } + + for (int i = 0; i < frameContactPoints.size(); i++) + { + FramePoint3D frameContactPoint = frameContactPoints.get(i); + + if (!accountForGroundOrientation.getBooleanValue()) + frameContactPoint.setZ(lowestContactPoint.getZ()); + } + } + + private void computeGroundPlaneEstimator() + { + double yaw = centerOfMassControlZUpFrame.getTransformToWorldFrame().getRotation().getYaw(); + + groundPlaneEstimator.compute(frameContactPoints, yaw); + groundPlaneEstimator.getPlanePoint(groundPlanePoint); + } + + public FrameTuple3DReadOnly getGroundPosition() + { + return groundPlanePoint; + } + + public FrameTuple3DReadOnly getGroundPosition(FrameTuple2DReadOnly touchDownPosition2D) + { + getGroundPosition(groundPlanePoint, touchDownPosition2D, groundPlaneEstimator.getPlane()); + return groundPlanePoint; + } + + public static void getGroundPosition(FramePoint3DBasics groundPosition3DToPack, FrameTuple2DReadOnly touchDownPosition2D, Plane3DReadOnly groundPlane) + { + groundPosition3DToPack.setMatchingFrame(touchDownPosition2D, 0.0); + GroundPlaneEstimator.projectZ(groundPosition3DToPack, groundPlane); + } + + public FrameOrientation3DReadOnly getGroundOrientation() + { + return (FrameOrientation3DReadOnly) groundPlaneEstimator.getGroundPlaneFrame().getOrientation(); + } + + public Plane3DReadOnly getGroundPlane() + { + return groundPlaneEstimator.getPlane(); + } + + public ReferenceFrame getGroundPlaneFrame() + { + return groundPlaneEstimator.getGroundPlaneFrame(); + } + + public ReferenceFrame getGroundPlaneZUpFrame() + { + return groundPlaneZUpFrame; + } + + public void hideGraphics() + { + groundPlaneEstimator.hideGraphics(); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/WholeBodyControllerCoreFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/WholeBodyControllerCoreFactory.java index 64f0aadbb921..4870ab0ba808 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/WholeBodyControllerCoreFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/WholeBodyControllerCoreFactory.java @@ -175,6 +175,7 @@ public LinearMomentumRateControlModule getOrCreateLinearMomentumRateControlModul contactableFeet, elevator, walkingControllerParameters, + controllerToolbox.getWalkingMessageHandler(), totalMassProvider, gravityZ, controlDT, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingControllerState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingControllerState.java index f9c5ab233630..a60eaec65e0c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingControllerState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingControllerState.java @@ -14,16 +14,22 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; +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.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics; import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.robotics.time.ExecutionTimer; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition; import us.ihmc.sensorProcessing.model.RobotMotionStatus; import us.ihmc.sensorProcessing.outputData.JointDesiredOutputList; import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.parameters.BooleanParameter; import us.ihmc.yoVariables.variable.YoBoolean; @@ -86,6 +92,13 @@ public WalkingControllerState(CommandInputManager commandInputManager, else kinematicsSimulationVirtualGroundReactionManager = null; + ///// + for (RobotSide robotSide : RobotSide.values) + { + controllerCoreOutputCoP.put(robotSide, new FramePoint2D()); + angularMomentumAboutFootInWorld.put(robotSide, new YoFrameVector3D(robotSide.getLowerCaseName() + "AngularMomentumAboutFootInWorldController_CoreOutput", ReferenceFrame.getWorldFrame(), registry)); + } + registry.addChild(walkingController.getYoVariableRegistry()); } @@ -100,6 +113,13 @@ public void initialize() kinematicsSimulationVirtualGroundReactionManager.initialize(); } + private final SideDependentList controllerCoreOutputCoP = new SideDependentList<>(); + private final SideDependentList angularMomentumAboutFootInWorld = new SideDependentList<>(); + private final FramePoint3D tempVelocity = new FramePoint3D(); + private final FramePoint3D tempCentroidalMomentum = new FramePoint3D(); + private final FramePoint3D tempCoP = new FramePoint3D(); + private final FramePoint3D tempCoM = new FramePoint3D(); + private final FramePoint3D tempAngularMomentumAboutFoot = new FramePoint3D(); @Override public void doAction(double timeInState) { @@ -152,6 +172,36 @@ public void doAction(double timeInState) controllerCore.compute(controllerCoreCommand); controllerCoreTimer.stopMeasurement(); + for (RobotSide robotSide : RobotSide.values) + { + if (controllerToolbox.getFootContactState(robotSide).inContact()) + { + controllerCore.getControllerCoreOutput().getDesiredCenterOfPressure(controllerCoreOutputCoP.get(robotSide), controllerToolbox.getContactableFeet().get(robotSide).getRigidBody()); + + controllerCoreOutputCoP.get(robotSide).changeFrameAndProjectToXYPlane(controllerToolbox.getReferenceFrames().getSoleZUpFrame(robotSide)); + tempCoP.setIncludingFrame(controllerCoreOutputCoP.get(robotSide), 0.0); + + tempCoM.setToZero(controllerToolbox.getCenterOfMassFrame()); + tempCoM.changeFrame(controllerToolbox.getReferenceFrames().getSoleZUpFrame(robotSide)); + + double height = tempCoM.getZ() - tempCoP.getZ(); + + tempVelocity.setIncludingFrame(controllerToolbox.getCenterOfMassVelocity()); + tempVelocity.changeFrame(controllerToolbox.getReferenceFrames().getSoleZUpFrame(robotSide)); + + tempAngularMomentumAboutFoot.setIncludingFrame(tempVelocity); + tempAngularMomentumAboutFoot.scale(height * controllerToolbox.getTotalMass(), height * controllerToolbox.getTotalMass(), 1.0); + tempAngularMomentumAboutFoot.changeFrame(ReferenceFrame.getWorldFrame()); + + tempCentroidalMomentum.setIncludingFrame(controllerToolbox.getAngularMomentum()); + tempCentroidalMomentum.changeFrame(ReferenceFrame.getWorldFrame()); + + angularMomentumAboutFootInWorld.get(robotSide).setToZero(); + angularMomentumAboutFootInWorld.get(robotSide).add(tempAngularMomentumAboutFoot); + angularMomentumAboutFootInWorld.get(robotSide).add(tempCentroidalMomentum); + } + } + linearMomentumRateControlModule.setInputFromControllerCore(controllerCore.getControllerCoreOutput()); linearMomentumRateControlModule.computeAchievedCMP(); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java index eacdc278c12f..44b48fe01b23 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java @@ -749,7 +749,7 @@ public void updateFailureDetection() capturePoint2d.setIncludingFrame(balanceManager.getCapturePoint()); failureDetectionControlModule.checkIfRobotIsFalling(capturePoint2d, balanceManager.getDesiredICP()); - if (failureDetectionControlModule.isRobotFalling()) + if (false)//failureDetectionControlModule.isRobotFalling()) { walkingMessageHandler.clearFootsteps(); walkingMessageHandler.clearFlamingoCommands(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferState.java index 6b8d05b4567a..5480d6225619 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferState.java @@ -45,6 +45,7 @@ public abstract class TransferState extends WalkingState private final DoubleProvider unloadFraction; private final DoubleProvider rhoMin; + public TransferState(WalkingStateEnum transferStateEnum, WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox controllerToolbox, @@ -107,6 +108,8 @@ public void doAction(double timeInState) double nominalPercentInUnloading = (percentInTransfer - unloadFraction.getValue()) / (1.0 - unloadFraction.getValue()); double icpBasedPercentInUnloading = 1.0 - MathTools.clamp(balanceManager.getNormalizedEllipticICPError() - 1.0, 0.0, 1.0); double percentInUnloading = Math.min(nominalPercentInUnloading, icpBasedPercentInUnloading); + if (controllerToolbox.getAlterEndConditionOfTransfer().getValue()) + percentInUnloading = 0.0; feetManager.unload(transferToSide.getOppositeSide(), percentInUnloading, rhoMin.getValue()); } } @@ -118,6 +121,9 @@ public void doAction(double timeInState) @Override public boolean isDone(double timeInState) { + if (controllerToolbox.getAlterEndConditionOfTransfer().getValue()) + return timeInState > stepTiming.getTransferTime(); + //If we're using a precomputed icp trajectory we can't rely on the icp planner's state to dictate when to exit transfer. boolean transferTimeElapsedUnderPrecomputedICPPlan = false; if (balanceManager.isPrecomputedICPPlannerActive()) 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 2ff6888b3876..592221e1d581 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 @@ -2,6 +2,7 @@ import java.util.function.Predicate; +import controller_msgs.msg.dds.WalkingStatusMessage; import org.apache.commons.math3.util.Precision; import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.YoPlaneContactState; @@ -18,6 +19,7 @@ import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler; import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.commonWalkingControlModules.momentumBasedController.ParameterProvider; +import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.FrameQuaternion; @@ -81,6 +83,10 @@ public class WalkingSingleSupportState extends SingleSupportState private final DoubleProvider swingFootCoPWeight; private final CenterOfPressureCommand copCommand = new CenterOfPressureCommand(); + private boolean haveWeEntered = false; + + private final WalkingMessageHandler.Listener listener = this::test; + public WalkingSingleSupportState(WalkingStateEnum stateEnum, WalkingMessageHandler walkingMessageHandler, TouchdownErrorCompensator touchdownErrorCompensator, @@ -132,6 +138,13 @@ 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.addListener(listener); + } + + public void poll() + { + walkingMessageHandler.poll(nextFootstep, footstepTiming); } int stepsToAdd; @@ -188,7 +201,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 @@ -251,6 +264,14 @@ private void updateWalkingTrajectoryPath() @Override public boolean isDone(double timeInState) { + if (controllerToolbox.getAlterEndConditionOfTransfer().getValue()) + { + if (!hasSwingFootTouchedDown.getValue() && super.isDone(timeInState)) + hasSwingFootTouchedDown.set(true); + + return super.isDone(timeInState); + } + if (!hasSwingFootTouchedDown.getValue() && super.isDone(timeInState)) hasSwingFootTouchedDown.set(true); @@ -273,21 +294,37 @@ public boolean isDone(double timeInState) return finishWhenICPPlannerIsDone.getValue() && balanceManager.isICPPlanDone(); } - @Override - public void onEntry() + private void test() { - super.onEntry(); + if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) + { + walkingMessageHandler.peekFootstep(0, nextFootstep); + + if (nextFootstep.getRobotSide() != swingSide || !haveWeEntered) + return; + + balanceManager.clearICPPlan(); + + swingTime = walkingMessageHandler.getNextSwingTime(); + + walkingMessageHandler.poll(nextFootstep, footstepTiming); + + + if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) + walkingMessageHandler.peekFootstep(0, nextNextFootstep); + + balanceManager.addFootstepToPlan(nextFootstep, footstepTiming); + } - hasSwingFootTouchedDown.set(false); - hasTriggeredTouchdown.set(false); - double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); - swingTime = walkingMessageHandler.getNextSwingTime(); - walkingMessageHandler.poll(nextFootstep, footstepTiming); - if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) - walkingMessageHandler.peekFootstep(0, nextNextFootstep); + + + + double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); + + feetManager.adjustHeightIfNeeded(nextFootstep); desiredFootPoseInWorld.set(nextFootstep.getFootstepPose()); desiredFootPoseInWorld.changeFrame(worldFrame); @@ -301,7 +338,6 @@ public void onEntry() balanceManager.minimizeAngularMomentumRateZ(minimizeAngularMomentumRateZDuringSwing.getValue()); balanceManager.setFinalTransferTime(finalTransferTime); - balanceManager.addFootstepToPlan(nextFootstep, footstepTiming); int stepsToAdd = Math.min(additionalFootstepsToConsider, walkingMessageHandler.getCurrentNumberOfFootsteps()); for (int i = 0; i < stepsToAdd; i++) @@ -311,27 +347,44 @@ public void onEntry() balanceManager.addFootstepToPlan(footsteps[i], footstepTimings[i]); } + walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); + failureDetectionControlModule.setNextFootstep(nextFootstep); + + feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); + + balanceManager.adjustFootstepInCoPPlan(nextFootstep); + + // FIXME I don't need to be computing this again + balanceManager.computeICPPlan(); + + updateHeightManager(); + } + + @Override + public void onEntry() + { + super.onEntry(); + + haveWeEntered = true; + + hasSwingFootTouchedDown.set(false); + hasTriggeredTouchdown.set(false); + + test(); + balanceManager.setICPPlanSupportSide(supportSide); balanceManager.initializeICPPlanForSingleSupport(); - /** This has to be called after calling initialize ICP Plan, as that resets the step constraints **/ - walkingMessageHandler.pollStepConstraints(stepConstraints); + if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) + walkingMessageHandler.pollStepConstraints(stepConstraints); balanceManager.setCurrentStepConstraints(stepConstraints); - updateHeightManager(); - feetManager.requestSwing(swingSide, nextFootstep, swingTime, balanceManager.getFinalDesiredCoMVelocity(), balanceManager.getFinalDesiredCoMAcceleration()); - if (feetManager.adjustHeightIfNeeded(nextFootstep)) - { - walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); - feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); - } - balanceManager.setSwingFootTrajectory(swingSide, feetManager.getSwingTrajectory(swingSide)); pelvisOrientationManager.initializeSwing(); @@ -346,6 +399,8 @@ public void onExit(double timeInState) { super.onExit(timeInState); + haveWeEntered = false; + triggerTouchdown(); } @@ -479,7 +534,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/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java index 4f1ccb859cba..686d7e6e7905 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.dyanmicsBasedFootstepGenerator.DynamicsBasedFootstepPlugin; 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, @@ -137,6 +140,17 @@ public ComponentBasedFootstepDataMessageGenerator buildPlugin(CommonHumanoidRefe ContinuousStepGenerator continuousStepGenerator = new ContinuousStepGenerator(registryField.get()); + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = new DynamicsBasedFootstepPlugin(robotModel, + referenceFrames, + updateDT, + yoGraphicsListRegistry, + timeProvider); + + registryField.get().addChild(dynamicsBasedFootstepPlugin.getRegistry()); + + continuousStepGenerator.setDynamicsBasedFootstepPlugin(dynamicsBasedFootstepPlugin); + + if (createSupportFootBasedFootstepAdjustment.hasValue() && createSupportFootBasedFootstepAdjustment.get()) continuousStepGenerator.setSupportFootBasedFootstepAdjustment(adjustPitchAndRoll.hasValue() && adjustPitchAndRoll.get()); if (primaryFootstepAdjusterField.hasValue() && primaryFootstepAdjusterField.get() != null) 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 e9b365897e99..3c79f4701ffd 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/JoystickBasedSteppingPlugin.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPlugin.java index bbb87ff065a9..0d7daf820a2a 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPlugin.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPlugin.java @@ -3,6 +3,7 @@ import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage; import us.ihmc.commonWalkingControlModules.controllers.Updatable; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.*; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator.DynamicsBasedFootstepPlugin; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; @@ -30,6 +31,7 @@ public JoystickBasedSteppingPlugin(ComponentBasedFootstepDataMessageGenerator st this.updatables = updatables; registry.addChild(stepGenerator.getRegistry()); registry.addChild(fastWalkingStepGenerator.getRegistry()); +// registry.addChild(dynamicsBasedFootstepPlugin.getRegistry()); } @Override 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 5edab54dc190..7fc9c777d7de 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 @@ -8,10 +8,13 @@ import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.FootstepAdjustment; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.FootstepPlanAdjustment; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.FootstepValidityIndicator; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator.DynamicsBasedFootstepPlugin; +import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator.DynamicsBasedFootstepPluginFactory; 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.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -25,6 +28,7 @@ public class JoystickBasedSteppingPluginFactory implements HumanoidSteppingPlugi { private final ComponentBasedFootstepDataMessageGeneratorFactory csgPluginFactory; private final VelocityBasedSteppingPluginFactory velocityPluginFactory; + private final DynamicsBasedFootstepPluginFactory dynamicsBasedFootstepPluginFactory; private final StepGeneratorCommandInputManager commandInputManager = new StepGeneratorCommandInputManager(); private final List updatables = new ArrayList<>(); private final List> planarRegionsListCommandConsumers = new ArrayList<>(); @@ -34,6 +38,7 @@ public JoystickBasedSteppingPluginFactory() { this.csgPluginFactory = new ComponentBasedFootstepDataMessageGeneratorFactory(); this.velocityPluginFactory = new VelocityBasedSteppingPluginFactory(); + this.dynamicsBasedFootstepPluginFactory = new DynamicsBasedFootstepPluginFactory(); // csgPluginFactory.setStepGeneratorCommandInputManager(commandInputManager); // velocityPluginFactory.setStepGeneratorCommandInputManager(commandInputManager); @@ -84,7 +89,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 +99,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 +108,8 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref yoGraphicsListRegistry, contactableFeet, timeProvider); - VelocityBasedSteppingPlugin fastWalkingPlugin = velocityPluginFactory.buildPlugin(referenceFrames, + VelocityBasedSteppingPlugin fastWalkingPlugin = velocityPluginFactory.buildPlugin(robotModel, + referenceFrames, updateDT, walkingControllerParameters, walkingStatusMessageOutputManager, @@ -110,17 +118,31 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref contactableFeet, timeProvider); +// DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = dynamicsBasedFootstepPluginFactory.buildPlugin(robotModel, +// referenceFrames, +// updateDT, +// walkingControllerParameters, +// walkingStatusMessageOutputManager, +// walkingCommandInputManager, +// yoGraphicsListRegistry, +// contactableFeet, +// timeProvider); + csgFootstepGenerator.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); csgFootstepGenerator.setDesiredTurningVelocityProvider(commandInputManager.createDesiredTurningVelocityProvider()); csgFootstepGenerator.setWalkInputProvider(commandInputManager.createWalkInputProvider()); csgFootstepGenerator.setSwingHeightInputProvider(commandInputManager.createSwingHeightProvider()); - fastWalkingPlugin.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); fastWalkingPlugin.setDesiredTurningVelocityProvider(commandInputManager.createDesiredTurningVelocityProvider()); fastWalkingPlugin.setWalkInputProvider(commandInputManager.createWalkInputProvider()); fastWalkingPlugin.setSwingHeightInputProvider(commandInputManager.createSwingHeightProvider()); +// dynamicsBasedFootstepPlugin.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); +// dynamicsBasedFootstepPlugin.setDesiredTurningVelocityProvider(commandInputManager.createDesiredTurningVelocityProvider()); +// dynamicsBasedFootstepPlugin.setWalkInputProvider(commandInputManager.createWalkInputProvider()); +// dynamicsBasedFootstepPlugin.setSwingHeightInputProvider(commandInputManager.createSwingHeightProvider()); + walkingStatusMessageOutputManager.attachStatusMessageListener(HighLevelStateChangeStatusMessage.class, commandInputManager::setHighLevelStateChangeStatusMessage); walkingStatusMessageOutputManager.attachStatusMessageListener(WalkingStatusMessage.class, @@ -135,7 +157,9 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref for (Consumer planarRegionsListCommandConsumer : planarRegionsListCommandConsumers) commandInputManager.addPlanarRegionsListCommandConsumer(planarRegionsListCommandConsumer); - JoystickBasedSteppingPlugin joystickBasedSteppingPlugin = new JoystickBasedSteppingPlugin(csgFootstepGenerator, fastWalkingPlugin, updatables); + JoystickBasedSteppingPlugin joystickBasedSteppingPlugin = new JoystickBasedSteppingPlugin(csgFootstepGenerator, + fastWalkingPlugin, + updatables); joystickBasedSteppingPlugin.setHighLevelStateChangeStatusListener(walkingStatusMessageOutputManager); return joystickBasedSteppingPlugin; 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 8a4029260572..688f7e5c63e8 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, 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 63521e71e4f8..94d34da1a592 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> listenerList = new ArrayList<>(); + public WalkingMessageHandler(double defaultTransferTime, double defaultSwingTime, double defaultInitialTransferTime, @@ -313,9 +317,22 @@ else if (upcomingFootsteps.isEmpty()) checkForPause(); + for (int i = 0; i < listenerList.size(); i++) + listenerList.get(i).doListenerAction(); + updateVisualization(); } + public void addListener(Listener listener) + { + listenerList.add(listener); + } + + public interface Listener> + { + public abstract void doListenerAction(); + } + public void handlePauseWalkingCommand(PauseWalkingCommand command) { if (!command.isPauseRequested() && isWalkingPaused.getValue()) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/HighLevelHumanoidControllerToolbox.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/HighLevelHumanoidControllerToolbox.java index 1fdd36e31029..710df07dd19c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/HighLevelHumanoidControllerToolbox.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/HighLevelHumanoidControllerToolbox.java @@ -70,6 +70,7 @@ 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.providers.BooleanProvider; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -182,6 +183,8 @@ public class HighLevelHumanoidControllerToolbox implements CenterOfMassStateProv private final YoBoolean controllerFailed = new YoBoolean("controllerFailed", registry); + private final YoBoolean alterEndConditionOfTransfer = new YoBoolean("alterEndConditionOfTransfer", registry); + public HighLevelHumanoidControllerToolbox(FullHumanoidRobotModel fullRobotModel, CenterOfMassStateProvider centerOfMassStateProvider, CommonHumanoidReferenceFrames referenceFrames, @@ -916,6 +919,11 @@ public YoBoolean getControllerFailedBoolean() return controllerFailed; } + public BooleanProvider getAlterEndConditionOfTransfer() + { + return alterEndConditionOfTransfer; + } + public void attachControllerFailureListener(ControllerFailureListener listener) { this.controllerFailureListeners.add(listener);