From b44c82009de5c305a5d97ab48b938bcc8f17bd29 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 25 Oct 2024 10:17:58 -0500 Subject: [PATCH 01/10] made some classes for QFP --- .../DynamicsBasedFootstepManager.java | 44 ++ ...amicsBasedFootstepTouchdownCalculator.java | 579 ++++++++++++++++++ 2 files changed, 623 insertions(+) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepTouchdownCalculator.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java new file mode 100644 index 00000000000..084ef9ea969 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java @@ -0,0 +1,44 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataListCommand; +import us.ihmc.humanoidRobotics.footstep.Footstep; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoEnum; + +public class DynamicsBasedFootstepManager +{ + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + private final DynamicsBasedFootstepTouchdownCalculator touchdownCalculator; + + private final FootstepDataListCommand command = new FootstepDataListCommand(); + private final FootstepDataCommand nextFootstep = new FootstepDataCommand(); + + private enum FootstepProviderMode {CSG, DB}; + private final YoEnum footstepProviderModeYoEnum = new YoEnum<>("footstepProviderMode", registry, FootstepProviderMode.class); + + public DynamicsBasedFootstepManager(YoRegistry parentRegistry) + { + touchdownCalculator = new DynamicsBasedFootstepTouchdownCalculator(); + + parentRegistry.addChild(registry); + } + + public FootstepDataListCommand update(FootstepDataListCommand otherCommand) + { + command.clear(); + + //nextFootstep.set(); + + command.addFootstep(nextFootstep); + + for (int i = 1; i < otherCommand.getNumberOfFootsteps(); i++) + { + FootstepDataCommand footstep = otherCommand.getFootstep(i); + command.addFootstep(footstep); + } + + return command; + } +} \ No newline at end of file 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 00000000000..2b992eb0eaa --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepTouchdownCalculator.java @@ -0,0 +1,579 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; + +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.mecano.frames.MovingReferenceFrame; +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.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 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 YoFrameVector2D predictedVelocityAtTouchdown; + + // This is the desired touchdown position to be used by the robot + private final YoFramePoint3D desiredTouchdownPosition; + private final YoFramePoint2D desiredStanceAtTouchdown; + 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 HighLevelHumanoidControllerToolbox controllerToolbox; + private final SideDependentList soleFrames; + private final Vector2DReadOnly desiredVelocityProvider; + private final FastWalkingParameters fastWalkingParameters; + private final YoFastWalkingConstantParametersForStep fastWalkingParametersForStep; + + // Temp value used for calculation + private final FrameVector2D tempVector = 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 timeToReachGoal; + private final RobotSide robotSide; + + private final double mass; + private final double gravity; + + public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, + MovingReferenceFrame centerOfMassControlZUpFrame, + FrameVector3DReadOnly centerOfMassVelocity, + FrameVector3DReadOnly centroidalAngularMomentum, + HighLevelHumanoidControllerToolbox controllerToolbox, + FastWalkingParameters fastWalkingParameters, + YoFastWalkingConstantParametersForStep fastWalkingParametersForStep, + Vector2DReadOnly desiredVelocityProvider, + YoRegistry parentRegistry) + { + this.robotSide = robotSide; + this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; + this.centerOfMassVelocity = centerOfMassVelocity; + this.centroidalAngularMomentum = centroidalAngularMomentum; + this.controllerToolbox = controllerToolbox; + this.fastWalkingParameters = fastWalkingParameters; + this.fastWalkingParametersForStep = fastWalkingParametersForStep; + this.desiredVelocityProvider = desiredVelocityProvider; + soleFrames = controllerToolbox.getFullRobotModel().getSoleFrames(); + mass = controllerToolbox.getTotalMass(); + gravity = controllerToolbox.getGravityZ(); + + 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 + desiredTouchdownPosition = new YoFramePoint3D(prefix + "DesiredTouchdownPosition", centerOfMassControlZUpFrame, registry); + desiredTouchdownPosition2D = new FramePoint2D(centerOfMassControlZUpFrame); + predictedVelocityAtTouchdown = new YoFrameVector2D(prefix + "PredictedVelocityAtTouchdown", centerOfMassControlZUpFrame, registry); + desiredStanceAtTouchdown = new YoFramePoint2D(prefix + "DesiredStanceAtTouchdown", centerOfMassControlZUpFrame, 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 = controllerToolbox.getOmega0Provider(); + omegaY = controllerToolbox.getOmega0Provider(); + + capturePointPole = fastWalkingParametersForStep.getCapturePointPole(); + + desiredCoMHeight = fastWalkingParametersForStep.getDesiredCoMHeight(); + swingDuration = fastWalkingParametersForStep.getSwingDuration(); + + omegaX.addListener(v -> computeCapturePointTimeConstant()); + omegaY.addListener(v -> computeCapturePointTimeConstant()); + capturePointPole.addListener(v -> computeCapturePointTimeConstant()); + + desiredCoMHeight.addListener(v -> computeCapturePointTimeConstant()); + swingDuration.addListener(v -> computeCapturePointTimeConstant()); + fastWalkingParametersForStep.getDoubleSupportFraction().addListener(v -> computeCapturePointTimeConstant()); + + computeCapturePointTimeConstant(); + + parentRegistry.addChild(registry); + } + + public void reset() + { + desiredALIPTouchdownPositionWithoutDS.setToNaN(); + desiredALIPTouchdownPositionWithDS.setToNaN(); + desiredTouchdownPosition.setToNaN(); + desiredStanceAtTouchdown.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 leavingDoubleSupport) + { + if (pendulumBase != null) + pendulumBase.checkReferenceFrameMatch(centerOfMassControlZUpFrame); + + this.timeToReachGoal.set(timeToReachGoal); + + // compute all the touchdown positions and their offset values + computeDesiredALIPTouchdownPosition(timeToReachGoal, pendulumBase); + computeDesiredALIPTouchdownPositionWithDS(timeToReachGoal, pendulumBase, netPendulumBase); + + computeDesiredStancePositionAtEndOfState(supportSide, timeToReachGoal, pendulumBase); + + desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithDS); + predictedVelocityAtTouchdown.set(predictedALIPVelocityAtTouchdownWithDS); + + // 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 + FastWalkingGroundPlaneEstimator groundPlaneEstimator = controllerToolbox.getGroundPlaneEstimator(); + desiredTouchdownPosition.setMatchingFrame(groundPlaneEstimator.getGroundPosition(desiredTouchdownPosition2D)); + } + + public FramePoint2DReadOnly getStancePositionAtTouchdown() + { + return desiredStanceAtTouchdown; + } + + public FramePoint3DReadOnly getDesiredTouchdownPosition() + { + return desiredTouchdownPosition; + } + + public FrameVector2DReadOnly getPredictedVelocityAtTouchdown() + { + return predictedVelocityAtTouchdown; + } + + private final FrameVector2D angularMomentum = new FrameVector2D(); + + private void computeDesiredStancePositionAtEndOfState(RobotSide supportSide, double timeToReachGoal, FramePoint2DReadOnly pendulumBase) + { + + // compute the current center of mass velocity + tempVector.setIncludingFrame(centerOfMassVelocity); + tempVector.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + tempVector.scale(1.0 / omegaX.getDoubleValue(), 1.0 / omegaY.getDoubleValue()); + + 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 && Double.isFinite(timeToReachGoal)) + { + double omegaTX = omegaX.getDoubleValue() * timeToReachGoal; + double omegaTY = omegaY.getDoubleValue() * timeToReachGoal; + double coshX = Math.cosh(omegaTX); + double sinhX = Math.sinh(omegaTX); + double coshY = Math.cosh(omegaTY); + double sinhY = Math.sinh(omegaTY); + double heightX = gravity / (omegaX.getDoubleValue() * omegaX.getDoubleValue()); + double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); + double mhXW = mass * heightX * omegaX.getValue(); + double mhYW = mass * heightY * omegaY.getValue(); + + desiredStanceAtTouchdown.set(sinhX * tempVector.getX(), sinhY * tempVector.getY()); + desiredStanceAtTouchdown.add(sinhX / mhXW * angularMomentum.getY(), -sinhY / mhYW * angularMomentum.getX()); + desiredStanceAtTouchdown.add(-coshX * pendulumBase.getX(), -coshY * pendulumBase.getY()); + desiredStanceAtTouchdown.negate(); + } + else + { + desiredStanceAtTouchdown.set(estimates.getFootPosition(supportSide)); + } + } + + private void computeDesiredALIPTouchdownPosition(double timeToReachGoal, FramePoint2DReadOnly pendulumBase) + { + + // compute the current center of mass velocity + tempVector.setIncludingFrame(centerOfMassVelocity); + tempVector.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + + 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 && 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) + { + + // 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 = fastWalkingParametersForStep.getSwingDuration().getDoubleValue() * fastWalkingParametersForStep.getDoubleSupportFraction().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 (controllerToolbox.getCommonVariables().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 computeDesiredTouchdownOffsetForStanceWidth(RobotSide supportSide) + { + // this is the desired offset distance to make the robot stably walk in place. + touchdownPositionOffsetForDesiredStanceWidth.set(computeDesiredTouchdownOffsetForStanceWidth(fastWalkingParameters, + fastWalkingParametersForStep, + supportSide, + 1/capturePointTimeConstantY.getDoubleValue())); + } + + private void computeDesiredTouchdownOffsetForVelocity(RobotSide supportSide) + { + // this is the desired offset distance to make the robot stably walk at the desired velocity + computeDesiredTouchdownOffsetForVelocity(fastWalkingParametersForStep, + supportSide, + 1/capturePointTimeConstantX.getDoubleValue(), + 1/capturePointTimeConstantY.getDoubleValue(), + desiredVelocityProvider, + touchdownPositionOffsetForDesiredSpeed); + } + + 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 = fastWalkingParametersForStep.getSwingDuration().getDoubleValue(); + double doubleSupportDuration = singleSupportDuration * fastWalkingParametersForStep.getDoubleSupportFraction().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 (fastWalkingParameters.getFastWalkingGaitParameters().getTouchdownCalculatorTypeToUse() == TouchdownType.ALIP_DS && 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); + } + } + + public static double computeDesiredTouchdownOffsetForStanceWidth(FastWalkingParameters fastWalkingParameters, + YoFastWalkingConstantParametersForStep fastWalkingParametersForStep, + RobotSide supportSide, + double omega) + { + double swingDuration = fastWalkingParametersForStep.getSwingDuration().getDoubleValue(); + double doubleSupportDuration = fastWalkingParametersForStep.getDoubleSupportFraction().getDoubleValue() * swingDuration; + + return computeDesiredTouchdownOffsetForStanceWidth(fastWalkingParameters, swingDuration, doubleSupportDuration, supportSide, omega); + } + + public static double computeDesiredTouchdownOffsetForStanceWidth(FastWalkingParameters fastWalkingParameters, + double swingDuration, + double doubleSupportDuration, + 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 * fastWalkingParameters.getStanceWidth() / (1.0 + Math.exp(omega * (swingDuration + doubleSupportDuration))); + return supportSide.negateIfLeftSide(stepWidthOffset); + } + + public static void computeDesiredTouchdownOffsetForVelocity(YoFastWalkingConstantParametersForStep fastWalkingParametersForStep, + RobotSide supportSide, + double omegaX, + double omegaY, + Vector2DReadOnly desiredVelocity, + FixedFrameVector2DBasics touchdownPositionOffsetToPack) + { + double swingDuration = fastWalkingParametersForStep.getSwingDuration().getDoubleValue(); + double doubleSupportDuration = fastWalkingParametersForStep.getDoubleSupportFraction().getDoubleValue() * swingDuration; + + computeDesiredTouchdownOffsetForVelocity(swingDuration, + doubleSupportDuration, + supportSide, + omegaX, + omegaY, + desiredVelocity, + touchdownPositionOffsetToPack); + } + + 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; + } +} From c65f331ccd06b6b137d09f39e4bd91da0208d6ed Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 31 Oct 2024 17:34:55 -0500 Subject: [PATCH 02/10] setup quickster touchdown calculator/footstep provider in step generator thread --- .../DynamicsBasedFootstepManager.java | 44 --- ...amicsBasedFootstepTouchdownCalculator.java | 253 +++++------------- .../plugin/JoystickBasedSteppingPlugin.java | 3 + .../JoystickBasedSteppingPluginFactory.java | 24 +- 4 files changed, 93 insertions(+), 231 deletions(-) delete mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java deleted file mode 100644 index 084ef9ea969..00000000000 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepManager.java +++ /dev/null @@ -1,44 +0,0 @@ -package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; - -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataListCommand; -import us.ihmc.humanoidRobotics.footstep.Footstep; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoEnum; - -public class DynamicsBasedFootstepManager -{ - private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); - - private final DynamicsBasedFootstepTouchdownCalculator touchdownCalculator; - - private final FootstepDataListCommand command = new FootstepDataListCommand(); - private final FootstepDataCommand nextFootstep = new FootstepDataCommand(); - - private enum FootstepProviderMode {CSG, DB}; - private final YoEnum footstepProviderModeYoEnum = new YoEnum<>("footstepProviderMode", registry, FootstepProviderMode.class); - - public DynamicsBasedFootstepManager(YoRegistry parentRegistry) - { - touchdownCalculator = new DynamicsBasedFootstepTouchdownCalculator(); - - parentRegistry.addChild(registry); - } - - public FootstepDataListCommand update(FootstepDataListCommand otherCommand) - { - command.clear(); - - //nextFootstep.set(); - - command.addFootstep(nextFootstep); - - for (int i = 1; i < otherCommand.getNumberOfFootsteps(); i++) - { - FootstepDataCommand footstep = otherCommand.getFootstep(i); - command.addFootstep(footstep); - } - - return command; - } -} \ No newline at end of file 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 index 2b992eb0eaa..ad70b7ce966 100644 --- 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 @@ -1,12 +1,11 @@ package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; -import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; - import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.FrameVector2D; 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; @@ -24,6 +23,8 @@ public class DynamicsBasedFootstepTouchdownCalculator 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. @@ -40,7 +41,6 @@ public class DynamicsBasedFootstepTouchdownCalculator // This is the desired touchdown position to be used by the robot private final YoFramePoint3D desiredTouchdownPosition; - private final YoFramePoint2D desiredStanceAtTouchdown; 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 @@ -49,15 +49,15 @@ public class DynamicsBasedFootstepTouchdownCalculator // 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 HighLevelHumanoidControllerToolbox controllerToolbox; private final SideDependentList soleFrames; private final Vector2DReadOnly desiredVelocityProvider; - private final FastWalkingParameters fastWalkingParameters; - private final YoFastWalkingConstantParametersForStep fastWalkingParametersForStep; // 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; @@ -74,6 +74,8 @@ public class DynamicsBasedFootstepTouchdownCalculator private final YoDouble desiredCoMHeight; private final YoDouble swingDuration; + private final YoDouble doubleSupportFraction; + private final YoDouble stanceWidth; private final YoDouble timeToReachGoal; private final RobotSide robotSide; @@ -85,9 +87,10 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, MovingReferenceFrame centerOfMassControlZUpFrame, FrameVector3DReadOnly centerOfMassVelocity, FrameVector3DReadOnly centroidalAngularMomentum, - HighLevelHumanoidControllerToolbox controllerToolbox, - FastWalkingParameters fastWalkingParameters, - YoFastWalkingConstantParametersForStep fastWalkingParametersForStep, + FullHumanoidRobotModel robotModel, + FootSoleBasedGroundPlaneEstimator groundPlaneEstimator, + DynamicsBasedFootstepParameters parameters, + double gravity, Vector2DReadOnly desiredVelocityProvider, YoRegistry parentRegistry) { @@ -95,13 +98,11 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; this.centerOfMassVelocity = centerOfMassVelocity; this.centroidalAngularMomentum = centroidalAngularMomentum; - this.controllerToolbox = controllerToolbox; - this.fastWalkingParameters = fastWalkingParameters; - this.fastWalkingParametersForStep = fastWalkingParametersForStep; + this.groundPlaneEstimator = groundPlaneEstimator; this.desiredVelocityProvider = desiredVelocityProvider; - soleFrames = controllerToolbox.getFullRobotModel().getSoleFrames(); - mass = controllerToolbox.getTotalMass(); - gravity = controllerToolbox.getGravityZ(); + soleFrames = robotModel.getSoleFrames(); + mass = robotModel.getTotalMass(); + this.gravity = gravity; YoRegistry registry = new YoRegistry(robotSide.getCamelCaseName() + getClass().getSimpleName()); @@ -126,7 +127,6 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, desiredTouchdownPosition = new YoFramePoint3D(prefix + "DesiredTouchdownPosition", centerOfMassControlZUpFrame, registry); desiredTouchdownPosition2D = new FramePoint2D(centerOfMassControlZUpFrame); predictedVelocityAtTouchdown = new YoFrameVector2D(prefix + "PredictedVelocityAtTouchdown", centerOfMassControlZUpFrame, registry); - desiredStanceAtTouchdown = new YoFramePoint2D(prefix + "DesiredStanceAtTouchdown", centerOfMassControlZUpFrame, registry); // Step position offsets for desired walking speed and stance width touchdownPositionOffsetForDesiredStanceWidth = new YoDouble(prefix + "TouchdownPositionOffsetForDesiredStanceWidth", registry); @@ -139,13 +139,15 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, capturePointTimeConstantXWithoutDS = new YoDouble(prefix + "CapturePointTimeConstantXWithoutDS", registry); capturePointTimeConstantYWithoutDS = new YoDouble(prefix + "CapturePointTimeConstantYWithoutDS", registry); - omegaX = controllerToolbox.getOmega0Provider(); - omegaY = controllerToolbox.getOmega0Provider(); + omegaX = parameters.getOmega(robotSide); + omegaY = parameters.getOmega(robotSide); - capturePointPole = fastWalkingParametersForStep.getCapturePointPole(); + capturePointPole = parameters.getPole(robotSide); - desiredCoMHeight = fastWalkingParametersForStep.getDesiredCoMHeight(); - swingDuration = fastWalkingParametersForStep.getSwingDuration(); + desiredCoMHeight = parameters.getDesiredCoMHeight(robotSide); + swingDuration = parameters.getSwingDuration(robotSide); + doubleSupportFraction = parameters.getDoubleSupportFraction(robotSide); + stanceWidth = parameters.getStanceWidth(robotSide); omegaX.addListener(v -> computeCapturePointTimeConstant()); omegaY.addListener(v -> computeCapturePointTimeConstant()); @@ -153,7 +155,7 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, desiredCoMHeight.addListener(v -> computeCapturePointTimeConstant()); swingDuration.addListener(v -> computeCapturePointTimeConstant()); - fastWalkingParametersForStep.getDoubleSupportFraction().addListener(v -> computeCapturePointTimeConstant()); + doubleSupportFraction.addListener(v -> computeCapturePointTimeConstant()); computeCapturePointTimeConstant(); @@ -165,7 +167,6 @@ public void reset() desiredALIPTouchdownPositionWithoutDS.setToNaN(); desiredALIPTouchdownPositionWithDS.setToNaN(); desiredTouchdownPosition.setToNaN(); - desiredStanceAtTouchdown.setToNaN(); isInitialStanceValid.set(false); } @@ -176,7 +177,7 @@ public void initializeForContactChange(RobotSide supportSide) stanceFootPositionAtBeginningOfState.setFromReferenceFrame(soleFrames.get(supportSide)); } - public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeToReachGoal, FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean leavingDoubleSupport) + public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeToReachGoal, FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean isInDoubleSupport) { if (pendulumBase != null) pendulumBase.checkReferenceFrameMatch(centerOfMassControlZUpFrame); @@ -184,10 +185,7 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo this.timeToReachGoal.set(timeToReachGoal); // compute all the touchdown positions and their offset values - computeDesiredALIPTouchdownPosition(timeToReachGoal, pendulumBase); - computeDesiredALIPTouchdownPositionWithDS(timeToReachGoal, pendulumBase, netPendulumBase); - - computeDesiredStancePositionAtEndOfState(supportSide, timeToReachGoal, pendulumBase); + computeDesiredALIPTouchdownPositionWithDS(timeToReachGoal, pendulumBase, netPendulumBase, isInDoubleSupport); desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithDS); predictedVelocityAtTouchdown.set(predictedALIPVelocityAtTouchdownWithDS); @@ -203,19 +201,13 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo desiredTouchdownPosition2D.addY(touchdownPositionOffsetForDesiredStanceWidth.getValue()); // add extra offsets for fixing drifting - desiredTouchdownPosition2D.addX(fastWalkingParameters.getFixedForwardOffset()); - desiredTouchdownPosition2D.addY(supportSide.negateIfLeftSide(fastWalkingParameters.getFixedWidthOffset())); +// desiredTouchdownPosition2D.addX(fastWalkingParameters.getFixedForwardOffset()); +// desiredTouchdownPosition2D.addY(supportSide.negateIfLeftSide(fastWalkingParameters.getFixedWidthOffset())); // determine touchdown z position from x position, y position, and ground plane - FastWalkingGroundPlaneEstimator groundPlaneEstimator = controllerToolbox.getGroundPlaneEstimator(); desiredTouchdownPosition.setMatchingFrame(groundPlaneEstimator.getGroundPosition(desiredTouchdownPosition2D)); } - public FramePoint2DReadOnly getStancePositionAtTouchdown() - { - return desiredStanceAtTouchdown; - } - public FramePoint3DReadOnly getDesiredTouchdownPosition() { return desiredTouchdownPosition; @@ -226,88 +218,7 @@ public FrameVector2DReadOnly getPredictedVelocityAtTouchdown() return predictedVelocityAtTouchdown; } - private final FrameVector2D angularMomentum = new FrameVector2D(); - - private void computeDesiredStancePositionAtEndOfState(RobotSide supportSide, double timeToReachGoal, FramePoint2DReadOnly pendulumBase) - { - - // compute the current center of mass velocity - tempVector.setIncludingFrame(centerOfMassVelocity); - tempVector.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - tempVector.scale(1.0 / omegaX.getDoubleValue(), 1.0 / omegaY.getDoubleValue()); - - 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 && Double.isFinite(timeToReachGoal)) - { - double omegaTX = omegaX.getDoubleValue() * timeToReachGoal; - double omegaTY = omegaY.getDoubleValue() * timeToReachGoal; - double coshX = Math.cosh(omegaTX); - double sinhX = Math.sinh(omegaTX); - double coshY = Math.cosh(omegaTY); - double sinhY = Math.sinh(omegaTY); - double heightX = gravity / (omegaX.getDoubleValue() * omegaX.getDoubleValue()); - double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); - double mhXW = mass * heightX * omegaX.getValue(); - double mhYW = mass * heightY * omegaY.getValue(); - - desiredStanceAtTouchdown.set(sinhX * tempVector.getX(), sinhY * tempVector.getY()); - desiredStanceAtTouchdown.add(sinhX / mhXW * angularMomentum.getY(), -sinhY / mhYW * angularMomentum.getX()); - desiredStanceAtTouchdown.add(-coshX * pendulumBase.getX(), -coshY * pendulumBase.getY()); - desiredStanceAtTouchdown.negate(); - } - else - { - desiredStanceAtTouchdown.set(estimates.getFootPosition(supportSide)); - } - } - - private void computeDesiredALIPTouchdownPosition(double timeToReachGoal, FramePoint2DReadOnly pendulumBase) - { - - // compute the current center of mass velocity - tempVector.setIncludingFrame(centerOfMassVelocity); - tempVector.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); - - 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 && 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) + private void computeDesiredALIPTouchdownPositionWithDS(double timeToReachGoal, FramePoint2DReadOnly pendulumBase, FramePoint2DReadOnly netPendulumBase, boolean isInDoubleSupport) { // Get CoM velocity and change frame to CoM control frame @@ -325,7 +236,7 @@ private void computeDesiredALIPTouchdownPositionWithDS(double timeToReachGoal, F double heightY = gravity / (omegaY.getDoubleValue() * omegaY.getDoubleValue()); double mhX = mass * heightX; double mhY = mass * heightY; - double doubleSupportDuration = fastWalkingParametersForStep.getSwingDuration().getDoubleValue() * fastWalkingParametersForStep.getDoubleSupportFraction().getDoubleValue(); + double doubleSupportDuration = swingDuration.getDoubleValue() * doubleSupportFraction.getDoubleValue(); double B2X; double B2Y; double C2X; @@ -334,7 +245,7 @@ private void computeDesiredALIPTouchdownPositionWithDS(double timeToReachGoal, F // Prediction horizons to project/predict state at end of single and double support double singleSupportHorizon; double doubleSupportHorizon; - if (controllerToolbox.getCommonVariables().isInDoubleSupport()) + if (isInDoubleSupport) { singleSupportHorizon = 0.0; doubleSupportHorizon = timeToReachGoal; @@ -414,26 +325,6 @@ private void computeDesiredALIPTouchdownPositionWithDS(double timeToReachGoal, F capturePointTimeConstantY.getDoubleValue() * predictedALIPVelocityAtTouchdownWithDS.getY()); } - private void computeDesiredTouchdownOffsetForStanceWidth(RobotSide supportSide) - { - // this is the desired offset distance to make the robot stably walk in place. - touchdownPositionOffsetForDesiredStanceWidth.set(computeDesiredTouchdownOffsetForStanceWidth(fastWalkingParameters, - fastWalkingParametersForStep, - supportSide, - 1/capturePointTimeConstantY.getDoubleValue())); - } - - private void computeDesiredTouchdownOffsetForVelocity(RobotSide supportSide) - { - // this is the desired offset distance to make the robot stably walk at the desired velocity - computeDesiredTouchdownOffsetForVelocity(fastWalkingParametersForStep, - supportSide, - 1/capturePointTimeConstantX.getDoubleValue(), - 1/capturePointTimeConstantY.getDoubleValue(), - desiredVelocityProvider, - touchdownPositionOffsetForDesiredSpeed); - } - private void computeCapturePointTimeConstant() { double omegaX = this.omegaX.getDoubleValue(); @@ -442,8 +333,8 @@ private void computeCapturePointTimeConstant() double heightY = gravity / (omegaY * omegaY); double m = mass; double lambda = capturePointPole.getDoubleValue(); - double singleSupportDuration = fastWalkingParametersForStep.getSwingDuration().getDoubleValue(); - double doubleSupportDuration = singleSupportDuration * fastWalkingParametersForStep.getDoubleSupportFraction().getDoubleValue(); + double singleSupportDuration = swingDuration.getDoubleValue(); + double doubleSupportDuration = singleSupportDuration * doubleSupportFraction.getDoubleValue(); double coshXDS = Math.cosh(omegaX * doubleSupportDuration); double coshYDS = Math.cosh(omegaY * doubleSupportDuration); @@ -461,7 +352,7 @@ private void computeCapturePointTimeConstant() capturePointTimeConstantXWithoutDS.set(timeConstantXNoDS); capturePointTimeConstantYWithoutDS.set(timeConstantYNoDS); - if (fastWalkingParameters.getFastWalkingGaitParameters().getTouchdownCalculatorTypeToUse() == TouchdownType.ALIP_DS && doubleSupportDuration > 0) + if (doubleSupportDuration > 0) { timeConstantXWithDS = (doubleSupportDuration * (coshXDS * coshXSS - lambda + sinhXDS * sinhXSS)) / (coshXDS * lambda - lambda + doubleSupportDuration * coshXDS * omegaX * sinhXSS + doubleSupportDuration * coshXSS * omegaX * sinhXDS); @@ -479,50 +370,16 @@ private void computeCapturePointTimeConstant() } } - public static double computeDesiredTouchdownOffsetForStanceWidth(FastWalkingParameters fastWalkingParameters, - YoFastWalkingConstantParametersForStep fastWalkingParametersForStep, - RobotSide supportSide, - double omega) - { - double swingDuration = fastWalkingParametersForStep.getSwingDuration().getDoubleValue(); - double doubleSupportDuration = fastWalkingParametersForStep.getDoubleSupportFraction().getDoubleValue() * swingDuration; - - return computeDesiredTouchdownOffsetForStanceWidth(fastWalkingParameters, swingDuration, doubleSupportDuration, supportSide, omega); - } - - public static double computeDesiredTouchdownOffsetForStanceWidth(FastWalkingParameters fastWalkingParameters, - double swingDuration, - double doubleSupportDuration, - 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 * fastWalkingParameters.getStanceWidth() / (1.0 + Math.exp(omega * (swingDuration + doubleSupportDuration))); - return supportSide.negateIfLeftSide(stepWidthOffset); - } - - public static void computeDesiredTouchdownOffsetForVelocity(YoFastWalkingConstantParametersForStep fastWalkingParametersForStep, - RobotSide supportSide, - double omegaX, - double omegaY, - Vector2DReadOnly desiredVelocity, - FixedFrameVector2DBasics touchdownPositionOffsetToPack) + private void computeDesiredTouchdownOffsetForVelocity(RobotSide supportSide) { - double swingDuration = fastWalkingParametersForStep.getSwingDuration().getDoubleValue(); - double doubleSupportDuration = fastWalkingParametersForStep.getDoubleSupportFraction().getDoubleValue() * swingDuration; - - computeDesiredTouchdownOffsetForVelocity(swingDuration, - doubleSupportDuration, + // this is the desired offset distance to make the robot stably walk at the desired velocity + computeDesiredTouchdownOffsetForVelocity(swingDuration.getDoubleValue(), + doubleSupportFraction.getDoubleValue() * swingDuration.getDoubleValue(), supportSide, - omegaX, - omegaY, - desiredVelocity, - touchdownPositionOffsetToPack); + 1/capturePointTimeConstantX.getDoubleValue(), + 1/capturePointTimeConstantY.getDoubleValue(), + desiredVelocityProvider, + touchdownPositionOffsetForDesiredSpeed); } public static void computeDesiredTouchdownOffsetForVelocity(double swingDuration, @@ -576,4 +433,30 @@ else if (desiredVelocity < 0.0 && supportSide == RobotSide.RIGHT) 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/highLevelHumanoidControl/plugin/JoystickBasedSteppingPlugin.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPlugin.java index bbb87ff065a..734168aa7fc 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; @@ -23,6 +24,7 @@ public class JoystickBasedSteppingPlugin implements HumanoidSteppingPlugin public JoystickBasedSteppingPlugin(ComponentBasedFootstepDataMessageGenerator stepGenerator, VelocityBasedSteppingPlugin fastWalkingStepGenerator, + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin, List updatables) { this.stepGenerator = stepGenerator; @@ -30,6 +32,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 5edab54dc19..02688d493ea 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,6 +8,8 @@ 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; @@ -25,6 +27,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 +37,7 @@ public JoystickBasedSteppingPluginFactory() { this.csgPluginFactory = new ComponentBasedFootstepDataMessageGeneratorFactory(); this.velocityPluginFactory = new VelocityBasedSteppingPluginFactory(); + this.dynamicsBasedFootstepPluginFactory = new DynamicsBasedFootstepPluginFactory(); // csgPluginFactory.setStepGeneratorCommandInputManager(commandInputManager); // velocityPluginFactory.setStepGeneratorCommandInputManager(commandInputManager); @@ -110,17 +114,30 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref contactableFeet, timeProvider); + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = dynamicsBasedFootstepPluginFactory.buildPlugin(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 +152,10 @@ 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, + dynamicsBasedFootstepPlugin, + updatables); joystickBasedSteppingPlugin.setHighLevelStateChangeStatusListener(walkingStatusMessageOutputManager); return joystickBasedSteppingPlugin; From 3ffeb22636297b8bf86f63b4a9d532df471c9aca Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 31 Oct 2024 17:36:06 -0500 Subject: [PATCH 03/10] setup quickster touchdown calculator/footstep provider in step generator thread --- .../DynamicsBasedFootstepParameters.java | 164 ++++++++ .../DynamicsBasedFootstepPlugin.java | 366 ++++++++++++++++++ .../DynamicsBasedFootstepPluginFactory.java | 118 ++++++ .../FootSoleBasedGroundPlaneEstimator.java | 190 +++++++++ 4 files changed, 838 insertions(+) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepParameters.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPluginFactory.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/FootSoleBasedGroundPlaneEstimator.java 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 00000000000..b5adf6e096b --- /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.8; + 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"; + + 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, doubleSupportFraction, 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)); + + stageableYoDoubles.put(robotSide, new ArrayList<>()); + } + + 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 00000000000..f73f05c5d0c --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java @@ -0,0 +1,366 @@ +package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; + +import controller_msgs.msg.dds.FootstepDataListMessage; +import controller_msgs.msg.dds.FootstepDataMessage; +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.commons.lists.RecyclingArrayList; +import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataListCommand; +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.MovingZUpFrame; +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 MovingReferenceFrame centerOfMassControlFrame; + private final MovingZUpFrame centerOfMassControlZUpFrame; + private final ReferenceFrame centerOfMassFrame; + private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; + + // Command/Desired output related stuff + private final SideDependentList touchdownCalculator = new SideDependentList<>(); + private final FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + private final RecyclingArrayList footsteps = footstepDataListMessage.getFootstepDataList(); + + // 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 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 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; + + public DynamicsBasedFootstepPlugin(FullHumanoidRobotModel robotModel, double updateDT, CommonHumanoidReferenceFrames referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry) + { + this.updateDT = updateDT; + this.referenceFrames = referenceFrames; + + parameters = new DynamicsBasedFootstepParameters(gravity, registry); + + centerOfMassFrame = referenceFrames.getCenterOfMassFrame(); + + 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((FrameQuaternionReadOnly) desiredPelvisOrientation); + } + }; + + centerOfMassControlZUpFrame = new MovingZUpFrame(centerOfMassControlFrame, "centerOfMassControlZUpFrame" + variableNameSuffix); + + groundPlaneEstimator = new FootSoleBasedGroundPlaneEstimator(centerOfMassControlZUpFrame, robotModel, yoGraphicsListRegistry, registry); + + for (RobotSide robotSide : RobotSide.values) + { + footStates.put(robotSide, new YoEnum<>("footStates" + variableNameSuffix, registry, FootState.class)); + touchdownCalculator = new DynamicsBasedFootstepTouchdownCalculator(robotSide, centerOfMassControlZUpFrame, getCenterOfMassVelocity(), , robotModel, groundPlaneEstimator, parameters, gravity, , registry); + } + } + + @Override + public void update(double time) + { + groundPlaneEstimator.update(); + + currentCoMPosition.setFromReferenceFrame(centerOfMassFrame); + + // TODO maybe wrong? + desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); + desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); + + centerOfMassControlFrame.update(); + centerOfMassControlZUpFrame.update(); + + currentCoMVelocity.setMatchingFrame(controllerToolbox.getCenterOfMassVelocity()); + + + + 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()); + + if (!walk.getValue()) + { + footsteps.clear(); + + if (stopWalkingMessenger != null && walk.getValue() != walkPreviousValue.getValue()) + { + stopWalkingMessenger.submitStopWalkingRequest(); + } + + walkPreviousValue.set(false); + return; + } + else if (startWalkingMessenger != null && walk.getValue() != walkPreviousValue.getValue()) + { + startWalkingMessenger.submitStartWalkingRequest(); + } + + if (walk.getValue() != walkPreviousValue.getValue()) + { + counter = numberOfTicksBeforeSubmittingFootsteps.getValue(); // To make footsteps being sent right away. + } + + { // Processing footstep status + FootstepStatus statusToProcess = latestFootstepStatusReceived.getValue(); + + if (statusToProcess != null) + { + if (statusToProcess == FootstepStatus.STARTED) + { + if (!footsteps.isEmpty()) + footsteps.remove(0); + } + else if (statusToProcess == FootstepStatus.COMPLETED) + { + currentSupportSide.set(footstepCompletionSide.getValue()); + if (parameters.getNumberOfFixedFootsteps() == 0) + footsteps.clear(); + } + } + + latestFootstepStatusReceived.setValue(null); + footstepCompletionSide.setValue(null); + } + + + 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); + + } + + private FrameVector3DReadOnly getCenterOfMassVelocity() + { + return currentCoMVelocity; + } + + /** + * 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) + { + + } + + @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 00000000000..598423dfce7 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPluginFactory.java @@ -0,0 +1,118 @@ +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.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(CommonHumanoidReferenceFrames referenceFrames, + double updateDT, + WalkingControllerParameters walkingControllerParameters, + StatusMessageOutputManager walkingStatusMessageOutputManager, + CommandInputManager walkingCommandInputManager, + YoGraphicsListRegistry yoGraphicsListRegistry, + SideDependentList contactableFeet, + DoubleProvider timeProvider) + { + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = new DynamicsBasedFootstepPlugin(); + + 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); + + + return dynamicsBasedFootstepPlugin; + } +} 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 00000000000..edc315e56ee --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/FootSoleBasedGroundPlaneEstimator.java @@ -0,0 +1,190 @@ +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.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 FullHumanoidRobotModel robotModel; + + 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, FullHumanoidRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry, YoRegistry parentRegistry) + { + this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; + + this.robotModel = robotModel; + + 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(robotModel.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(); + } +} From 53099b980c6c6477707fa2399e5aa8908d56c52e Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 13 Nov 2024 14:32:02 -0600 Subject: [PATCH 04/10] added full robot model to build method of humanoid stepping plugins --- .../avatar/AvatarStepGeneratorThread.java | 3 +- .../DynamicsBasedFootstepPlugin.java | 50 +++++++++++++++---- .../DynamicsBasedFootstepPluginFactory.java | 21 ++++---- .../FootSoleBasedGroundPlaneEstimator.java | 9 ++-- ...edFootstepDataMessageGeneratorFactory.java | 4 +- .../plugin/HumanoidSteppingPluginFactory.java | 7 ++- .../JoystickBasedSteppingPluginFactory.java | 13 +++-- .../VelocityBasedSteppingPluginFactory.java | 4 +- 8 files changed, 80 insertions(+), 31 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java index d7d56850d58..c42bbfac4bc 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java @@ -98,7 +98,8 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory, pluginFactory.createStepGeneratorNetworkSubscriber(drcRobotModel.getSimpleRobotName(), ros2Node); humanoidReferenceFrames = new HumanoidReferenceFrames(fullRobotModel); - continuousStepGeneratorPlugin = pluginFactory.buildPlugin(humanoidReferenceFrames, + continuousStepGeneratorPlugin = pluginFactory.buildPlugin(fullRobotModel, + humanoidReferenceFrames, drcRobotModel.getStepGeneratorDT(), drcRobotModel.getWalkingControllerParameters(), walkingOutputManager, diff --git a/ihmc-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 index f73f05c5d0c..e3e10fa3d15 100644 --- 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 @@ -87,8 +87,9 @@ public enum FootState {SUPPORT, SWING} private DirectionalControlMessenger directionalControlMessenger; private StopWalkingMessenger stopWalkingMessenger; private StartWalkingMessenger startWalkingMessenger; + private FootstepMessenger footstepMessenger; - public DynamicsBasedFootstepPlugin(FullHumanoidRobotModel robotModel, double updateDT, CommonHumanoidReferenceFrames referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry) + public DynamicsBasedFootstepPlugin(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoGraphicsListRegistry yoGraphicsListRegistry) { this.updateDT = updateDT; this.referenceFrames = referenceFrames; @@ -115,18 +116,18 @@ protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.getTranslation().set(currentCoMPosition); - transformToParent.getRotation().set((FrameQuaternionReadOnly) desiredPelvisOrientation); + transformToParent.getRotation().set(desiredPelvisOrientation); } }; centerOfMassControlZUpFrame = new MovingZUpFrame(centerOfMassControlFrame, "centerOfMassControlZUpFrame" + variableNameSuffix); - groundPlaneEstimator = new FootSoleBasedGroundPlaneEstimator(centerOfMassControlZUpFrame, robotModel, yoGraphicsListRegistry, registry); + groundPlaneEstimator = new FootSoleBasedGroundPlaneEstimator(centerOfMassControlZUpFrame, referenceFrames, yoGraphicsListRegistry, registry); for (RobotSide robotSide : RobotSide.values) { footStates.put(robotSide, new YoEnum<>("footStates" + variableNameSuffix, registry, FootState.class)); - touchdownCalculator = new DynamicsBasedFootstepTouchdownCalculator(robotSide, centerOfMassControlZUpFrame, getCenterOfMassVelocity(), , robotModel, groundPlaneEstimator, parameters, gravity, , registry); + touchdownCalculator = new DynamicsBasedFootstepTouchdownCalculator(robotSide, centerOfMassControlZUpFrame, getCenterOfMassVelocity(), , robotModel, groundPlaneEstimator, parameters, gravity, desiredVelocityProvider, registry); } } @@ -137,6 +138,8 @@ public void update(double time) currentCoMPosition.setFromReferenceFrame(centerOfMassFrame); + handleDesiredsFromProviders(); + // TODO maybe wrong? desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); desiredPelvisOrientation.appendYawRotation(desiredTurningVelocity.getDoubleValue() * updateDT); @@ -146,8 +149,6 @@ public void update(double time) currentCoMVelocity.setMatchingFrame(controllerToolbox.getCenterOfMassVelocity()); - - footstepDataListMessage.setDefaultSwingDuration(parameters.getSwingDuration()); footstepDataListMessage.setDefaultTransferDuration(parameters.getTransferDuration()); footstepDataListMessage.setFinalTransferDuration(parameters.getTransferDuration()); @@ -201,7 +202,29 @@ else if (statusToProcess == FootstepStatus.COMPLETED) footstepCompletionSide.setValue(null); } + // Submit footsteps + if (walk.getValue() && footstepMessenger != null) + { + if (counter >= numberOfTicksBeforeSubmittingFootsteps.getValue()) + { + currentFootstepDataListCommandID.increment(); + footstepDataListMessage.setSequenceId(currentFootstepDataListCommandID.getValue()); + footstepDataListMessage.getQueueingProperties().setSequenceId(currentFootstepDataListCommandID.getValue()); + footstepDataListMessage.getQueueingProperties().setMessageId(currentFootstepDataListCommandID.getValue()); + footstepMessenger.submitFootsteps(footstepDataListMessage); + counter = 0; + } + else + { + counter++; + } + } + + walkPreviousValue.set(walk.getValue()); + } + private void handleDesiredsFromProviders() + { Vector2DReadOnly desiredVelocity = desiredVelocityProvider.getDesiredVelocity(); double desiredVelocityX = desiredVelocity.getX(); double desiredVelocityY = desiredVelocity.getY(); @@ -223,12 +246,16 @@ else if (statusToProcess == FootstepStatus.COMPLETED) this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); this.desiredTurningVelocity.set(turningVelocity); - } - private FrameVector3DReadOnly getCenterOfMassVelocity() + /** + * Sets the protocol for sending footsteps to the controller. + * + * @param footstepMessenger the callback used to send footsteps. + */ + public void setFootstepMessenger(FootstepMessenger footstepMessenger) { - return currentCoMVelocity; + this.footstepMessenger = footstepMessenger; } /** @@ -358,6 +385,11 @@ public void setFootstepAdjustment(FootstepAdjustment footstepAdjustment) } + private FrameVector3DReadOnly getCenterOfMassVelocity() + { + return currentCoMVelocity; + } + @Override public YoRegistry getRegistry() { 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 index 598423dfce7..fd62dda2a12 100644 --- 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 @@ -12,6 +12,7 @@ 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; @@ -76,16 +77,17 @@ public void addUpdatable(Updatable updatable) } @Override - public DynamicsBasedFootstepPlugin buildPlugin(CommonHumanoidReferenceFrames referenceFrames, - double updateDT, - WalkingControllerParameters walkingControllerParameters, - StatusMessageOutputManager walkingStatusMessageOutputManager, - CommandInputManager walkingCommandInputManager, - YoGraphicsListRegistry yoGraphicsListRegistry, - SideDependentList contactableFeet, - DoubleProvider timeProvider) + 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(); + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = new DynamicsBasedFootstepPlugin(robotModel, referenceFrames, updateDT, yoGraphicsListRegistry); dynamicsBasedFootstepPlugin.setStopWalkingMessenger(new StopWalkingMessenger() { @@ -112,6 +114,7 @@ public void submitStartWalkingRequest() 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/FootSoleBasedGroundPlaneEstimator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/FootSoleBasedGroundPlaneEstimator.java index edc315e56ee..8d6ea0a26f7 100644 --- 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 @@ -18,6 +18,7 @@ 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; @@ -27,7 +28,7 @@ public class FootSoleBasedGroundPlaneEstimator { - private final FullHumanoidRobotModel robotModel; + private final CommonHumanoidReferenceFrames referenceFrames; private final MovingReferenceFrame centerOfMassControlZUpFrame; @@ -51,11 +52,11 @@ public class FootSoleBasedGroundPlaneEstimator * * @param parentRegistry the {@code YoRegistry} used to register YoVariables constructed within this class */ - public FootSoleBasedGroundPlaneEstimator(MovingReferenceFrame centerOfMassControlZUpFrame, FullHumanoidRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry, YoRegistry parentRegistry) + public FootSoleBasedGroundPlaneEstimator(MovingReferenceFrame centerOfMassControlZUpFrame, CommonHumanoidReferenceFrames referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry, YoRegistry parentRegistry) { this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; - this.robotModel = robotModel; + this.referenceFrames = referenceFrames; groundPlaneEstimator = new YoGroundPlaneEstimator("FootSoleBased", parentRegistry, @@ -116,7 +117,7 @@ private void updateFrameContactPoints(boolean isInToeOff) if (!isInToeOff) { - frameContactPoint.setMatchingFrame(robotModel.getSoleFrame(supportSide), contactPoints.get(i)); + frameContactPoint.setMatchingFrame(referenceFrames.getSoleFrame(supportSide), contactPoints.get(i)); frameContactPoint.addZ(groundPlaneVerticalOffset.getDoubleValue()); } else if (i > 1) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/ComponentBasedFootstepDataMessageGeneratorFactory.java index 4f1ccb859cb..1a8d34897fc 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 @@ -13,6 +13,7 @@ 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 +122,8 @@ public StepGeneratorCommandInputManager getStepGeneratorCommandInputManager() } @Override - public ComponentBasedFootstepDataMessageGenerator buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + public ComponentBasedFootstepDataMessageGenerator 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/HumanoidSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java index e9b365897e9..3c79f4701ff 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/HumanoidSteppingPluginFactory.java @@ -15,6 +15,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.ros2.ROS2Topic; @@ -51,7 +52,8 @@ default HumanoidSteppingPlugin buildPlugin(HighLevelControllerFactoryHelper cont { HighLevelHumanoidControllerToolbox controllerToolbox = controllerFactoryHelper.getHighLevelHumanoidControllerToolbox(); - return buildPlugin(controllerToolbox.getReferenceFrames(), + return buildPlugin(controllerToolbox.getFullRobotModel(), + controllerToolbox.getReferenceFrames(), controllerToolbox.getControlDT(), controllerFactoryHelper.getWalkingControllerParameters(), controllerFactoryHelper.getStatusMessageOutputManager(), @@ -61,7 +63,8 @@ default HumanoidSteppingPlugin buildPlugin(HighLevelControllerFactoryHelper cont controllerToolbox.getYoTime()); } - HumanoidSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + HumanoidSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, double updateDT, WalkingControllerParameters walkingControllerParameters, StatusMessageOutputManager walkingStatusMessageOutputManager, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java index 02688d493ea..37f6d3b13f3 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 @@ -14,6 +14,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -88,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, @@ -97,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, @@ -105,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, @@ -114,7 +118,8 @@ public JoystickBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames ref contactableFeet, timeProvider); - DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = dynamicsBasedFootstepPluginFactory.buildPlugin(referenceFrames, + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = dynamicsBasedFootstepPluginFactory.buildPlugin(robotModel, + referenceFrames, updateDT, walkingControllerParameters, walkingStatusMessageOutputManager, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java index 8a402926057..688f7e5c63e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/VelocityBasedSteppingPluginFactory.java @@ -10,6 +10,7 @@ import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PlanarRegionsListCommand; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.contactable.ContactableBody; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames; @@ -93,7 +94,8 @@ public StepGeneratorCommandInputManager getStepGeneratorCommandInputManager() } @Override - public VelocityBasedSteppingPlugin buildPlugin(CommonHumanoidReferenceFrames referenceFrames, + public VelocityBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel, + CommonHumanoidReferenceFrames referenceFrames, double updateDT, WalkingControllerParameters walkingControllerParameters, StatusMessageOutputManager walkingStatusMessageOutputManager, From 4e4f7676aecce47bf43276aa0a4942383bbbf2dc Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Thu, 14 Nov 2024 16:42:50 -0600 Subject: [PATCH 05/10] lumped new footstep provider into csg istelf as a 2d footstep position provider --- .../scs2/SCS2AvatarSimulationFactory.java | 2 +- .../ContinuousStepGenerator.java | 123 ++++-- .../DynamicsBasedFootstepParameters.java | 2 +- .../DynamicsBasedFootstepPlugin.java | 353 +++++++++++++----- .../DynamicsBasedFootstepPluginFactory.java | 2 +- ...amicsBasedFootstepTouchdownCalculator.java | 12 +- ...edFootstepDataMessageGeneratorFactory.java | 12 + .../plugin/JoystickBasedSteppingPlugin.java | 3 +- .../JoystickBasedSteppingPluginFactory.java | 27 +- 9 files changed, 397 insertions(+), 139 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java index 6c7f7dcb8c5..ab255e43f2b 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java @@ -460,7 +460,7 @@ private void setupStepGeneratorThread() else { JoystickBasedSteppingPluginFactory joystickPluginFactory = new JoystickBasedSteppingPluginFactory(); - if (heightMapForFootstepZ.hasValue()) + if (heightMapForFootstepZ.hasValue() && heightMapForFootstepZ.get() != null) joystickPluginFactory.setFootStepAdjustment(new HeightMapBasedFootstepAdjustment(heightMapForFootstepZ.get())); else { diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index fe87f5efaaf..bde78d83021 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; @@ -119,6 +121,13 @@ 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); @@ -198,12 +207,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 +233,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 +257,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED) footstepCompletionSide.setValue(null); } + // Determine swing side RobotSide swingSide; if (footsteps.isEmpty()) @@ -274,6 +279,24 @@ else if (statusToProcess == FootstepStatus.COMPLETED) 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(); double maxStepWidth = parameters.getMaxStepWidth(); double minStepWidth = parameters.getMinStepWidth(); @@ -307,32 +330,44 @@ else if (statusToProcess == FootstepStatus.COMPLETED) 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.fromByte(footstep.getRobotSide()) == null) ? RobotSide.fromByte(footstep.getRobotSide()) : RobotSide.LEFT; - 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().update(time); + dynamicsBasedFootstepPlugin.get().getDesiredTouchdownPosition2D(footstepSide, nextFootstepPose2D.getPosition()); + } + else + { + 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 +476,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 +575,7 @@ public void setSwingHeight(double swingHeight) public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider desiredTurningVelocityProvider) { this.desiredTurningVelocityProvider = desiredTurningVelocityProvider; + dynamicsBasedFootstepPlugin.get().setDesiredTurningVelocityProvider(desiredTurningVelocityProvider); } /** @@ -520,6 +586,7 @@ public void setDesiredTurningVelocityProvider(DesiredTurningVelocityProvider des public void setDesiredVelocityProvider(DesiredVelocityProvider desiredVelocityProvider) { this.desiredVelocityProvider = desiredVelocityProvider; + dynamicsBasedFootstepPlugin.get().setDesiredVelocityProvider(desiredVelocityProvider); } /** @@ -601,6 +668,7 @@ public void notifyFootstepStarted() public void setFootstepStatusListener(StatusMessageOutputManager statusMessageOutputManager) { statusMessageOutputManager.attachStatusMessageListener(FootstepStatusMessage.class, this::consumeFootstepStatus); + dynamicsBasedFootstepPlugin.get().setFootstepStatusListener(statusMessageOutputManager); } /** @@ -756,6 +824,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 index b5adf6e096b..37dfd70c89d 100644 --- 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 @@ -62,6 +62,7 @@ public DynamicsBasedFootstepParameters(double gravityZ, YoRegistry parentRegistr 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)); @@ -71,7 +72,6 @@ public DynamicsBasedFootstepParameters(double gravityZ, YoRegistry parentRegistr poleCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "pole", suffix + suffix2, pole, registry)); omegaCurrentStep.put(robotSide, createStageableYoDouble(robotSide, "omega", suffix + suffix2, omega, registry)); - stageableYoDoubles.put(robotSide, new ArrayList<>()); } comHeight.addListener(change -> omega.set(Math.sqrt(Math.abs(gravityZ / comHeight.getDoubleValue())))); 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 index e3e10fa3d15..99b6b9cf264 100644 --- 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 @@ -1,30 +1,29 @@ package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.dyanmicsBasedFootstepGenerator; -import controller_msgs.msg.dds.FootstepDataListMessage; -import controller_msgs.msg.dds.FootstepDataMessage; 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.commons.lists.RecyclingArrayList; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; +import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +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.yoGraphics.YoGraphicsListRegistry; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataListCommand; 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; @@ -53,15 +52,17 @@ public class DynamicsBasedFootstepPlugin implements HumanoidSteppingPlugin // 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 ReferenceFrame centerOfMassFrame; - private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; + private final MovingReferenceFrame centerOfMassFrame; +// private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; // Command/Desired output related stuff private final SideDependentList touchdownCalculator = new SideDependentList<>(); - private final FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); - private final RecyclingArrayList footsteps = footstepDataListMessage.getFootstepDataList(); + private final SideDependentList desiredTouchdownPositions = new SideDependentList<>(); // Desired inputs private final YoDouble desiredTurningVelocity = new YoDouble("desiredTurningVelocity" + variableNameSuffix, registry); @@ -74,9 +75,16 @@ public class DynamicsBasedFootstepPlugin implements HumanoidSteppingPlugin // 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 static Vector2DReadOnly zero2D = new Vector2D(); @@ -89,14 +97,18 @@ public enum FootState {SUPPORT, SWING} private StartWalkingMessenger startWalkingMessenger; private FootstepMessenger footstepMessenger; - public DynamicsBasedFootstepPlugin(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoGraphicsListRegistry yoGraphicsListRegistry) + public DynamicsBasedFootstepPlugin(FullHumanoidRobotModel robotModel, CommonHumanoidReferenceFrames referenceFrames, double updateDT, YoGraphicsListRegistry yoGraphicsListRegistry, DoubleProvider yoTime) { this.updateDT = updateDT; this.referenceFrames = referenceFrames; + // Parameters parameters = new DynamicsBasedFootstepParameters(gravity, registry); - centerOfMassFrame = referenceFrames.getCenterOfMassFrame(); + // All things estimates + centerOfMassFrame = (MovingReferenceFrame) referenceFrames.getCenterOfMassFrame(); + + angularExcursionCalculator = new AngularExcursionCalculator(centerOfMassFrame, robotModel.getElevator(), updateDT, registry, null); desiredPelvisOrientation = new YoFrameQuaternion("pelvisDesiredOrientation" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); @@ -122,23 +134,132 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) centerOfMassControlZUpFrame = new MovingZUpFrame(centerOfMassControlFrame, "centerOfMassControlZUpFrame" + variableNameSuffix); - groundPlaneEstimator = new FootSoleBasedGroundPlaneEstimator(centerOfMassControlZUpFrame, referenceFrames, yoGraphicsListRegistry, registry); + //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<>("footStates" + variableNameSuffix, registry, FootState.class)); - touchdownCalculator = new DynamicsBasedFootstepTouchdownCalculator(robotSide, centerOfMassControlZUpFrame, getCenterOfMassVelocity(), , robotModel, groundPlaneEstimator, parameters, gravity, desiredVelocityProvider, registry); + footStates.put(robotSide, new YoEnum<>(robotSide.getLowerCaseName() + "FootStates" + variableNameSuffix, registry, FootState.class)); + + pendulumBase.put(robotSide, new FramePoint2D(centerOfMassControlZUpFrame)); + + 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)); + } + } + + private class SupportFootState implements State + { + private final RobotSide robotSide; + + private SupportFootState(RobotSide robotSide) + { + this.robotSide = robotSide; + } + + @Override + public void onEntry() + { + pendulumBase.get(robotSide).setFromReferenceFrame(referenceFrames.getSoleZUpFrame(robotSide)); + } + + @Override + public void doAction(double timeInState) + { + + } + + @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(); +// groundPlaneEstimator.update(); currentCoMPosition.setFromReferenceFrame(centerOfMassFrame); - handleDesiredsFromProviders(); + angularExcursionCalculator.compute(); + currentCentroidalLinearMomentum.setMatchingFrame(angularExcursionCalculator.getLinearMomentum()); + currentCentroidalAngularMomentum.setMatchingFrame(angularExcursionCalculator.getAngularMomentum()); // TODO maybe wrong? desiredPelvisOrientation.setToYawOrientation(referenceFrames.getChestFrame().getTransformToWorldFrame().getRotation().getYaw()); @@ -147,80 +268,95 @@ public void update(double time) centerOfMassControlFrame.update(); centerOfMassControlZUpFrame.update(); - currentCoMVelocity.setMatchingFrame(controllerToolbox.getCenterOfMassVelocity()); + currentCoMVelocity.setMatchingFrame(centerOfMassFrame.getTwistOfFrame().getLinearPart()); - footstepDataListMessage.setDefaultSwingDuration(parameters.getSwingDuration()); - footstepDataListMessage.setDefaultTransferDuration(parameters.getTransferDuration()); - footstepDataListMessage.setFinalTransferDuration(parameters.getTransferDuration()); - footstepDataListMessage.setAreFootstepsAdjustable(parameters.getStepsAreAdjustable()); - footstepDataListMessage.setOffsetFootstepsWithExecutionError(parameters.getShiftUpcomingStepsWithTouchdown()); + handleDesiredsFromProviders(); - if (!ignoreWalkInputProvider.getBooleanValue() && walkInputProvider != null) - walk.set(walkInputProvider.getValue()); + inDoubleSupport.set(footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT && + footStates.get(RobotSide.RIGHT).getEnumValue() == FootState.SUPPORT); - if (!walk.getValue()) - { - footsteps.clear(); + calculateNetPendulumBase(); - if (stopWalkingMessenger != null && walk.getValue() != walkPreviousValue.getValue()) - { - stopWalkingMessenger.submitStopWalkingRequest(); - } + for (RobotSide robotSide : RobotSide.values) + footStateMachines.get(robotSide).doActionAndTransition(); + } - walkPreviousValue.set(false); - return; - } - else if (startWalkingMessenger != null && walk.getValue() != walkPreviousValue.getValue()) - { - startWalkingMessenger.submitStartWalkingRequest(); - } + 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()); + } - if (walk.getValue() != walkPreviousValue.getValue()) - { - counter = numberOfTicksBeforeSubmittingFootsteps.getValue(); // To make footsteps being sent right away. - } + private final FramePoint2DBasics tempPendulumBase = new FramePoint2D(); + private final FramePoint2DBasics tempNetPendulumBase = new FramePoint2D();; - { // Processing footstep status - FootstepStatus statusToProcess = latestFootstepStatusReceived.getValue(); + public void calculate(DynamicsBasedFootstepTouchdownCalculator touchdownCalculator, + RobotSide swingSide, + double timeToReachGoal, + FramePoint2DReadOnly pendulumBase, + FramePoint2DReadOnly netPendulumBase, + boolean isInDoubleSupport, + FixedFramePoint2DBasics touchdownPositionToPack) + { + tempPendulumBase.setIncludingFrame(pendulumBase); + tempPendulumBase.changeFrame(centerOfMassControlZUpFrame); - if (statusToProcess != null) - { - if (statusToProcess == FootstepStatus.STARTED) - { - if (!footsteps.isEmpty()) - footsteps.remove(0); - } - else if (statusToProcess == FootstepStatus.COMPLETED) - { - currentSupportSide.set(footstepCompletionSide.getValue()); - if (parameters.getNumberOfFixedFootsteps() == 0) - footsteps.clear(); - } - } + tempNetPendulumBase.setIncludingFrame(netPendulumBase); + tempNetPendulumBase.changeFrame(referenceFrames.getSoleZUpFrame(getTrailingSide())); - latestFootstepStatusReceived.setValue(null); - footstepCompletionSide.setValue(null); - } + touchdownCalculator.computeDesiredTouchdownPosition(swingSide.getOppositeSide(), timeToReachGoal, tempPendulumBase, tempNetPendulumBase, isInDoubleSupport); + touchdownPositionToPack.setMatchingFrame(touchdownCalculator.getDesiredTouchdownPosition2D()); + } - // Submit footsteps - if (walk.getValue() && footstepMessenger != null) + private void calculateNetPendulumBase() + { + // if only one foot is in contact, get the pendulum base directly from the support state + if (!inDoubleSupport.getBooleanValue()) { - if (counter >= numberOfTicksBeforeSubmittingFootsteps.getValue()) + if (footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT) { - currentFootstepDataListCommandID.increment(); - footstepDataListMessage.setSequenceId(currentFootstepDataListCommandID.getValue()); - footstepDataListMessage.getQueueingProperties().setSequenceId(currentFootstepDataListCommandID.getValue()); - footstepDataListMessage.getQueueingProperties().setMessageId(currentFootstepDataListCommandID.getValue()); - footstepMessenger.submitFootsteps(footstepDataListMessage); - counter = 0; + netPendulumBase.setMatchingFrame(pendulumBase.get(RobotSide.LEFT)); + setTrailingSide(RobotSide.LEFT); } + else { - counter++; + netPendulumBase.setMatchingFrame(pendulumBase.get(RobotSide.RIGHT)); + setTrailingSide(RobotSide.RIGHT); } + + return; } - walkPreviousValue.set(walk.getValue()); + // 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.changeFrame(referenceFrames.getSoleZUpFrame(trailingSide)); } private void handleDesiredsFromProviders() @@ -230,22 +366,22 @@ private void handleDesiredsFromProviders() double desiredVelocityY = desiredVelocity.getY(); double turningVelocity = desiredTurningVelocityProvider.getTurningVelocity(); - if (desiredVelocityProvider.isUnitVelocity()) - { - double minMaxVelocityX = maxStepLength / stepTime.getValue(); - double minMaxVelocityY = maxStepWidth / stepTime.getValue(); - desiredVelocityX = minMaxVelocityX * MathTools.clamp(desiredVelocityX, 1.0); - desiredVelocityY = minMaxVelocityY * MathTools.clamp(desiredVelocityY, 1.0); - } - - if (desiredTurningVelocityProvider.isUnitVelocity()) - { - double minMaxVelocityTurn = (turnMaxAngleOutward - turnMaxAngleInward) / stepTime.getValue(); - turningVelocity = minMaxVelocityTurn * MathTools.clamp(turningVelocity, 1.0); - } - - this.desiredVelocity.set(desiredVelocityX, desiredVelocityY); - this.desiredTurningVelocity.set(turningVelocity); +// 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); } /** @@ -385,11 +521,46 @@ public void setFootstepAdjustment(FootstepAdjustment footstepAdjustment) } - private FrameVector3DReadOnly getCenterOfMassVelocity() + private void setTrailingSide(RobotSide robotSide) + { + trailingSide = robotSide; + } + + private RobotSide getTrailingSide() + { + return trailingSide; + } + + public void getDesiredTouchdownPosition2D(RobotSide robotSide, FixedFramePoint2DBasics touchdownPositionToPack) + { + touchdownPositionToPack.setMatchingFrame(getDesiredTouchdownPosition2D(robotSide)); + } + + public FramePoint2DReadOnly getDesiredTouchdownPosition2D(RobotSide robotSide) + { + 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() { 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 index fd62dda2a12..b865a7eb7e6 100644 --- 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 @@ -87,7 +87,7 @@ public DynamicsBasedFootstepPlugin buildPlugin(FullHumanoidRobotModel robotModel SideDependentList contactableFeet, DoubleProvider timeProvider) { - DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = new DynamicsBasedFootstepPlugin(robotModel, referenceFrames, updateDT, yoGraphicsListRegistry); + DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = new DynamicsBasedFootstepPlugin(robotModel, referenceFrames, updateDT, yoGraphicsListRegistry, timeProvider); dynamicsBasedFootstepPlugin.setStopWalkingMessenger(new StopWalkingMessenger() { 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 index ad70b7ce966..103a268e91e 100644 --- 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 @@ -23,7 +23,7 @@ public class DynamicsBasedFootstepTouchdownCalculator private final FrameVector3DReadOnly centroidalAngularMomentum; - private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; +// private final FootSoleBasedGroundPlaneEstimator groundPlaneEstimator; private final YoBoolean isInitialStanceValid; @@ -88,7 +88,6 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, FrameVector3DReadOnly centerOfMassVelocity, FrameVector3DReadOnly centroidalAngularMomentum, FullHumanoidRobotModel robotModel, - FootSoleBasedGroundPlaneEstimator groundPlaneEstimator, DynamicsBasedFootstepParameters parameters, double gravity, Vector2DReadOnly desiredVelocityProvider, @@ -98,7 +97,7 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, this.centerOfMassControlZUpFrame = centerOfMassControlZUpFrame; this.centerOfMassVelocity = centerOfMassVelocity; this.centroidalAngularMomentum = centroidalAngularMomentum; - this.groundPlaneEstimator = groundPlaneEstimator; +// this.groundPlaneEstimator = groundPlaneEstimator; this.desiredVelocityProvider = desiredVelocityProvider; soleFrames = robotModel.getSoleFrames(); mass = robotModel.getTotalMass(); @@ -205,7 +204,7 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo // desiredTouchdownPosition2D.addY(supportSide.negateIfLeftSide(fastWalkingParameters.getFixedWidthOffset())); // determine touchdown z position from x position, y position, and ground plane - desiredTouchdownPosition.setMatchingFrame(groundPlaneEstimator.getGroundPosition(desiredTouchdownPosition2D)); + desiredTouchdownPosition.setMatchingFrame(desiredTouchdownPosition2D, 0.0); } public FramePoint3DReadOnly getDesiredTouchdownPosition() @@ -213,6 +212,11 @@ public FramePoint3DReadOnly getDesiredTouchdownPosition() return desiredTouchdownPosition; } + public FramePoint2DReadOnly getDesiredTouchdownPosition2D() + { + return desiredTouchdownPosition2D; + } + public FrameVector2DReadOnly getPredictedVelocityAtTouchdown() { return predictedVelocityAtTouchdown; 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 1a8d34897fc..686d7e6e790 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,6 +8,7 @@ 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; @@ -139,6 +140,17 @@ public ComponentBasedFootstepDataMessageGenerator buildPlugin(FullHumanoidRobotM 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/JoystickBasedSteppingPlugin.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPlugin.java index 734168aa7fc..0d7daf820a2 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 @@ -24,7 +24,6 @@ public class JoystickBasedSteppingPlugin implements HumanoidSteppingPlugin public JoystickBasedSteppingPlugin(ComponentBasedFootstepDataMessageGenerator stepGenerator, VelocityBasedSteppingPlugin fastWalkingStepGenerator, - DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin, List updatables) { this.stepGenerator = stepGenerator; @@ -32,7 +31,7 @@ public JoystickBasedSteppingPlugin(ComponentBasedFootstepDataMessageGenerator st this.updatables = updatables; registry.addChild(stepGenerator.getRegistry()); registry.addChild(fastWalkingStepGenerator.getRegistry()); - registry.addChild(dynamicsBasedFootstepPlugin.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 37f6d3b13f3..7fc9c777d7d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/JoystickBasedSteppingPluginFactory.java @@ -118,15 +118,15 @@ public JoystickBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel contactableFeet, timeProvider); - DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = dynamicsBasedFootstepPluginFactory.buildPlugin(robotModel, - referenceFrames, - updateDT, - walkingControllerParameters, - walkingStatusMessageOutputManager, - walkingCommandInputManager, - yoGraphicsListRegistry, - contactableFeet, - timeProvider); +// DynamicsBasedFootstepPlugin dynamicsBasedFootstepPlugin = dynamicsBasedFootstepPluginFactory.buildPlugin(robotModel, +// referenceFrames, +// updateDT, +// walkingControllerParameters, +// walkingStatusMessageOutputManager, +// walkingCommandInputManager, +// yoGraphicsListRegistry, +// contactableFeet, +// timeProvider); csgFootstepGenerator.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); csgFootstepGenerator.setDesiredTurningVelocityProvider(commandInputManager.createDesiredTurningVelocityProvider()); @@ -138,10 +138,10 @@ public JoystickBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel fastWalkingPlugin.setWalkInputProvider(commandInputManager.createWalkInputProvider()); fastWalkingPlugin.setSwingHeightInputProvider(commandInputManager.createSwingHeightProvider()); - dynamicsBasedFootstepPlugin.setDesiredVelocityProvider(commandInputManager.createDesiredVelocityProvider()); - dynamicsBasedFootstepPlugin.setDesiredTurningVelocityProvider(commandInputManager.createDesiredTurningVelocityProvider()); - dynamicsBasedFootstepPlugin.setWalkInputProvider(commandInputManager.createWalkInputProvider()); - dynamicsBasedFootstepPlugin.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); @@ -159,7 +159,6 @@ public JoystickBasedSteppingPlugin buildPlugin(FullHumanoidRobotModel robotModel JoystickBasedSteppingPlugin joystickBasedSteppingPlugin = new JoystickBasedSteppingPlugin(csgFootstepGenerator, fastWalkingPlugin, - dynamicsBasedFootstepPlugin, updatables); joystickBasedSteppingPlugin.setHighLevelStateChangeStatusListener(walkingStatusMessageOutputManager); From ef96404f36d5ccbe7be93ac9a943a50d20b2b475 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Mon, 18 Nov 2024 20:20:46 -0600 Subject: [PATCH 06/10] footstep can be adjusted mid swing. Need to clean up lots of stuff though --- .../ContinuousStepGenerator.java | 60 ++++++++++++------ .../DynamicsBasedFootstepPlugin.java | 21 ++++++- ...amicsBasedFootstepTouchdownCalculator.java | 13 ++-- .../states/WalkingSingleSupportState.java | 62 ++++++++++++++----- .../WalkingMessageHandler.java | 17 +++++ 5 files changed, 131 insertions(+), 42 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java index bde78d83021..169d0dda5bd 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 @@ -134,6 +134,7 @@ public enum CSGMode {STANDARD, DYNAMIC} 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); @@ -268,15 +269,19 @@ 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(); - - previousFootstepPose.set(previousFootstep.getLocation(), previousFootstep.getOrientation()); + footsteps.clear(); + swingSide = currentSwingSide.getEnumValue(); + previousFootstepPose.set(currentSupportFootPose); } // Set parameters for FootstepDataListMessage @@ -331,7 +336,8 @@ else if (statusToProcess == FootstepStatus.COMPLETED) for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++) { FootstepDataMessage footstep = footsteps.add(); - RobotSide footstepSide = !(RobotSide.fromByte(footstep.getRobotSide()) == null) ? RobotSide.fromByte(footstep.getRobotSide()) : RobotSide.LEFT; +// RobotSide footstepSide = RobotSide.RIGHT;//!(RobotSide.fromByte(footstep.getRobotSide()) == null) ? RobotSide.fromByte(footstep.getRobotSide()) : RobotSide.LEFT; +// footstep.setRobotSide(footstepSide.toByte()); ////// TODO new stuff here if (csgMode.getEnumValue() == CSGMode.DYNAMIC && dynamicsBasedFootstepPlugin.hasValue()) @@ -339,15 +345,29 @@ else if (statusToProcess == FootstepStatus.COMPLETED) if (i == startingIndexToAdjust) { dynamicsBasedFootstepPlugin.get().update(time); - dynamicsBasedFootstepPlugin.get().getDesiredTouchdownPosition2D(footstepSide, nextFootstepPose2D.getPosition()); + dynamicsBasedFootstepPlugin.get().getDesiredTouchdownPosition2D(swingSide, nextFootstepPose2D.getPosition()); } else { - dynamicsBasedFootstepPlugin.get().calculate(footstepSide, - dynamicsBasedFootstepPlugin.get().getStepDuration(footstepSide), - footstepPose2D, - footstepPose2D, - nextFootstepPose2D); + 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 @@ -641,6 +661,7 @@ public void notifyFootstepCompleted(RobotSide robotSide) { latestStatusReceived.setValue(FootstepStatus.COMPLETED); footstepCompletionSide.setValue(robotSide); + currentSwingSide.set(robotSide.getOppositeSide()); } /** @@ -650,16 +671,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)}. *

* @@ -672,7 +694,7 @@ public void setFootstepStatusListener(StatusMessageOutputManager statusMessageOu } /** - * Consumes a newly received message and calls {@link #notifyFootstepStarted()} or + * Consumes a newly received message and calls {@link #notifyFootstepStarted(RobotSide robotSide)} or * {@link #notifyFootstepCompleted(RobotSide)} according to the status. * * @param statusMessage the newly received footstep status. @@ -683,7 +705,7 @@ public void consumeFootstepStatus(FootstepStatusMessage statusMessage) if (status == FootstepStatus.COMPLETED) notifyFootstepCompleted(RobotSide.fromByte(statusMessage.getRobotSide())); else if (status == FootstepStatus.STARTED) - notifyFootstepStarted(); + notifyFootstepStarted(RobotSide.fromByte(statusMessage.getRobotSide())); } /** diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/dyanmicsBasedFootstepGenerator/DynamicsBasedFootstepPlugin.java index 99b6b9cf264..9573cd9fd2d 100644 --- 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 @@ -181,12 +181,31 @@ private SupportFootState(RobotSide robotSide) public void onEntry() { pendulumBase.get(robotSide).setFromReferenceFrame(referenceFrames.getSoleZUpFrame(robotSide)); + + 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) { - + 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 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 index 103a268e91e..62653c51057 100644 --- 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 @@ -2,6 +2,7 @@ import us.ihmc.euclid.referenceFrame.FramePoint2D; 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; @@ -40,7 +41,7 @@ public class DynamicsBasedFootstepTouchdownCalculator private final YoFrameVector2D predictedVelocityAtTouchdown; // This is the desired touchdown position to be used by the robot - private final YoFramePoint3D desiredTouchdownPosition; + private final YoFramePoint3D desiredTouchdownPositionInWorld3D; 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 @@ -123,7 +124,7 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, desiredALIPTouchdownPositionWithDS = new YoFramePoint2D(prefix + "DesiredALIPTouchdownPositionWithDS", centerOfMassControlZUpFrame, registry); // Final touchdown position and predicted velocity. These are populated by the touchdown calculator currently in use - desiredTouchdownPosition = new YoFramePoint3D(prefix + "DesiredTouchdownPosition", centerOfMassControlZUpFrame, registry); + desiredTouchdownPositionInWorld3D = new YoFramePoint3D(prefix + "DesiredTouchdownPositionInWorld", ReferenceFrame.getWorldFrame(), registry); desiredTouchdownPosition2D = new FramePoint2D(centerOfMassControlZUpFrame); predictedVelocityAtTouchdown = new YoFrameVector2D(prefix + "PredictedVelocityAtTouchdown", centerOfMassControlZUpFrame, registry); @@ -165,7 +166,7 @@ public void reset() { desiredALIPTouchdownPositionWithoutDS.setToNaN(); desiredALIPTouchdownPositionWithDS.setToNaN(); - desiredTouchdownPosition.setToNaN(); + desiredTouchdownPositionInWorld3D.setToNaN(); isInitialStanceValid.set(false); } @@ -204,12 +205,12 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo // desiredTouchdownPosition2D.addY(supportSide.negateIfLeftSide(fastWalkingParameters.getFixedWidthOffset())); // determine touchdown z position from x position, y position, and ground plane - desiredTouchdownPosition.setMatchingFrame(desiredTouchdownPosition2D, 0.0); + desiredTouchdownPositionInWorld3D.setMatchingFrame(desiredTouchdownPosition2D, 0.0); } - public FramePoint3DReadOnly getDesiredTouchdownPosition() + public FramePoint3DReadOnly getDesiredTouchdownPositionInWorld3D() { - return desiredTouchdownPosition; + return desiredTouchdownPositionInWorld3D; } public FramePoint2DReadOnly getDesiredTouchdownPosition2D() diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java index 2ff6888b387..4e24fb54e40 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; @@ -202,7 +215,7 @@ else if (resubmitStepsInSwingEveryTick.getBooleanValue()) // delayed for another reason. So we should check before updating step adjustment boolean footstepIsBeingAdjusted = false; if (!feetManager.getCurrentConstraintType(swingSide).isLoadBearing()) - footstepIsBeingAdjusted = balanceManager.checkAndUpdateStepAdjustment(nextFootstep); + footstepIsBeingAdjusted = true; //balanceManager.checkAndUpdateStepAdjustment(nextFootstep); if (footstepIsBeingAdjusted) { @@ -273,17 +286,19 @@ public boolean isDone(double timeInState) return finishWhenICPPlannerIsDone.getValue() && balanceManager.isICPPlanDone(); } - @Override - public void onEntry() + private void test() { - super.onEntry(); + walkingMessageHandler.peekFootstep(0, nextFootstep); - hasSwingFootTouchedDown.set(false); - hasTriggeredTouchdown.set(false); + if (nextFootstep.getRobotSide() != swingSide || !haveWeEntered) + return; + + balanceManager.clearICPPlan(); double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); swingTime = walkingMessageHandler.getNextSwingTime(); + walkingMessageHandler.poll(nextFootstep, footstepTiming); if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) walkingMessageHandler.peekFootstep(0, nextNextFootstep); @@ -311,26 +326,39 @@ public void onEntry() balanceManager.addFootstepToPlan(footsteps[i], footstepTimings[i]); } - balanceManager.setICPPlanSupportSide(supportSide); - balanceManager.initializeICPPlanForSingleSupport(); - /** This has to be called after calling initialize ICP Plan, as that resets the step constraints **/ walkingMessageHandler.pollStepConstraints(stepConstraints); - balanceManager.setCurrentStepConstraints(stepConstraints); updateHeightManager(); - feetManager.requestSwing(swingSide, - nextFootstep, - swingTime, - balanceManager.getFinalDesiredCoMVelocity(), - balanceManager.getFinalDesiredCoMAcceleration()); - if (feetManager.adjustHeightIfNeeded(nextFootstep)) { walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); } + } + + @Override + public void onEntry() + { + super.onEntry(); + + haveWeEntered = true; + + hasSwingFootTouchedDown.set(false); + hasTriggeredTouchdown.set(false); + + test(); + + balanceManager.setICPPlanSupportSide(supportSide); + balanceManager.initializeICPPlanForSingleSupport(); + balanceManager.setCurrentStepConstraints(stepConstraints); + + feetManager.requestSwing(swingSide, + nextFootstep, + swingTime, + balanceManager.getFinalDesiredCoMVelocity(), + balanceManager.getFinalDesiredCoMAcceleration()); balanceManager.setSwingFootTrajectory(swingSide, feetManager.getSwingTrajectory(swingSide)); @@ -346,6 +374,8 @@ public void onExit(double timeInState) { super.onExit(timeInState); + haveWeEntered = false; + triggerTouchdown(); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/messageHandlers/WalkingMessageHandler.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/messageHandlers/WalkingMessageHandler.java index 63521e71e4f..94d34da1a59 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()) From b21014811abc6464a6caa9ab2a3917602374264f Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 19 Nov 2024 14:35:48 -0600 Subject: [PATCH 07/10] changed linear mmomentum rate --- .../LinearMomentumRateControlModule.java | 21 ++++++++++++++++++- .../controller/ICPController.java | 2 +- .../ContinuousStepGenerator.java | 2 +- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java index 7ee8f8c8fd3..9d63b8c415d 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 @@ -110,6 +110,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(); @@ -385,7 +386,7 @@ public boolean computeControllerCoreCommands() yoCenterOfMassVelocity.set(capturePointCalculator.getCenterOfMassVelocity()); yoCapturePoint.set(capturePoint); - success = success && computeDesiredLinearMomentumRateOfChange(); + success = success && computeDesiredLinearMomentumRateOfChangeNew(); selectionMatrix.setToLinearSelectionOnly(); selectionMatrix.selectLinearX(!useCenterOfPressureCommandOnly.getValue()); @@ -563,6 +564,24 @@ private boolean computeDesiredLinearMomentumRateOfChange() return success; } + private boolean computeDesiredLinearMomentumRateOfChangeNew() + { + double totalMass = totalMassProvider.getValue(); + double desiredVerticalRateOfChange = totalMass * desiredCoMHeightAcceleration; + + desiredPendulumBase.setIncludingFrame(desiredCoP, 0.0); + 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/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index 577fda9b4ad..ce0861cad38 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -367,7 +367,7 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly // run a temporary computation here to get what the unconstrained CMP would be with none of the scaling computeUnconstrainedFeedbackCMP(perfectCoP, perfectCMPOffset, unconstrainedFeedbackNoScaling, unconstrainedFeedbackCMPNoScaling); - computeFeedForwardAndFeedBackAlphas(); + //computeFeedForwardAndFeedBackAlphas(); referenceFeedForwardCMPOffset.setAndScale(1.0 - feedForwardAlpha.getDoubleValue(), perfectCMPOffset); referenceFeedForwardCoP.interpolate(perfectCoP, desiredICP, feedForwardAlpha.getDoubleValue()); 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 169d0dda5bd..ba990e1d3ec 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 @@ -68,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:
    From 59e0befb7ed829fd8c85c404948982994ced8b4a Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Fri, 22 Nov 2024 14:40:07 -0600 Subject: [PATCH 08/10] far too many changes for one commit, this is a mess --- .../avatar/AvatarStepGeneratorThread.java | 2 +- .../HumanoidKinematicsSimulation.java | 1 + ...ingControllerPreviewToolboxController.java | 1 + .../LinearMomentumRateControlModule.java | 93 ++++++++++++++++++- .../controller/ICPController.java | 2 +- .../controlModules/foot/SwingState.java | 10 +- .../ContinuousStepGenerator.java | 2 +- .../DynamicsBasedFootstepParameters.java | 4 +- .../DynamicsBasedFootstepPlugin.java | 66 +++++++++++-- ...amicsBasedFootstepTouchdownCalculator.java | 7 +- .../WholeBodyControllerCoreFactory.java | 1 + .../WalkingHighLevelHumanoidController.java | 2 +- .../states/TransferState.java | 6 ++ .../states/WalkingSingleSupportState.java | 63 +++++++++---- .../HighLevelHumanoidControllerToolbox.java | 8 ++ 15 files changed, 228 insertions(+), 40 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java index c42bbfac4bc..649b5b57e34 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java @@ -104,7 +104,7 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory, 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 78714226871..efce2b90e84 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 c074a75c41e..fd77326b491 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-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 9d63b8c415d..47c329e1cc7 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; @@ -156,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 FramePoint2D leadingPendulumBase = new FramePoint2D(); + private final FramePoint2D trailingPendulumBase = new FramePoint2D(); + private final FramePoint2D alternateCoP = new FramePoint2D(); + private final WalkingMessageHandler walkingMessageHandler; + public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox controllerToolbox, WalkingControllerParameters walkingControllerParameters, YoRegistry parentRegistry) @@ -165,6 +177,7 @@ public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox contro controllerToolbox.getContactableFeet(), controllerToolbox.getFullRobotModel().getElevator(), walkingControllerParameters, + controllerToolbox.getWalkingMessageHandler(), controllerToolbox.getTotalMassProvider(), controllerToolbox.getGravityZ(), controllerToolbox.getControlDT(), @@ -177,6 +190,7 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta SideDependentList contactableFeet, RigidBodyBasics elevator, WalkingControllerParameters walkingControllerParameters, + WalkingMessageHandler walkingMessageHandler, DoubleProvider totalMassProvider, double gravityZ, double controlDT, @@ -185,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); @@ -257,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.changeFrameAndProjectToXYPlane(worldFrame); + trailingPendulumBase.changeFrameAndProjectToXYPlane(worldFrame); + alternateCoP.changeFrameAndProjectToXYPlane(worldFrame); + alternateCoP.interpolate(trailingPendulumBase, leadingPendulumBase, alpha); + } + public void reset() { desiredLinearMomentumRateWeight.set(linearMomentumRateWeight); @@ -380,8 +463,14 @@ public boolean computeControllerCoreCommands() else desiredLinearMomentumRateWeight.update(linearMomentumRateWeight); + computeAlternateCoP(); + FixedFramePoint2DBasics desiredCoPToUse = desiredCoP; + + if (useAlternateCoP.getBooleanValue()) + desiredCoPToUse.setMatchingFrame(alternateCoP); + yoDesiredCMP.set(desiredCMP); - yoDesiredCoP.set(desiredCoP); + yoDesiredCoP.set(desiredCoPToUse); yoCenterOfMass.setFromReferenceFrame(centerOfMassFrame); yoCenterOfMassVelocity.set(capturePointCalculator.getCenterOfMassVelocity()); yoCapturePoint.set(capturePoint); @@ -397,7 +486,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; } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index ce0861cad38..577fda9b4ad 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -367,7 +367,7 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly // run a temporary computation here to get what the unconstrained CMP would be with none of the scaling computeUnconstrainedFeedbackCMP(perfectCoP, perfectCMPOffset, unconstrainedFeedbackNoScaling, unconstrainedFeedbackCMPNoScaling); - //computeFeedForwardAndFeedBackAlphas(); + computeFeedForwardAndFeedBackAlphas(); referenceFeedForwardCMPOffset.setAndScale(1.0 - feedForwardAlpha.getDoubleValue(), perfectCMPOffset); referenceFeedForwardCoP.interpolate(perfectCoP, desiredICP, feedForwardAlpha.getDoubleValue()); 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 b92de320c97..0eef6bcf9de 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 @@ -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 ba990e1d3ec..ae96517743b 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 @@ -186,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); 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 index 37dfd70c89d..0f0378deedd 100644 --- 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 @@ -15,7 +15,7 @@ public class DynamicsBasedFootstepParameters 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.8; + private static final double COM_HEIGHT = 0.9; private static final double POLE = 0.275; // YoVariables @@ -66,7 +66,7 @@ public DynamicsBasedFootstepParameters(double gravityZ, YoRegistry parentRegistr 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, 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)); 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 index 9573cd9fd2d..6ebe8498c31 100644 --- 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 @@ -12,6 +12,8 @@ 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; @@ -85,6 +87,11 @@ public enum FootState {SUPPORT, SWING} // private final SideDependentList pendulumBase = new SideDependentList<>(); private final FramePoint2D netPendulumBase = new FramePoint2D(); + private final SideDependentList pendulumBase3DInWorld = new SideDependentList<>(); + private final YoFramePoint3D netPendulumBase3DInWorld = new YoFramePoint3D("netPendulumBase3DInWorld" + variableNameSuffix, ReferenceFrame.getWorldFrame(), registry); + private final SideDependentList pendulumBaseViz = new SideDependentList<>(); + private final YoGraphicPosition netPendulumBaseViz; + // private final static Vector2DReadOnly zero2D = new Vector2D(); @@ -134,6 +141,13 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) 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 @@ -143,6 +157,19 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) 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(), @@ -165,7 +192,11 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent) 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 @@ -180,7 +211,9 @@ private SupportFootState(RobotSide robotSide) @Override public void onEntry() { - pendulumBase.get(robotSide).setFromReferenceFrame(referenceFrames.getSoleZUpFrame(robotSide)); + pendulumBase.get(robotSide).setToZero(referenceFrames.getSoleZUpFrame(robotSide)); + pendulumBase.get(robotSide).changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); + calculateNetPendulumBase(); if (inDoubleSupport.getBooleanValue()) calculate(touchdownCalculator.get(robotSide), @@ -195,6 +228,10 @@ public void onEntry() @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)); @@ -297,7 +334,12 @@ public void update(double time) 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, @@ -315,7 +357,7 @@ public void calculate(RobotSide swingSide, } private final FramePoint2DBasics tempPendulumBase = new FramePoint2D(); - private final FramePoint2DBasics tempNetPendulumBase = new FramePoint2D();; + private final FramePoint2DBasics tempNetPendulumBase = new FramePoint2D(); public void calculate(DynamicsBasedFootstepTouchdownCalculator touchdownCalculator, RobotSide swingSide, @@ -342,13 +384,15 @@ private void calculateNetPendulumBase() { if (footStates.get(RobotSide.LEFT).getEnumValue() == FootState.SUPPORT) { - netPendulumBase.setMatchingFrame(pendulumBase.get(RobotSide.LEFT)); + netPendulumBase.setIncludingFrame(pendulumBase.get(RobotSide.LEFT)); + netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(RobotSide.LEFT)); setTrailingSide(RobotSide.LEFT); } else { - netPendulumBase.setMatchingFrame(pendulumBase.get(RobotSide.RIGHT)); + netPendulumBase.setIncludingFrame(pendulumBase.get(RobotSide.RIGHT)); + netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(RobotSide.RIGHT)); setTrailingSide(RobotSide.RIGHT); } @@ -375,7 +419,7 @@ private void calculateNetPendulumBase() // 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.changeFrame(referenceFrames.getSoleZUpFrame(trailingSide)); + netPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(trailingSide)); } private void handleDesiredsFromProviders() @@ -550,13 +594,23 @@ private RobotSide getTrailingSide() return trailingSide; } + private final FramePoint2D tempTouchdownPosition = new FramePoint2D(); public void getDesiredTouchdownPosition2D(RobotSide robotSide, FixedFramePoint2DBasics touchdownPositionToPack) { - touchdownPositionToPack.setMatchingFrame(getDesiredTouchdownPosition2D(robotSide)); + 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); } 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 index 62653c51057..addd783fa14 100644 --- 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 @@ -42,6 +42,7 @@ public class DynamicsBasedFootstepTouchdownCalculator // 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 @@ -125,8 +126,9 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, // 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); - predictedVelocityAtTouchdown = new YoFrameVector2D(prefix + "PredictedVelocityAtTouchdown", centerOfMassControlZUpFrame, registry); + predictedVelocityAtTouchdown = new YoFrameVector2D(prefix + "PredictedVelocityAtTouchdownInWorld", ReferenceFrame.getWorldFrame(), registry); // Step position offsets for desired walking speed and stance width touchdownPositionOffsetForDesiredStanceWidth = new YoDouble(prefix + "TouchdownPositionOffsetForDesiredStanceWidth", registry); @@ -188,7 +190,7 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo computeDesiredALIPTouchdownPositionWithDS(timeToReachGoal, pendulumBase, netPendulumBase, isInDoubleSupport); desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithDS); - predictedVelocityAtTouchdown.set(predictedALIPVelocityAtTouchdownWithDS); + predictedVelocityAtTouchdown.setMatchingFrame(predictedALIPVelocityAtTouchdownWithDS); // compute offsets for desired walking speed and stance width computeDesiredTouchdownOffsetForVelocity(supportSide); @@ -206,6 +208,7 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo // determine touchdown z position from x position, y position, and ground plane desiredTouchdownPositionInWorld3D.setMatchingFrame(desiredTouchdownPosition2D, 0.0); + desiredTouchdownPositionInCOM2D.setMatchingFrame(desiredTouchdownPosition2D); } public FramePoint3DReadOnly getDesiredTouchdownPositionInWorld3D() 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 64f0aadbb92..4870ab0ba80 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/WalkingHighLevelHumanoidController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java index eacdc278c12..44b48fe01b2 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 6b8d05b4567..5480d622561 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 4e24fb54e40..b857fb3a68c 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 @@ -215,7 +215,7 @@ else if (resubmitStepsInSwingEveryTick.getBooleanValue()) // delayed for another reason. So we should check before updating step adjustment boolean footstepIsBeingAdjusted = false; if (!feetManager.getCurrentConstraintType(swingSide).isLoadBearing()) - footstepIsBeingAdjusted = true; //balanceManager.checkAndUpdateStepAdjustment(nextFootstep); + footstepIsBeingAdjusted = balanceManager.checkAndUpdateStepAdjustment(nextFootstep); if (footstepIsBeingAdjusted) { @@ -264,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); @@ -288,21 +296,35 @@ public boolean isDone(double timeInState) private void test() { - walkingMessageHandler.peekFootstep(0, nextFootstep); + if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) + { + walkingMessageHandler.peekFootstep(0, nextFootstep); - if (nextFootstep.getRobotSide() != swingSide || !haveWeEntered) - return; + if (nextFootstep.getRobotSide() != swingSide || !haveWeEntered) + return; - balanceManager.clearICPPlan(); + balanceManager.clearICPPlan(); - double finalTransferTime = walkingMessageHandler.getFinalTransferTime(); + swingTime = walkingMessageHandler.getNextSwingTime(); + + walkingMessageHandler.poll(nextFootstep, footstepTiming); + + + if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) + walkingMessageHandler.peekFootstep(0, nextNextFootstep); + + balanceManager.addFootstepToPlan(nextFootstep, footstepTiming); + } - 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); @@ -316,7 +338,6 @@ private void test() 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++) @@ -326,16 +347,17 @@ private void test() balanceManager.addFootstepToPlan(footsteps[i], footstepTimings[i]); } - /** This has to be called after calling initialize ICP Plan, as that resets the step constraints **/ - walkingMessageHandler.pollStepConstraints(stepConstraints); + walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); + failureDetectionControlModule.setNextFootstep(nextFootstep); - updateHeightManager(); + feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); - if (feetManager.adjustHeightIfNeeded(nextFootstep)) - { - walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep); - feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime); - } + balanceManager.adjustFootstepInCoPPlan(nextFootstep); + + // FIXME I don't need to be computing this again + balanceManager.computeICPPlan(); + + updateHeightManager(); } @Override @@ -352,6 +374,9 @@ public void onEntry() balanceManager.setICPPlanSupportSide(supportSide); balanceManager.initializeICPPlanForSingleSupport(); + /** This has to be called after calling initialize ICP Plan, as that resets the step constraints **/ + if (walkingMessageHandler.getCurrentNumberOfFootsteps() > 0) + walkingMessageHandler.pollStepConstraints(stepConstraints); balanceManager.setCurrentStepConstraints(stepConstraints); feetManager.requestSwing(swingSide, 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 1fdd36e3102..710df07dd19 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); From 3f3080491d4a6a6a92d4507f1b5f783a166b8931 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Tue, 3 Dec 2024 14:10:39 -0600 Subject: [PATCH 09/10] fixed empty support polygon bug --- .../walkingController/states/WalkingSingleSupportState.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java index b857fb3a68c..592221e1d58 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 @@ -201,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 @@ -534,7 +534,7 @@ public void handleChangeInContactState() return; controllerToolbox.updateBipedSupportPolygons(); - balanceManager.computeICPPlan(isFootInContact); + balanceManager.computeICPPlan(); } @Override From efe6fc38833def662460c0d260cfdf2fc720a957 Mon Sep 17 00:00:00 2001 From: Stefan Fasano Date: Wed, 4 Dec 2024 18:46:36 -0600 Subject: [PATCH 10/10] who even knows --- .../capturePoint/BalanceManager.java | 2 + .../LinearMomentumRateControlModule.java | 24 +++-- .../controlModules/foot/SwingState.java | 24 ++--- .../ContinuousStepGenerator.java | 7 +- .../DynamicsBasedFootstepPlugin.java | 42 ++++---- ...amicsBasedFootstepTouchdownCalculator.java | 99 +++++++++++++++++-- .../WalkingControllerState.java | 50 ++++++++++ 7 files changed, 196 insertions(+), 52 deletions(-) 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 d0ff0cf05db..58653c32037 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 47c329e1cc7..1f02e4ba1f1 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 @@ -163,9 +163,9 @@ public class LinearMomentumRateControlModule implements SCS2YoGraphicHolder private final SideDependentList contactableFeet; private final WalkingControllerParameters walkingControllerParameters; private final double controlDt; - private final FramePoint2D leadingPendulumBase = new FramePoint2D(); - private final FramePoint2D trailingPendulumBase = new FramePoint2D(); - private final FramePoint2D alternateCoP = new FramePoint2D(); + 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, @@ -337,9 +337,9 @@ else if (leftInContact && rightInContact) if (transferDuration > 0.0) alpha = MathTools.clamp(leadingTimeInContact / transferDuration, 0.0, 1.0); - leadingPendulumBase.changeFrameAndProjectToXYPlane(worldFrame); - trailingPendulumBase.changeFrameAndProjectToXYPlane(worldFrame); - alternateCoP.changeFrameAndProjectToXYPlane(worldFrame); + leadingPendulumBase.changeFrame(worldFrame); + trailingPendulumBase.changeFrame(worldFrame); + alternateCoP.changeFrame(worldFrame); alternateCoP.interpolate(trailingPendulumBase, leadingPendulumBase, alpha); } @@ -467,7 +467,7 @@ public boolean computeControllerCoreCommands() FixedFramePoint2DBasics desiredCoPToUse = desiredCoP; if (useAlternateCoP.getBooleanValue()) - desiredCoPToUse.setMatchingFrame(alternateCoP); + desiredCoPToUse.set(alternateCoP); yoDesiredCMP.set(desiredCMP); yoDesiredCoP.set(desiredCoPToUse); @@ -475,7 +475,10 @@ public boolean computeControllerCoreCommands() yoCenterOfMassVelocity.set(capturePointCalculator.getCenterOfMassVelocity()); yoCapturePoint.set(capturePoint); - success = success && computeDesiredLinearMomentumRateOfChangeNew(); + if (useAlternateCoP.getBooleanValue()) + success = success && computeDesiredLinearMomentumRateOfChangeNew(); + else + success = success && computeDesiredLinearMomentumRateOfChange(); selectionMatrix.setToLinearSelectionOnly(); selectionMatrix.selectLinearX(!useCenterOfPressureCommandOnly.getValue()); @@ -658,15 +661,16 @@ private boolean computeDesiredLinearMomentumRateOfChangeNew() double totalMass = totalMassProvider.getValue(); double desiredVerticalRateOfChange = totalMass * desiredCoMHeightAcceleration; - desiredPendulumBase.setIncludingFrame(desiredCoP, 0.0); + 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); +// linearMomentumRateOfChange.changeFrame(worldFrame); return true; } 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 0eef6bcf9de..e66b2aba547 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()) { 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 ae96517743b..23bb14aa3b7 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 @@ -309,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(); @@ -328,9 +328,11 @@ 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++) @@ -344,7 +346,6 @@ else if (statusToProcess == FootstepStatus.COMPLETED) { if (i == startingIndexToAdjust) { - dynamicsBasedFootstepPlugin.get().update(time); dynamicsBasedFootstepPlugin.get().getDesiredTouchdownPosition2D(swingSide, nextFootstepPose2D.getPosition()); } else 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 index 6ebe8498c31..b8799714ae2 100644 --- 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 @@ -215,14 +215,14 @@ public void onEntry() 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)); +// if (inDoubleSupport.getBooleanValue()) +// calculate(touchdownCalculator.get(robotSide), +// robotSide, +// getTransferDuration(robotSide), +// pendulumBase.get(robotSide.getOppositeSide()), +// netPendulumBase, +// inDoubleSupport.getBooleanValue(), +// desiredTouchdownPositions.get(robotSide)); } @Override @@ -235,14 +235,14 @@ public void doAction(double timeInState) 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)); +// 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 @@ -358,6 +358,7 @@ public void calculate(RobotSide swingSide, 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, @@ -368,13 +369,15 @@ public void calculate(DynamicsBasedFootstepTouchdownCalculator touchdownCalculat FixedFramePoint2DBasics touchdownPositionToPack) { tempPendulumBase.setIncludingFrame(pendulumBase); - tempPendulumBase.changeFrame(centerOfMassControlZUpFrame); + tempPendulumBase.changeFrameAndProjectToXYPlane(centerOfMassControlZUpFrame); tempNetPendulumBase.setIncludingFrame(netPendulumBase); - tempNetPendulumBase.changeFrame(referenceFrames.getSoleZUpFrame(getTrailingSide())); + tempNetPendulumBase.changeFrameAndProjectToXYPlane(referenceFrames.getSoleZUpFrame(getTrailingSide())); touchdownCalculator.computeDesiredTouchdownPosition(swingSide.getOppositeSide(), timeToReachGoal, tempPendulumBase, tempNetPendulumBase, isInDoubleSupport); - touchdownPositionToPack.setMatchingFrame(touchdownCalculator.getDesiredTouchdownPosition2D()); + tempTouchdownPosition.setIncludingFrame(touchdownCalculator.getDesiredTouchdownPosition2D()); + tempTouchdownPosition.changeFrameAndProjectToXYPlane(touchdownPositionToPack.getReferenceFrame()); + touchdownPositionToPack.set(tempTouchdownPosition); } private void calculateNetPendulumBase() @@ -594,7 +597,6 @@ private RobotSide getTrailingSide() return trailingSide; } - private final FramePoint2D tempTouchdownPosition = new FramePoint2D(); public void getDesiredTouchdownPosition2D(RobotSide robotSide, FixedFramePoint2DBasics touchdownPositionToPack) { tempTouchdownPosition.setIncludingFrame(getDesiredTouchdownPosition2D(robotSide)); 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 index addd783fa14..c53fbcc0f18 100644 --- 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 @@ -1,6 +1,7 @@ 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.*; @@ -12,6 +13,7 @@ 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; @@ -38,7 +40,9 @@ public class DynamicsBasedFootstepTouchdownCalculator // 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 YoFrameVector2D predictedVelocityAtTouchdown; + 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; @@ -128,7 +132,8 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, desiredTouchdownPositionInWorld3D = new YoFramePoint3D(prefix + "DesiredTouchdownPositionInWorld", ReferenceFrame.getWorldFrame(), registry); desiredTouchdownPositionInCOM2D = new YoFramePoint2D(prefix + "DesiredTouchdownPositionInCOM2D", centerOfMassControlZUpFrame, registry); desiredTouchdownPosition2D = new FramePoint2D(centerOfMassControlZUpFrame); - predictedVelocityAtTouchdown = new YoFrameVector2D(prefix + "PredictedVelocityAtTouchdownInWorld", ReferenceFrame.getWorldFrame(), registry); + 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); @@ -164,6 +169,39 @@ public DynamicsBasedFootstepTouchdownCalculator(RobotSide robotSide, 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(); @@ -184,13 +222,17 @@ public void computeDesiredTouchdownPosition(RobotSide supportSide, double timeTo if (pendulumBase != null) pendulumBase.checkReferenceFrameMatch(centerOfMassControlZUpFrame); + calculateCurrentAngularMomentum(pendulumBase, supportSide); + this.timeToReachGoal.set(timeToReachGoal); // compute all the touchdown positions and their offset values - computeDesiredALIPTouchdownPositionWithDS(timeToReachGoal, pendulumBase, netPendulumBase, isInDoubleSupport); + computeDesiredALIPTouchdownPosition(timeToReachGoal, pendulumBase); - desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithDS); - predictedVelocityAtTouchdown.setMatchingFrame(predictedALIPVelocityAtTouchdownWithDS); + desiredTouchdownPosition2D.set(desiredALIPTouchdownPositionWithoutDS); + predictedVelocityAtTouchdown2D.setIncludingFrame(predictedALIPVelocityAtTouchdownWithoutDS); + predictedVelocityAtTouchdown2D.changeFrameAndProjectToXYPlane(predictedVelocityAtTouchdown3D.getReferenceFrame()); + predictedVelocityAtTouchdown3D.set(predictedVelocityAtTouchdown2D); // compute offsets for desired walking speed and stance width computeDesiredTouchdownOffsetForVelocity(supportSide); @@ -221,9 +263,52 @@ public FramePoint2DReadOnly getDesiredTouchdownPosition2D() return desiredTouchdownPosition2D; } - public FrameVector2DReadOnly getPredictedVelocityAtTouchdown() + public FrameVector3DReadOnly getPredictedVelocityAtTouchdown3D() + { + return predictedVelocityAtTouchdown3D; + } + + private void computeDesiredALIPTouchdownPosition(double timeToReachGoal, FramePoint2DReadOnly pendulumBase) { - return predictedVelocityAtTouchdown; + + // 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) 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 f9c5ab23363..a60eaec65e0 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(); }