diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java new file mode 100644 index 000000000000..2732977ba40c --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java @@ -0,0 +1,332 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +import us.ihmc.commonWalkingControlModules.donkeyKick.KickParameters; +import us.ihmc.commonWalkingControlModules.capturePoint.ALIPTools; +import us.ihmc.commonWalkingControlModules.capturePoint.CapturePointTools; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickInputParameters; +import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CenterOfMassDynamicsTools; +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.FramePoint2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DReadOnly; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class KickDynamicPlanner +{ + private static final double assumedFinalShiftDuration = 0.2; + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + ///////// State variables + private final ReferenceFrame centerOfMassControlFrame; + private final SideDependentList soleFrames; + + ///////// Inputs + private final KickParameters kickParameters; + private final YoDouble estimatedTouchdownDuration = new YoDouble("estimatedTouchdownDuration", registry); + private final double gravityZ; + private final double totalMass; + + ///////// Resulting output variables that are computed + private final YoDouble angularMomentumFromImpact = new YoDouble("angularMomentumFromImpact", registry); + private final YoDouble equivalentVelocityChangeFromImpact = new YoDouble("equivalentVelocityChangeFromImpact", registry); + private final YoDouble acpShiftFromImpact = new YoDouble("acpShiftFromImpact", registry); + + private final FramePoint2D desiredSwingFootStart = new FramePoint2D(); + private final FramePoint2D desiredShiftCoP = new FramePoint2D(); + private final FramePoint2D desiredStanceCoP = new FramePoint2D(); + private final FramePoint2D goalFootPositionAtTouchdown = new FramePoint2D(); + + private final FramePoint2D goalACPPosition = new FramePoint2D(); + private final FramePoint2D acpInWorldAtTouchdown = new FramePoint2D(); + private final FramePoint2D acpInWorldAfterImpact = new FramePoint2D(); + private final FramePoint2D acpInWorldBeforeImpact = new FramePoint2D(); + private final FramePoint2D acpAfterShift = new FramePoint2D(); + + private final FramePoint2D comPositionBeforeImpact = new FramePoint2D(); + private final FramePoint2D comPositionAfterShift = new FramePoint2D(); + private final FramePoint2D comPositionBeforeShift = new FramePoint2D(); + + private final FrameVector2D angularMomentumBeforeImpact = new FrameVector2D(); + private final FrameVector2D angularMomentumAfterShift = new FrameVector2D(); + private final FrameVector2D angularMomentumBeforeShift = new FrameVector2D(); + + // temp variables + private final FramePoint3D tempPoint = new FramePoint3D(); + private final FramePoint3D acpInWorldAtTouchdown3D = new FramePoint3D(); + private final FramePoint3D desiredStanceCoP3D = new FramePoint3D(); + + public KickDynamicPlanner(KickParameters kickParameters, + SideDependentList soleFrames, + ReferenceFrame centerOfMassControlFrame, + double gravityZ, + double totalMass, + YoRegistry parentRegistry) + { + this.kickParameters = kickParameters; + this.centerOfMassControlFrame = centerOfMassControlFrame; + this.soleFrames = soleFrames; + // figure out the touchdown duration after the fact. + this.gravityZ = Math.abs(gravityZ); + this.totalMass = totalMass; + + if (parentRegistry != null) + parentRegistry.addChild(registry); + } + + public FramePoint2DReadOnly getGoalFootPositionAtTouchdown() + { + return goalFootPositionAtTouchdown; + } + + public FramePoint2DReadOnly getGoalACPPosition() + { + return goalACPPosition; + } + + public FramePoint2DReadOnly getACPPositionAtTouchdown() + { + return acpInWorldAtTouchdown; + } + + public FramePoint2DReadOnly getACPPositionAfterImpact() + { + return acpInWorldAfterImpact; + } + + public FramePoint2DReadOnly getACPPositionBeforeImpact() + { + return acpInWorldBeforeImpact; + } + + public FramePoint2DReadOnly getACPPositionAfterShift() + { + return acpAfterShift; + } + + public FramePoint2DReadOnly getDesiredStanceFootCoP() + { + return desiredStanceCoP; + } + + public FramePoint2DReadOnly getDesiredShiftCoP() + { + return desiredShiftCoP; + } + + public FramePoint2DReadOnly getDesiredSwingFootStartNominal() + { + return desiredSwingFootStart; + } + + public FramePoint2DReadOnly getCoMPositionBeforeImpact() + { + return comPositionBeforeImpact; + } + + public FramePoint2DReadOnly getCoMPositionAfterShift() + { + return comPositionAfterShift; + } + + public FramePoint2DReadOnly getCoMPositionBeforeShift() + { + return comPositionBeforeShift; + } + + public FrameVector2DReadOnly getAngularMomentumBeforeImpact() + { + return angularMomentumBeforeImpact; + } + + public FrameVector2DReadOnly getAngularMomentumAfterShift() + { + return angularMomentumAfterShift; + } + + public void compute(KickInputParameters inputParameters) + { + double touchdownDuration = inputParameters.getKickHeight() / kickParameters.getTouchdownHeightSpeed(); + estimatedTouchdownDuration.set(touchdownDuration); + kickParameters.setPreShiftWeightDistribution(inputParameters.getPrekickWeightDistribution()); + + RobotSide supportSide = inputParameters.getKickFootSide().getOppositeSide(); + double omega = Math.sqrt(gravityZ / kickParameters.getDesiredCoMHeight()); + angularMomentumFromImpact.set(getChangeInAngularMomentumFromImpact(inputParameters)); + equivalentVelocityChangeFromImpact.set(angularMomentumFromImpact.getDoubleValue() / (kickParameters.getDesiredCoMHeight() * totalMass)); + acpShiftFromImpact.set(equivalentVelocityChangeFromImpact.getDoubleValue() / omega); + + // This is the desired CoP position of the stance foot throughout the kick + desiredStanceCoP3D.setToZero(soleFrames.get(supportSide)); + desiredStanceCoP3D.setY(supportSide.negateIfLeftSide(kickParameters.getCopShiftInside())); + desiredStanceCoP3D.changeFrame(ReferenceFrame.getWorldFrame()); + desiredStanceCoP.setIncludingFrame(desiredStanceCoP3D); + + computeGoalFootstepPosition(inputParameters); + computeACPAtTouchdown(supportSide, omega); + computePositionAfterImpactFromGoal(omega); + computePositionBeforeImpact(omega, inputParameters); + computePositionAfterShift(omega); + computeInitialCoPAndCoM(inputParameters, omega); + } + + private void computeGoalFootstepPosition(KickInputParameters inputParameters) + { + RobotSide kickSide = inputParameters.getKickFootSide(); + RobotSide stanceSide = kickSide.getOppositeSide(); + + // Compute the foot position at touchdown, as given by the kick position. This is the foot position in the world once the kick is complete. + double distanceOfTouchdown = inputParameters.getKickTargetDistance() + kickParameters.getDesiredTouchdownPositionRelativeToTarget(); + + tempPoint.setToZero(soleFrames.get(kickSide)); + tempPoint.changeFrame(soleFrames.get(stanceSide)); + tempPoint.setX(-distanceOfTouchdown); + + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + goalFootPositionAtTouchdown.set(tempPoint); + } + + private void computeACPAtTouchdown(RobotSide supportSide, double omega) + { + ///// This method computes where the ACP should be after the final touchdown. When the foot hits the ground, the weight is shifted from one foot to the + ///// goal position, which is where the robot should be stably resting. + goalACPPosition.setIncludingFrame(goalFootPositionAtTouchdown); + goalACPPosition.changeFrameAndProjectToXYPlane(soleFrames.get(supportSide)); + goalACPPosition.scale(0.5); + goalACPPosition.addY(supportSide.negateIfLeftSide(kickParameters.getEndACPCheatInside())); + goalACPPosition.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + + tempPoint.setIncludingFrame(goalACPPosition, 0.0); + acpInWorldAtTouchdown3D.setToZero(ReferenceFrame.getWorldFrame()); + + CenterOfMassDynamicsTools.computeDesiredDCMPositionBackwardTime(omega, + assumedFinalShiftDuration, + assumedFinalShiftDuration, + tempPoint, + desiredStanceCoP3D, + tempPoint, + acpInWorldAtTouchdown3D); + acpInWorldAtTouchdown.set(acpInWorldAtTouchdown3D); + } + + // Back calculate the position of the ACP after the impact using the ACP at touchdown and the desired stance CoP + private void computePositionAfterImpactFromGoal(double omega) + { + double touchdownDuration = estimatedTouchdownDuration.getValue(); + + CapturePointTools.computeDesiredCapturePointPosition(omega, -touchdownDuration, acpInWorldAtTouchdown, desiredStanceCoP, acpInWorldAfterImpact); + } + + // Back calculate the position of the ACP before the impact using the ACP after the impact and the shift from the impact + private void computePositionBeforeImpact(double omega, KickInputParameters inputParameters) + { + tempPoint.setIncludingFrame(acpInWorldAfterImpact, 0.0); + tempPoint.changeFrame(centerOfMassControlFrame); + // this is the velocity change from the impact itself + tempPoint.subX(acpShiftFromImpact.getValue()); + + double angularMomentumFromImpact = getChangeInAngularMomentumFromImpact(inputParameters); + double velocityEquivalent = angularMomentumFromImpact / (totalMass * kickParameters.getDesiredCoMHeight()); + + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + acpInWorldBeforeImpact.set(tempPoint); + + // get the com position right before impact + tempPoint.changeFrame(centerOfMassControlFrame); + tempPoint.addX(velocityEquivalent / omega); + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + comPositionBeforeImpact.set(tempPoint); + + tempPoint.setToZero(centerOfMassControlFrame); + tempPoint.setY(-angularMomentumFromImpact); + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + angularMomentumBeforeImpact.set(tempPoint); + } + + // Back calculate the position of the ACP after the shift using the ACP before the impact + private void computePositionAfterShift(double omega) + { + // Compute the ACP translation during the kick itself + double kickMotionDuration = kickParameters.getPushDuration() + kickParameters.getChamberDuration(); + + CapturePointTools.computeDesiredCapturePointPosition(omega, -kickMotionDuration, acpInWorldBeforeImpact, desiredStanceCoP, acpAfterShift); + + ALIPTools.computeCenterOfMassPosition(-kickMotionDuration, + omega, + totalMass, + gravityZ, + comPositionBeforeImpact, + angularMomentumBeforeImpact, + desiredStanceCoP, + comPositionAfterShift); + ALIPTools.computeAngularMomentum(-kickMotionDuration, + omega, + totalMass, + gravityZ, + comPositionBeforeImpact, + angularMomentumBeforeImpact, + desiredStanceCoP, + angularMomentumAfterShift); + } + + private void computeInitialCoPAndCoM(KickInputParameters inputParameters, double omega) + { + RobotSide kickSide = inputParameters.getKickFootSide(); + RobotSide supportSide = kickSide.getOppositeSide(); + double shiftDuration = kickParameters.getShiftDuration(); + double exponential = Math.exp(omega * shiftDuration); + + ReferenceFrame supportSoleFrame = soleFrames.get(supportSide); + // from the y icp dynamics, we can figure out what the necessary distribution between the stance and swing foot are for weight for the constant cop + acpAfterShift.changeFrameAndProjectToXYPlane(supportSoleFrame); + comPositionAfterShift.changeFrameAndProjectToXYPlane(supportSoleFrame); + tempPoint.setToZero(soleFrames.get(kickSide)); + tempPoint.changeFrame(supportSoleFrame); + + double stanceWidth = tempPoint.getY(); + double initialWeightDistribution = kickParameters.getPreShiftWeightDistribution(); + double initialICPY = initialWeightDistribution * stanceWidth; + double desiredShiftCoPY = (-exponential * initialICPY + acpAfterShift.getY()) / (1.0 - exponential); + + // this is the fraction to go from the + double shiftWeightDistribution = desiredShiftCoPY / stanceWidth; + + // compute the necessary swing foot initial position + double desiredSwingFootInitialPositionX = comPositionAfterShift.getX() / ((initialWeightDistribution - shiftWeightDistribution) * Math.cosh(omega * kickParameters.getShiftDuration()) + shiftWeightDistribution); + double desiredShiftCoPX = shiftWeightDistribution * desiredSwingFootInitialPositionX; + + desiredShiftCoP.setIncludingFrame(supportSoleFrame, desiredShiftCoPX, desiredShiftCoPY); + desiredSwingFootStart.setIncludingFrame(supportSoleFrame, desiredSwingFootInitialPositionX, tempPoint.getY()); + + acpAfterShift.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + comPositionAfterShift.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + desiredShiftCoP.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + desiredSwingFootStart.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + + ALIPTools.computeCenterOfMassPosition(-shiftDuration, + omega, + totalMass, + gravityZ, + comPositionAfterShift, + angularMomentumAfterShift, + desiredStanceCoP, + comPositionBeforeShift); + ALIPTools.computeAngularMomentum(-shiftDuration, + omega, + totalMass, + gravityZ, + comPositionAfterShift, + angularMomentumAfterShift, + desiredStanceCoP, + angularMomentumBeforeShift); + } + + private static double getChangeInAngularMomentumFromImpact(KickInputParameters inputParameters) + { + return inputParameters.getKickImpulse() * inputParameters.getKickHeight(); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java new file mode 100644 index 000000000000..5db1e17172a4 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java @@ -0,0 +1,96 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.KickDoorCommand; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +public class KickInputParameters +{ + private YoEnum kickFootSide; // side for the foot to kick + private YoDouble kickHeight; // height of where you want the kick + private YoDouble kickImpulse; // Newton-seconds + private YoDouble kickTargetDistance; + private YoDouble prekickWeightDistribution; + + public KickInputParameters(YoRegistry parentRegistry) + { + YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + kickFootSide = new YoEnum<>("inputKickFootSide", registry, RobotSide.class); + kickHeight = new YoDouble("inputKickHeight", registry); + kickImpulse = new YoDouble("inputKickImpulse", registry); + kickTargetDistance = new YoDouble("inputKickTargetDistance", registry); + prekickWeightDistribution = new YoDouble("inputPrekickWeightDistribution", registry); + + // These default parameters are generally overwritten with set(KickDoorCommand kickMessage) + kickFootSide.set(RobotSide.LEFT); + kickHeight.set(0.55); + kickImpulse.set(55.0); + kickTargetDistance.set(0.75); + prekickWeightDistribution.set(0.5); + + if (parentRegistry != null) + parentRegistry.addChild(registry); + } + + public double getKickHeight() + { + return kickHeight.getDoubleValue(); + } + + public double getKickImpulse() + { + return kickImpulse.getDoubleValue(); + } + + public double getKickTargetDistance() + { + return kickTargetDistance.getDoubleValue(); + } + + public RobotSide getKickFootSide() + { + return kickFootSide.getEnumValue(); + } + + public double getPrekickWeightDistribution() + { + return prekickWeightDistribution.getDoubleValue(); + } + + public void setKickHeight(double kickHeight) + { + this.kickHeight.set(kickHeight); + } + + public void setKickImpulse(double kickImpulse) + { + this.kickImpulse.set(kickImpulse); + } + + public void setKickTargetDistance(double kickTargetDistance) + { + this.kickTargetDistance.set(kickTargetDistance); + } + + public void setKickFootSide(RobotSide kickFootSide) + { + this.kickFootSide.set(kickFootSide); + } + + public void setPrekickWeightDistribution(double prekickWeightDistribution) + { + this.prekickWeightDistribution.set(prekickWeightDistribution); + } + + public void set(KickDoorCommand kickMessage) + { + setKickFootSide(kickMessage.getRobotSide()); + setKickHeight(kickMessage.getKickHeight()); + setKickImpulse(kickMessage.getKickImpulse()); + setKickTargetDistance(kickMessage.getKickTargetDistance()); + setPrekickWeightDistribution(kickMessage.getPrekickWeightDistribution()); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java new file mode 100644 index 000000000000..05f641c0876b --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java @@ -0,0 +1,211 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +public class KickParameters +{ + private static final double desiredCoMHeight = 0.85; + + private static final double coefficientOfFriction = 0.75; + + // When computing the touchdown position to remain stable, this is the extra length to step, as a safety margin + private static final double extraStepLengthInTouchdownForSafety = 0.25; // m + // When computing the touchdown positino to remain stable, this is the minimum step length + private static final double minTouchdownStepDistance = 0.15; // m + + // This is how long we can estimate the force will be applied. This is used to determine how hard the foot should push for + private static final double estimatedImpactDuration = 0.05; // s + // This is the desired height of the foot at the end of chambering, relative to stance + private static final double kickChamberHeight = 0.18; // m + // This is the position of the foot at the end of chambering, relative to stance. + private static final double kickChamberX = 0.25; // m + // This is the mass of the kick leg to assume in order to generate the right amount of angular momentum + private static final double kickLegMass = 100.0; // kg + private static final double kickPickUpVelocity = 0.25; // m/s + + // This is a shift of the desired CoP position towards the inside of the foot when computing the dynamic planner. + private static final double copShiftInside = 0.02; // m + // This cheats the goal position of hte ACP at the touchdown event towards the inside, trying to incentivize the ACP position to be further + // horizontally the stance foot + private static final double endACPCheatInside = 0.02; // m + + // Defines how far past the target the touchdown position should be. Positive is further away, negative is closer. + private static final double desiredTouchdownPositionRelativeToTarget = -0.15; // m + private static final double touchdownHeightSpeed = 1.0; // m /s + + private double preShiftWeightDistribution = 0.75; // percentage + private static final double preShiftDuration = 2.5; // s + private static final double shiftDuration = 0.5; // s // TODO make this a computed value + private static final double chamberDuration = 0.25; // s + private static final double pushDuration = 0.2; // s + + private static final double maxAnkleVelocity = 2.5; // rad/s + + private static final double kickShiftProximityThreshold = 0.01; + private static final double kickMinTimeInContact = 0.05; + private static final double minFractionThroughTouchdownToDetectContact = 0.85; + + private static final double kickPenetrationThreshold = 0.04; + + private static final double kpICP = 15.0; + private static final double maxCMPFeedack = 0.10; + private static final double minCMPFeedack = 0.10; + private static final double cmpDistanceInside = 0.005; + private static final double cmpDistanceInsideInTouchdown = 0.025; + private static final double maxCMPRate = 5.0; + + private final KickingWBCCParameters fastWalkingWBCCParameters = new KickingWBCCParameters(); + + public void setPreShiftWeightDistribution(double preShiftWeightDistribution) + { + this.preShiftWeightDistribution = preShiftWeightDistribution; + } + + public double getDesiredCoMHeight() + { + return desiredCoMHeight; + } + + public double getCoefficientOfFriction() + { + return coefficientOfFriction; + } + + public double getExtraStepLengthInTouchdownForSafety() + { + return extraStepLengthInTouchdownForSafety; + } + + public double getMinTouchdownStepDistance() + { + return minTouchdownStepDistance; + } + + public double getEstimatedImpactDuration() + { + return estimatedImpactDuration; + } + + public double getKickChamberX() + { + return kickChamberX; + } + + public double getKickChamberHeight() + { + return kickChamberHeight; + } + + public double getKickLegMass() + { + return kickLegMass; + } + + public double getKickPickUpVelocity() + { + return kickPickUpVelocity; + } + + public double getCopShiftInside() + { + return copShiftInside; + } + + public double getEndACPCheatInside() + { + return endACPCheatInside; + } + + public double getDesiredTouchdownPositionRelativeToTarget() + { + return desiredTouchdownPositionRelativeToTarget; + } + + public double getTouchdownHeightSpeed() + { + return touchdownHeightSpeed; + } + + public double getPreShiftWeightDistribution() + { + return preShiftWeightDistribution; + } + + public double getPreShiftDuration() + { + return preShiftDuration; + } + + public double getShiftDuration() + { + return shiftDuration; + } + + public double getChamberDuration() + { + return chamberDuration; + } + + public double getPushDuration() + { + return pushDuration; + } + + public double getMaxAnkleVelocity() + { + return maxAnkleVelocity; + } + + public double getKickShiftProximityThreshold() + { + return kickShiftProximityThreshold; + } + + public double getKickMinTimeInContact() + { + return kickMinTimeInContact; + } + + public double getMinFractionThroughTouchdownToDetectContact() + { + return minFractionThroughTouchdownToDetectContact; + } + + public double getKpICP() + { + return kpICP; + } + + public double getMaxCMPFeedack() + { + return maxCMPFeedack; + } + + public double getMinCMPFeedack() + { + return minCMPFeedack; + } + + public double getCmpDistanceInside() + { + return cmpDistanceInside; + } + + public double getCmpDistanceInsideInTouchdown() + { + return cmpDistanceInsideInTouchdown; + } + + public double getMaxCMPRate() + { + return maxCMPRate; + } + + public double getKickPenetrationThreshold() + { + return kickPenetrationThreshold; + } + + public KickingWBCCParameters getKickingWBCCParameters() + { + return fastWalkingWBCCParameters; + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java new file mode 100644 index 000000000000..f339cdfc6a75 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java @@ -0,0 +1,176 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.robotics.controllers.pidGains.*; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPIDSE3Gains; +import us.ihmc.robotics.controllers.pidGains.implementations.PDGains; +import us.ihmc.robotics.weightMatrices.SolverWeightLevels; + +public class KickingWBCCParameters +{ + private final PID3DGains chestOrientationGains = new DefaultPID3DGains(); + private final static double chestOrientationWeight = 50.0; + + private final PID3DGains pelvisOrientationGains = new DefaultPID3DGains(); + private final static double pelvisOrientationWeight = 50.0; + + private final static double stanceFootWeight = SolverWeightLevels.FOOT_SUPPORT_WEIGHT; + + private final PID3DGains chamberFootGains = new DefaultPID3DGains(); + private final static double chamberFootWeight = 50.0; + + private final PID3DGains touchdownFootGains = new DefaultPID3DGains(); + private final static double touchdownFootWeight = 150.0; + + private final PID3DGains settlingFootGains = new DefaultPID3DGains(); + + private final PID3DGains pushFootGains = new DefaultPID3DGains(); + private final static double pushFootWeight = 100.0; + + private final PDGains kickingAnkleGains = new PDGains(); + private final PDGains kickingHipYawGains = new PDGains(); + private final static double kickingAnkleWeight = 50.0; + private final static double kickingHipYawWeight = 50.0; + + private final static double angularMomentumRateWeight = 5.0; + private final static Vector3DReadOnly linearMomentumRateWeight = new Vector3D(0.1, 0.1, 0.1); + + private final PDGains standingHeightControlGains = new PDGains(); + + private final static double desiredCoPPositionWeight = 10.0; + + public KickingWBCCParameters() + { + standingHeightControlGains.setKp(50.0); + standingHeightControlGains.setZeta(0.7); + + chamberFootGains.setProportionalGains(300.0, 300.0, 100.0); + chamberFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(chamberFootGains.getProportionalGains()[0], 0.7), + GainCalculator.computeDerivativeGain(chamberFootGains.getProportionalGains()[1], 0.7), + GainCalculator.computeDerivativeGain(chamberFootGains.getProportionalGains()[2], 0.2)); + + touchdownFootGains.setProportionalGains(300.0); + touchdownFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(300.0, 0.7)); + + settlingFootGains.setProportionalGains(200.0); + settlingFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(200.0, 0.7)); + + pushFootGains.setProportionalGains(100.0, 50.0, 5.0); + pushFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(pushFootGains.getProportionalGains()[0], 0.7), + GainCalculator.computeDerivativeGain(pushFootGains.getProportionalGains()[1], 0.7), + GainCalculator.computeDerivativeGain(pushFootGains.getProportionalGains()[2], 0.7)); + + chestOrientationGains.setProportionalGains(40.0); + chestOrientationGains.setDerivativeGains(GainCalculator.computeDerivativeGain(40.0, 0.8)); + + pelvisOrientationGains.setProportionalGains(100.0); + pelvisOrientationGains.setDerivativeGains(GainCalculator.computeDerivativeGain(100.0, 0.8)); + + kickingAnkleGains.setKp(50.0); + kickingAnkleGains.setZeta(0.75); + kickingHipYawGains.setKp(50.0); + kickingHipYawGains.setZeta(0.75); + } + + public PDGainsReadOnly getStandingHeightControlGains() + { + return standingHeightControlGains; + } + + public double getChestOrientationWeight() + { + return chestOrientationWeight; + } + + public double getPelvisOrientationWeight() + { + return pelvisOrientationWeight; + } + + public double getChamberFootWeight() + { + return chamberFootWeight; + } + + public double getTouchdownFootWeight() + { + return touchdownFootWeight; + } + + public PID3DGainsReadOnly getChamberFootGains() + { + return chamberFootGains; + } + + public PID3DGainsReadOnly getTouchdownFootGains() + { + return touchdownFootGains; + } + + public PID3DGainsReadOnly getSettlingFootGains() + { + return settlingFootGains; + } + + public double getPushFootWeight() + { + return pushFootWeight; + } + + public PID3DGainsReadOnly getPushFootGains() + { + return pushFootGains; + } + + public PID3DGainsReadOnly getChestOrientationGains() + { + return chestOrientationGains; + } + + public PID3DGainsReadOnly getPelvisOrientationGains() + { + return pelvisOrientationGains; + } + + public Vector3DReadOnly getLinearMomentumRateWeight() + { + return linearMomentumRateWeight; + } + + public double getAngularMomentumRateWeight() + { + return angularMomentumRateWeight; + } + + public double getStanceFootWeight() + { + return stanceFootWeight; + } + + public PDGainsReadOnly getKickingAnkleGains() + { + return kickingAnkleGains; + } + + public PDGainsReadOnly getKickingHipYawGains() + { + return kickingHipYawGains; + } + + public double getKickingAnkleWeight() + { + return kickingAnkleWeight; + } + + public double getKickingHipYawWeight() + { + return kickingHipYawWeight; + } + + public double getDesiredCoPPositionWeight() + { + return desiredCoPPositionWeight; + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java index 5a844bad24f7..76844cbf020e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java @@ -70,6 +70,7 @@ public class ControllerAPIDefinition commands.add(AbortWalkingCommand.class); commands.add(PrepareForLocomotionCommand.class); commands.add(PauseWalkingCommand.class); + commands.add(KickDoorCommand.class); commands.add(SpineDesiredAccelerationsCommand.class); commands.add(HandLoadBearingCommand.class); commands.add(HandHybridJointspaceTaskspaceTrajectoryCommand.class); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java index 92368b66e6fe..a58fda9d1b17 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java @@ -18,40 +18,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.communication.controllerAPI.command.Command; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.AbortWalkingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ArmDesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ArmTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.AutomaticManipulationAbortCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.CenterOfMassTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ChestHybridJointspaceTaskspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ChestTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.DesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootLoadBearingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataListCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.GoHomeCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandHybridJointspaceTaskspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandLoadBearingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandWrenchTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HeadHybridJointspaceTaskspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HeadTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.JointspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.LegTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.MomentumTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.NeckDesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.NeckTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PauseWalkingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PelvisHeightTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PelvisOrientationTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PelvisTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PrepareForLocomotionCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SpineDesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SpineTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.StopAllTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.WrenchTrajectoryControllerCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.*; import us.ihmc.humanoidRobotics.communication.directionalControlToolboxAPI.DirectionalControlInputCommand; import us.ihmc.humanoidRobotics.communication.fastWalkingAPI.FastWalkingGaitParametersCommand; import us.ihmc.humanoidRobotics.communication.packets.walking.HumanoidBodyPart; @@ -156,6 +123,7 @@ public WalkingCommandConsumer(CommandInputManager commandInputManager, commandsToRegister.add(CenterOfMassTrajectoryCommand.class); commandsToRegister.add(AbortWalkingCommand.class); commandsToRegister.add(PrepareForLocomotionCommand.class); + commandsToRegister.add(KickDoorCommand.class); commandsToRegister.add(DirectionalControlInputCommand.class); commandsToRegister.add(FastWalkingGaitParametersCommand.class); @@ -605,6 +573,15 @@ public void consumeAbortWalkingCommands(YoBoolean abortWalkingRequested) abortWalkingRequested.set(commandConsumerWithDelayBuffers.pollNewestCommand(AbortWalkingCommand.class).isAbortWalkingRequested()); } + public KickDoorCommand consumeTriggerKickCommands() + { + if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(KickDoorCommand.class)) + return null; + + KickDoorCommand command = commandConsumerWithDelayBuffers.pollNewestCommand(KickDoorCommand.class); + return command; + } + public void consumePrepareForLocomotionCommands() { if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(PrepareForLocomotionCommand.class)) diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java index 40d91a713947..67114a78352d 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java @@ -95,6 +95,9 @@ public final class ControllerAPI inputMessageClasses.add(WholeBodyTrajectoryMessage.class); inputMessageClasses.add(WholeBodyStreamingMessage.class); + // Commands supported by the kicking controller + inputMessageClasses.add(KickDoorMessage.class); + // Statuses supported by bipedal walking controller {@link WalkingControllerState} outputMessageClasses.add(CapturabilityBasedStatus.class); outputMessageClasses.add(FootstepStatusMessage.class); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java index eeee9f54b3b7..dd41a9b7c89e 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java @@ -220,6 +220,14 @@ private void renderMenuBar(SceneGraphModificationQueue modificationQueue) modificationQueue.accept(new SceneGraphNodeAddition(rightDoorPanel.getSceneNode(), predefinedRigidBodySceneNodeBuilder.getParent())); addUISceneNode(rightDoorPanel); } + //TODO: pattern match this to add a reference frame graphic for the door handle to be dragged around + if (ImGui.button(labels.get("Add Door Handle"))) + { + RDXPredefinedRigidBodySceneNode doorHandle = predefinedRigidBodySceneNodeBuilder.build("Box"); + modificationQueue.accept(new SceneGraphNodeAddition(doorHandle.getSceneNode(), predefinedRigidBodySceneNodeBuilder.getParent())); + addUISceneNode(doorHandle); + } + ImGui.endTable(); } ImGui.endDisabled(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java new file mode 100644 index 000000000000..5a0386151ee3 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java @@ -0,0 +1,92 @@ +package us.ihmc.rdx.ui.behavior.actions; + +import imgui.ImGui; +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.behaviors.sequence.actions.KickDoorActionDefinition; +import us.ihmc.behaviors.sequence.actions.KickDoorActionState; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; +import us.ihmc.rdx.imgui.ImDoubleWrapper; +import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.rdx.ui.RDXBaseUI; +import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class RDXKickDoorAction extends RDXActionNode +{ + private final DRCRobotModel robotModel; + private final ROS2SyncedRobotModel syncedRobot; + private final KickDoorActionState state; + private final KickDoorActionDefinition definition; + private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final ImDoubleWrapper kickHeight; + private final ImDoubleWrapper kickImpulse; + private final ImDoubleWrapper kickDistance; + private final ImDoubleWrapper preKickWeightDistribution; + + public RDXKickDoorAction(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + RDXBaseUI baseUI, + DRCRobotModel robotModel, + ROS2SyncedRobotModel syncedRobot, + ReferenceFrameLibrary referenceFrameLibrary, + DefaultFootstepPlannerParametersBasics footstepPlannerParameters) + { + super(new KickDoorActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + this.robotModel = robotModel; + this.syncedRobot = syncedRobot; + + definition.setName("Kick Door"); + + kickHeight = new ImDoubleWrapper(definition::getKickHeight, + definition::setKickHeight, + imDouble -> ImGui.inputDouble(labels.get("Kick height"), imDouble)); + kickImpulse = new ImDoubleWrapper(definition::getKickImpulse, + definition::setKickImpulse, + imDouble -> ImGui.inputDouble(labels.get("Kick impulse"), imDouble)); + kickDistance = new ImDoubleWrapper(definition::getKickTargetDistance, + definition::setKickTargetDistance, + imDouble -> ImGui.inputDouble(labels.get("Kick distance"), imDouble)); + preKickWeightDistribution = new ImDoubleWrapper(definition::getPrekickWeightDistribution, + definition::setPrekickWeightDistribution, + imDouble -> ImGui.inputDouble(labels.get("Weight distribution"), imDouble)); + } + + @Override + public void update() + { + super.update(); + } + + @Override + public void renderTreeViewIconArea() + { + super.renderTreeViewIconArea(); + + ImGui.sameLine(); + } + + @Override + protected void renderImGuiWidgetsInternal() + { + ImGui.pushItemWidth(80.0f); + kickHeight.renderImGuiWidget(); + kickImpulse.renderImGuiWidget(); + kickDistance.renderImGuiWidget(); + preKickWeightDistribution.renderImGuiWidget(); + ImGui.popItemWidth(); + } + + @Override + public String getActionTypeTitle() + { + return "Kick Door"; + } +} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java new file mode 100644 index 000000000000..7dbd443ef0d6 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java @@ -0,0 +1,191 @@ +package us.ihmc.rdx.ui.behavior.actions; + +import com.badlogic.gdx.graphics.g3d.Renderable; +import com.badlogic.gdx.utils.Array; +import com.badlogic.gdx.utils.Pool; +import imgui.ImGui; +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.behaviors.sequence.actions.KickDoorApproachPlanActionDefinition; +import us.ihmc.behaviors.sequence.actions.KickDoorApproachPlanActionState; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.packets.ExecutionMode; +import us.ihmc.rdx.imgui.ImDoubleWrapper; +import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.rdx.input.ImGui3DViewInput; +import us.ihmc.rdx.ui.RDXBaseUI; +import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode; +import us.ihmc.rdx.ui.graphics.RDXFootstepGraphic; +import us.ihmc.rdx.vr.RDXVRContext; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class RDXKickDoorApproachPlanAction extends RDXActionNode +{ + private final KickDoorApproachPlanActionState state; + private final KickDoorApproachPlanActionDefinition definition; + private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final ImDoubleWrapper swingDurationWidget; + private final ImDoubleWrapper transferDurationWidget; + private final ImDoubleWrapper kickImpulseWidget; + private final ImDoubleWrapper kickTargetDistanceWidget; + private final ImDoubleWrapper horizontalDistanceFromHandleWidget; + private final ImDoubleWrapper preKickWeightDistributionWidget; + private final ImDoubleWrapper stanceFootWidthWidget; + private final ImDoubleWrapper kickHeightWidget; + private final SideDependentList goalFeetGraphics = new SideDependentList<>(); + private final RDXFootstepGraphic kickGoalGraphic; + + public RDXKickDoorApproachPlanAction(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + RDXBaseUI baseUI, + DRCRobotModel robotModel, + ReferenceFrameLibrary referenceFrameLibrary) + { + super(new KickDoorApproachPlanActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + definition.setName("Footstep plan"); + + swingDurationWidget = new ImDoubleWrapper(definition::getSwingDuration, + definition::setSwingDuration, + imDouble -> ImGui.inputDouble(labels.get("Swing duration"), imDouble)); + transferDurationWidget = new ImDoubleWrapper(definition::getTransferDuration, + definition::setTransferDuration, + imDouble -> ImGui.inputDouble(labels.get("Transfer duration"), imDouble)); + + kickImpulseWidget = new ImDoubleWrapper(definition.getKickImpulse()::getValue, + definition.getKickImpulse()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Kick impulse"), imDouble)); + kickTargetDistanceWidget = new ImDoubleWrapper(definition.getKickTargetDistance()::getValue, + definition.getKickTargetDistance()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Kick Target Distance"), imDouble)); + horizontalDistanceFromHandleWidget = new ImDoubleWrapper(definition.getHorizontalDistanceFromHandle()::getValue, + definition.getHorizontalDistanceFromHandle()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Horizontal distance from handle"), imDouble)); + preKickWeightDistributionWidget = new ImDoubleWrapper(definition.getPrekickWeightDistribution()::getValue, + definition.getPrekickWeightDistribution()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Prekick weight distribution"), imDouble)); + stanceFootWidthWidget = new ImDoubleWrapper(definition.getStanceFootWidth()::getValue, + definition.getStanceFootWidth()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Stance foot width"), imDouble)); + kickHeightWidget = new ImDoubleWrapper(definition.getKickHeight()::getValue, + definition.getKickHeight()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Kick height"), imDouble)); + + for (RobotSide side : RobotSide.values) + { + RDXFootstepGraphic goalFootGraphic = new RDXFootstepGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints(), side); + goalFootGraphic.create(); + goalFeetGraphics.put(side, goalFootGraphic); + } + kickGoalGraphic = new RDXFootstepGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints(), RobotSide.LEFT); + kickGoalGraphic.create(); + + + baseUI.getPrimary3DPanel().addImGuiOverlayAddition(this::render3DPanelImGuiOverlays); + } + + @Override + public void update() + { + super.update(); + + if (state.areFramesInWorld()) + { + // Add a footstep to the action data only + + // Update arrow graphic geometry + + kickGoalGraphic.setPose(state.getKickGoalPose().getValueReadOnly()); + goalFeetGraphics.get(RobotSide.LEFT).setPose(state.getLeftFootGoalPose().getValueReadOnly()); + goalFeetGraphics.get(RobotSide.RIGHT).setPose(state.getRightFootGoalPose().getValueReadOnly()); + } + } + + @Override + public void renderTreeViewIconArea() + { + super.renderTreeViewIconArea(); + + ImGui.sameLine(); + } + + @Override + public void calculateVRPick(RDXVRContext vrContext) + { + } + + @Override + public void processVRInput(RDXVRContext vrContext) + { + } + + @Override + public void calculate3DViewPick(ImGui3DViewInput input) + { + } + + @Override + public void process3DViewInput(ImGui3DViewInput input) + { + + } + + @Override + protected void renderImGuiWidgetsInternal() + { + ImGui.pushItemWidth(80.0f); + swingDurationWidget.renderImGuiWidget(); + transferDurationWidget.renderImGuiWidget(); + kickImpulseWidget.renderImGuiWidget(); + kickTargetDistanceWidget.renderImGuiWidget(); + preKickWeightDistributionWidget.renderImGuiWidget(); + horizontalDistanceFromHandleWidget.renderImGuiWidget(); + stanceFootWidthWidget.renderImGuiWidget(); + kickHeightWidget.renderImGuiWidget(); + ImGui.popItemWidth(); + ImGui.text("Execution mode:"); + ImGui.sameLine(); + if (ImGui.radioButton(labels.get("Override"), definition.getExecutionMode().getValue() == ExecutionMode.OVERRIDE)) + definition.getExecutionMode().setValue(ExecutionMode.OVERRIDE); + ImGui.sameLine(); + if (ImGui.radioButton(labels.get("Queue"), definition.getExecutionMode().getValue() == ExecutionMode.QUEUE)) + definition.getExecutionMode().setValue(ExecutionMode.QUEUE); + } + + @Override + public void deselectGizmos() + { + } + + public void render3DPanelImGuiOverlays() + { + + } + + @Override + public void getRenderables(Array renderables, Pool pool) + { + if (state.areFramesInWorld()) + { + for (RobotSide side : RobotSide.values) + { + goalFeetGraphics.get(side).setHighlighted(false); + goalFeetGraphics.get(side).getRenderables(renderables, pool); + } + kickGoalGraphic.setHighlighted(false); + kickGoalGraphic.getRenderables(renderables, pool); + } + } + + @Override + public String getActionTypeTitle() + { + return "Kick Door Approach Plan"; + } +} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java index c6e838adc32c..c147208aebb6 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java @@ -2,6 +2,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeDefinition; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeStateBuilder; import us.ihmc.behaviors.behaviorTree.BehaviorTreeRootNodeDefinition; @@ -11,6 +12,8 @@ import us.ihmc.behaviors.sequence.ActionNodeInitialization; import us.ihmc.behaviors.sequence.ActionSequenceDefinition; import us.ihmc.behaviors.sequence.actions.*; +import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.rdx.ui.RDX3DPanel; @@ -156,6 +159,14 @@ public RDXBehaviorTreeNodeBuilder(DRCRobotModel robotModel, selectionCollisionModel, referenceFrameLibrary); } + if (nodeType == KickDoorActionDefinition.class) + { + return new RDXKickDoorAction(id, crdtInfo, saveFileDirectory, baseUI, robotModel, syncedRobot, referenceFrameLibrary, footstepPlannerParametersBasics); + } + if (nodeType == KickDoorApproachPlanActionDefinition.class) + { + return new RDXKickDoorApproachPlanAction(id, crdtInfo, saveFileDirectory, baseUI, robotModel, referenceFrameLibrary); + } else { return null; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java index 0f1f8cee7560..f68da7280a25 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java @@ -119,6 +119,12 @@ public void renderImGuiWidgets(RDXBehaviorTreeNode relativeNode, BehaviorT ImGui.sameLine(); renderNodeCreationClickable(relativeNode, insertionType, side.getPascalCaseName(), HandWrenchActionDefinition.class, side); } + ImGui.text("Kick Door: "); + for (RobotSide side : RobotSide.values) + { + ImGui.sameLine(); + renderNodeCreationClickable(relativeNode, insertionType, side.getPascalCaseName(), KickDoorActionDefinition.class, side); + } ImGui.unindent(); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java index 4fda362fc30f..8b4b65b3712a 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java @@ -40,6 +40,10 @@ public static BehaviorTreeNodeDefinition createNode(Class definitionType, CRD { return new FootstepPlanActionDefinition(crdtInfo, saveFileDirectory); } + if (definitionType == KickDoorApproachPlanActionDefinition.class) + { + return new KickDoorApproachPlanActionDefinition(crdtInfo, saveFileDirectory); + } if (definitionType == HandPoseActionDefinition.class) { return new HandPoseActionDefinition(crdtInfo, saveFileDirectory); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java index 4a52d04ab5b6..08f83d454ec7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java @@ -100,6 +100,19 @@ public BehaviorTreeExecutorNodeBuilder(DRCRobotModel robotModel, footstepPlanner, footstepPlannerParameters); } + if (nodeType == KickDoorApproachPlanActionDefinition.class) + { + return new KickDoorApproachPlanActionExecutor(id, + crdtInfo, + saveFileDirectory, + ros2ControllerHelper, + syncedRobot, + controllerStatusTracker, + referenceFrameLibrary, + walkingControllerParameters, + footstepPlanner, + footstepPlannerParameters); + } if (nodeType == HandPoseActionDefinition.class) { return new HandPoseActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, referenceFrameLibrary, robotModel, syncedRobot); @@ -128,7 +141,11 @@ public BehaviorTreeExecutorNodeBuilder(DRCRobotModel robotModel, { return new FootPoseActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, referenceFrameLibrary, syncedRobot); } + if (nodeType == KickDoorActionDefinition.class) + { + return new KickDoorActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, syncedRobot, referenceFrameLibrary); + } - return null; + throw new RuntimeException("Node defintion type not found: " + nodeType.getSimpleName()); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index abec8bba9563..21e2c286c6cd 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -35,6 +35,8 @@ public static void clearLists(BehaviorTreeStateMessage treeStateMessage) treeStateMessage.getSakeHandCommandActions().clear(); treeStateMessage.getWaitDurationActions().clear(); treeStateMessage.getFootPoseActions().clear(); + treeStateMessage.getKickDoorActions().clear(); + treeStateMessage.getKickDoorApproachActions().clear(); } public static void packMessage(BehaviorTreeNodeState nodeState, BehaviorTreeStateMessage treeStateMessage) @@ -123,6 +125,18 @@ else if (nodeState instanceof FootPoseActionState footPoseActionState) treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getFootPoseActions().size()); footPoseActionState.toMessage(treeStateMessage.getFootPoseActions().add()); } + else if (nodeState instanceof KickDoorActionState kickDoorActionState) + { + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_ACTION); + treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); + kickDoorActionState.toMessage(treeStateMessage.getKickDoorActions().add()); + } + else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) + { + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION); + treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); + kickDoorApproachPlanActionState.toMessage(treeStateMessage.getKickDoorApproachActions().add()); + } else { treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.BASIC_NODE); @@ -191,6 +205,14 @@ else if (nodeState instanceof FootPoseActionState footPoseActionState) { footPoseActionState.fromMessage(subscriptionNode.getFootPoseActionStateMessage()); } + else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) + { + kickDoorApproachPlanActionState.fromMessage(subscriptionNode.getKickDoorApproachPlanActionStateMessage()); + } + else if (nodeState instanceof KickDoorActionState kickDoorActionState) + { + kickDoorActionState.fromMessage(subscriptionNode.getKickDoorActionStateMessage()); + } else { nodeState.fromMessage(subscriptionNode.getBehaviorTreeNodeStateMessage()); @@ -308,6 +330,23 @@ public static void packSubscriptionNode(byte nodeType, subscriptionNode.setBehaviorTreeNodeStateMessage(footPoseActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(footPoseActionStateMessage.getDefinition().getDefinition().getDefinition()); } + case BehaviorTreeStateMessage.KICK_DOOR_ACTION -> + { + KickDoorActionStateMessage kickDoorActionStateMessage = treeStateMessage.getKickDoorActions().get(indexInTypesList); + subscriptionNode.setKickDoorActionStateMessage(kickDoorActionStateMessage); + subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorActionStateMessage.getState().getState()); + subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorActionStateMessage.getDefinition().getDefinition().getDefinition()); + } + case BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION -> + { + KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage = treeStateMessage.getKickDoorApproachActions().get(indexInTypesList); + subscriptionNode.setKickDoorApproachPlanActionStateMessage(kickDoorApproachPlanActionStateMessage); + subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorApproachPlanActionStateMessage.getState().getState()); + subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorApproachPlanActionStateMessage.getDefinition().getDefinition().getDefinition()); + } + default -> + throw new RuntimeException("Undefined behavior tree state message type."); + } } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java index 683c4d8686c8..42f5cb67795d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java @@ -25,6 +25,8 @@ public class ROS2BehaviorTreeSubscriptionNode private PelvisHeightOrientationActionStateMessage pelvisHeightOrientationActionStateMessage; private WaitDurationActionStateMessage waitDurationActionStateMessage; private FootPoseActionStateMessage footPoseActionStateMessage; + private KickDoorActionStateMessage kickDoorActionStateMessage; + private KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage; private final List children = new ArrayList<>(); public void clear() @@ -47,6 +49,8 @@ public void clear() pelvisHeightOrientationActionStateMessage = null; waitDurationActionStateMessage = null; footPoseActionStateMessage = null; + kickDoorActionStateMessage = null; + kickDoorApproachPlanActionStateMessage = null; children.clear(); } @@ -230,6 +234,27 @@ public void setFootPoseActionStateMessage(FootPoseActionStateMessage footPoseAct this.footPoseActionStateMessage = footPoseActionStateMessage; } + public KickDoorActionStateMessage getKickDoorActionStateMessage() + { + return kickDoorActionStateMessage; + } + + public void setKickDoorActionStateMessage(KickDoorActionStateMessage kickDoorActionStateMessage) + { + this.kickDoorActionStateMessage = kickDoorActionStateMessage; + } + + public KickDoorApproachPlanStateMessage getKickDoorApproachPlanActionStateMessage() + { + return kickDoorApproachPlanActionStateMessage; + } + + public void setKickDoorApproachPlanActionStateMessage(KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage) + { + this.kickDoorApproachPlanActionStateMessage = kickDoorApproachPlanActionStateMessage; + } + + public List getChildren() { return children; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java index e8dcc05d7a23..a4d5cc00c42c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java @@ -53,6 +53,36 @@ else if (newAction instanceof ScrewPrimitiveActionState screwPrimitiveAction) .setObjectFrameName(findConvenientParentFrameName(actionSequence, HandPoseActionState.class, indexOfInsertion, sideOfNewAction)); screwPrimitiveAction.getState().update(); } + else if ( newAction instanceof KickDoorApproachPlanActionState kickDoorApproachPlanAction) + { + KickDoorApproachPlanActionState nextPreviousFootstepPlanAction = findNextPreviousAction(actionSequence, KickDoorApproachPlanActionState.class, indexOfInsertion, null); + if (nextPreviousFootstepPlanAction != null) + { + kickDoorApproachPlanAction.getDefinition().setParentFrameName(nextPreviousFootstepPlanAction.getDefinition().getParentFrameName()); + } + else + { + kickDoorApproachPlanAction.getDefinition().setParentFrameName(syncedRobot.getReferenceFrames().getMidFeetUnderPelvisFrame().getName()); + } + + kickDoorApproachPlanAction.getDefinition().setSide(sideOfNewAction); + kickDoorApproachPlanAction.update(); + } + else if ( newAction instanceof KickDoorActionState kickDoorAction) + { + KickDoorActionState nextPreviousFootstepPlanAction = findNextPreviousAction(actionSequence, KickDoorActionState.class, indexOfInsertion, null); + if (nextPreviousFootstepPlanAction != null) + { + kickDoorAction.getDefinition().setParentFrameName(nextPreviousFootstepPlanAction.getDefinition().getParentFrameName()); + } + else + { + kickDoorAction.getDefinition().setParentFrameName(syncedRobot.getReferenceFrames().getMidFeetUnderPelvisFrame().getName()); + } + + kickDoorAction.getDefinition().setSide(sideOfNewAction); + kickDoorAction.update(); + } if (newAction instanceof FootPoseActionState footPoseAction) { // Set the new action to where the last one was for faster authoring diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java new file mode 100644 index 000000000000..82d8d2a0dc66 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -0,0 +1,224 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorActionDefinitionMessage; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.node.ObjectNode; +import us.ihmc.behaviors.sequence.ActionNodeDefinition; +import us.ihmc.communication.crdt.*; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SidedObject; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorActionDefinition extends ActionNodeDefinition implements SidedObject +{ + public static final double KICK_HEIGHT = 0.55; + public static final double KICK_IMPULSE = 55.0; + public static final double KICK_TARGET_DISTANCE = 0.75; + public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; + public static final double STANCE_FOOT_WIDTH = 0.23; + + private final CRDTUnidirectionalString parentFrameName; + + private final CRDTUnidirectionalEnumField kickSide; + private final CRDTUnidirectionalDouble kickHeight; + private final CRDTUnidirectionalDouble kickImpulse; + private final CRDTUnidirectionalDouble kickTargetDistance; + private final CRDTUnidirectionalDouble prekickWeightDistribution; + private final CRDTUnidirectionalDouble stanceFootWidth; + + // On disk fields + private String onDiskParentFrameName; + private RobotSide onDiskSide; + private double onDiskKickHeight; + private double onDiskKickImpulse; + private double onDiskKickTargetDistance; + private double onDiskPrekickWeightDistribution; + private double onDiskStanceFootWidth; + + public KickDoorActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) + { + super(crdtInfo, saveFileDirectory); + + parentFrameName = new CRDTUnidirectionalString(ROS2ActorDesignation.OPERATOR, crdtInfo, ReferenceFrame.getWorldFrame().getName()); + + kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); + kickHeight = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_HEIGHT); + kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); + kickTargetDistance = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_TARGET_DISTANCE); + prekickWeightDistribution = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, PREKICK_WEIGHT_DISTRIBUTION); + stanceFootWidth = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, STANCE_FOOT_WIDTH); + } + + @Override + public void saveToFile(ObjectNode jsonNode) + { + super.saveToFile(jsonNode); + + jsonNode.put("parentFrame", parentFrameName.getValue()); + + jsonNode.put("kickSide", kickSide.getValue().getLowerCaseName()); + jsonNode.put("kickHeight", kickHeight.getValue()); + jsonNode.put("kickImpulse", kickImpulse.getValue()); + jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); + jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); + jsonNode.put("stanceFootWidth", stanceFootWidth.getValue()); + } + + @Override + public void loadFromFile(JsonNode jsonNode) + { + super.loadFromFile(jsonNode); + + parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); + + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").textValue())); + kickHeight.setValue(jsonNode.get("kickHeight").asDouble()); + kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); + kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); + prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); + stanceFootWidth.setValue(jsonNode.get("stanceFootWidth").asDouble()); + } + + @Override + public void setOnDiskFields() + { + super.setOnDiskFields(); + + onDiskParentFrameName = parentFrameName.getValue(); + + onDiskSide = kickSide.getValue(); + onDiskKickHeight = kickHeight.getValue(); + onDiskKickImpulse = kickImpulse.getValue(); + onDiskKickTargetDistance = kickTargetDistance.getValue(); + onDiskPrekickWeightDistribution = prekickWeightDistribution.getValue(); + onDiskStanceFootWidth = stanceFootWidth.getValue(); + } + + @Override + public void undoAllNontopologicalChanges() + { + super.undoAllNontopologicalChanges(); + + parentFrameName.setValue(onDiskParentFrameName); + + kickSide.setValue(onDiskSide); + kickHeight.setValue(onDiskKickHeight); + kickImpulse.setValue(onDiskKickImpulse); + kickTargetDistance.setValue(onDiskKickTargetDistance); + prekickWeightDistribution.setValue(onDiskPrekickWeightDistribution); + stanceFootWidth.setValue(onDiskStanceFootWidth); + } + + @Override + public boolean hasChanges() + { + boolean unchanged = !super.hasChanges(); + + unchanged &= parentFrameName.getValue().equals(onDiskParentFrameName); + + unchanged &= kickSide.getValue() == onDiskSide; + unchanged &= kickHeight.getValue() == onDiskKickHeight; + unchanged &= kickImpulse.getValue() == onDiskKickImpulse; + unchanged &= kickTargetDistance.getValue() == onDiskKickTargetDistance; + unchanged &= prekickWeightDistribution.getValue() == onDiskPrekickWeightDistribution; + unchanged &= stanceFootWidth.getValue() == onDiskStanceFootWidth; + + return !unchanged; + } + + public void toMessage(KickDoorActionDefinitionMessage message) + { + super.toMessage(message.getDefinition()); + + message.setParentFrameName(parentFrameName.toMessage()); + + message.setRobotSide(kickSide.toMessage().toByte()); + message.setKickHeight(kickHeight.getValue()); + message.setKickImpulse(kickImpulse.getValue()); + message.setKickTargetDistance(kickTargetDistance.getValue()); + message.setPrekickWeightDistribution(prekickWeightDistribution.getValue()); + message.setStanceFootWidth(stanceFootWidth.getValue()); + } + + public void fromMessage(KickDoorActionDefinitionMessage message) + { + super.fromMessage(message.getDefinition()); + + parentFrameName.fromMessage(message.getParentFrameNameAsString()); + + kickSide.fromMessage(RobotSide.fromByte(message.getRobotSide())); + kickHeight.fromMessage(message.getKickHeight()); + kickImpulse.fromMessage(message.getKickImpulse()); + kickTargetDistance.fromMessage(message.getKickTargetDistance()); + prekickWeightDistribution.fromMessage(message.getPrekickWeightDistribution()); + stanceFootWidth.fromMessage(message.getStanceFootWidth()); + } + + @Override + public RobotSide getSide() + { + return kickSide.getValue(); + } + + public void setSide(RobotSide kickSide) + { + this.kickSide.setValue(kickSide); + } + + public String getParentFrameName() + { + return parentFrameName.getValue(); + } + + public void setParentFrameName(String parentFrameName) + { + this.parentFrameName.setValue(parentFrameName); + } + + public double getKickHeight() + { + return kickHeight.getValue(); + } + + public void setKickHeight(double kickHeight) + { + this.kickHeight.setValue(kickHeight); + } + + public double getKickImpulse() + { + return kickImpulse.getValue(); + } + + public void setKickImpulse(double kickImpulse) + { + this.kickImpulse.setValue(kickImpulse); + } + + public double getKickTargetDistance() + { + return kickTargetDistance.getValue(); + } + + public void setKickTargetDistance(double kickTargetDistance) + { + this.kickTargetDistance.setValue(kickTargetDistance); + } + + public double getPrekickWeightDistribution() + { + return prekickWeightDistribution.getValue(); + } + + public void setPrekickWeightDistribution(double prekickWeightDistribution) + { + this.prekickWeightDistribution.setValue(prekickWeightDistribution); + } + + public double getStanceFootWidth() + { + return stanceFootWidth.getValue(); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java new file mode 100644 index 000000000000..7073c79d479c --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java @@ -0,0 +1,24 @@ +package us.ihmc.behaviors.sequence.actions; + +public enum KickDoorActionExecutionState +{ + STANDING, + SWITCHING_TO_KICK_CONTROLLER, + KICKING, + SWITCHING_TO_WALKING_CONTROLLER, + SQUARING_UP; + + public static final KickDoorActionExecutionState[] values = values(); + + public byte toByte() + { + return (byte) ordinal(); + } + + public static KickDoorActionExecutionState fromByte(byte enumAsByte) + { + if (enumAsByte == -1) + return null; + return values[enumAsByte]; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java new file mode 100644 index 000000000000..4b7f6ae134e7 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -0,0 +1,206 @@ +package us.ihmc.behaviors.sequence.actions; + +import controller_msgs.msg.dds.FootstepDataListMessage; +import controller_msgs.msg.dds.FootstepDataMessage; +import controller_msgs.msg.dds.HighLevelStateMessage; +import controller_msgs.msg.dds.KickDoorMessage; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.behaviors.sequence.ActionNodeExecutor; +import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.commons.Conversions; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; +import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.tools.NonWallTimer; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorActionExecutor extends ActionNodeExecutor +{ + + private final KickDoorActionState state; + private final KickDoorActionDefinition definition; + private final ROS2ControllerHelper ros2ControllerHelper; + private final ROS2SyncedRobotModel syncedRobot; + private final RobotSide kickSide; + private boolean switchToKickControllerMessageSent = false; + private boolean kickingMessageSent = false; + private boolean switchToWalkControllerMessageSent = false; + private boolean squareUpFootstepsSent = false; + + private final NonWallTimer stopwatch = new NonWallTimer(); + private KickDoorMessage kickMessage = new KickDoorMessage(); + private final FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + + public KickDoorActionExecutor(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + ROS2ControllerHelper ros2ControllerHelper, + ROS2SyncedRobotModel syncedRobot, + ReferenceFrameLibrary referenceFrameLibrary) + { + super(new KickDoorActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + kickSide = definition.getSide(); + + this.ros2ControllerHelper = ros2ControllerHelper; + this.syncedRobot = syncedRobot; + + computeKickMessage(); + } + + /** + * This update is running from the moment this behavior is added to the behavior tree, regardless of whether this behavior is being executed. + */ + @Override + public void update() + { + super.update(); + + stopwatch.update(Conversions.nanosecondsToSeconds(syncedRobot.getTimestamp())); + + // add goal frame to definition or state (probs definition) + } + + /** + * Called at the beginning of execution of this behvior. + */ + @Override + public void triggerActionExecution() + { + super.triggerActionExecution(); + + state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_TO_KICK_CONTROLLER); + } + + /** + * This is only called while this behavior is being executed. + */ + @Override + public void updateCurrentlyExecuting() + { + switch (state.getExecutionState().getValue()) + { + case STANDING -> + { + //Idle until the kick is requested + } + case SWITCHING_TO_KICK_CONTROLLER -> + { + if (!switchToKickControllerMessageSent) + { + // The kicking controller must be added with a boolean in the controller factory. + state.getLogger().info("Switching to kick controller message."); + changeHighLevelState(HighLevelControllerName.CUSTOM1); + switchToKickControllerMessageSent = true; + stopwatch.reset(); + } + + if (stopwatch.getElapsedTime() >= 0.2) + { + stopwatch.reset(); + state.getExecutionState().setValue(KickDoorActionExecutionState.KICKING); + } + } + case KICKING -> + { + //Execute the kick + state.setIsExecuting(true); + + if (!kickingMessageSent) + { + stopwatch.reset(); + kickingMessageSent = true; + state.getLogger().info("Executing kick."); + computeKickMessage(); + ros2ControllerHelper.publishToController(kickMessage); + } + + if (stopwatch.getElapsedTime() >= 6.0) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_TO_WALKING_CONTROLLER); + stopwatch.reset(); + } + } + case SWITCHING_TO_WALKING_CONTROLLER -> + { + if (!switchToWalkControllerMessageSent) + { + stopwatch.reset(); + state.getLogger().info("Sending switch to walking controller message."); + changeHighLevelState(HighLevelControllerName.WALKING); + switchToWalkControllerMessageSent = true; + } + + if (stopwatch.getElapsedTime() >= 0.2) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.SQUARING_UP); + stopwatch.reset(); + } + } + case SQUARING_UP -> + { + if (!squareUpFootstepsSent) + { + stopwatch.reset(); + state.getLogger().info("Sending square up footsteps."); + computeSquaredUpFootsteps(); + ros2ControllerHelper.publishToController(footstepDataListMessage); + squareUpFootstepsSent = true; + } + + if (stopwatch.getElapsedTime() >= 4.0) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.STANDING); + state.setIsExecuting(false); + stopwatch.reset(); + state.getLogger().info("Going back to standing state."); + state.setIsExecuting(false); + } + } + } + } + + private void changeHighLevelState(HighLevelControllerName highLevelControllerName) + { + // Switch the high level state machine to the desired controller + HighLevelStateMessage highLevelStateMessage = new HighLevelStateMessage(); + highLevelStateMessage.setHighLevelControllerName(highLevelControllerName.toByte()); + ros2ControllerHelper.publishToController(highLevelStateMessage); + } + + public void computeKickMessage() + { + kickMessage.setRobotSide(kickSide.toByte()); + kickMessage.setKickHeight(definition.getKickHeight()); + kickMessage.setKickImpulse(definition.getKickImpulse()); + kickMessage.setKickTargetDistance(definition.getKickTargetDistance()); + kickMessage.setPrekickWeightDistribution(definition.getPrekickWeightDistribution()); + } + + public void computeSquaredUpFootsteps() + { + MovingReferenceFrame pelvisFrame = syncedRobot.getReferenceFrames().getMidFeetUnderPelvisFrame(); + FramePose3D kickFootGoalPose = new FramePose3D(pelvisFrame); + kickFootGoalPose.setY(kickSide.negateIfRightSide(definition.getStanceFootWidth() / 2.0)); + kickFootGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); + kickFootGoalPose.setZ(0.0); + FootstepDataMessage kickFootStep = HumanoidMessageTools.createFootstepDataMessage(kickSide, kickFootGoalPose); + footstepDataListMessage.getFootstepDataList().add().set(kickFootStep); + + FramePose3D supportFootGoalPose = new FramePose3D(pelvisFrame); + supportFootGoalPose.setY(kickSide.negateIfRightSide(-definition.getStanceFootWidth() / 2.0)); + supportFootGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); + supportFootGoalPose.setZ(0.0); + FootstepDataMessage supportFootStep = HumanoidMessageTools.createFootstepDataMessage(kickSide.getOppositeSide(), supportFootGoalPose); + footstepDataListMessage.getFootstepDataList().add().set(supportFootStep); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java new file mode 100644 index 000000000000..80c96d7bf08a --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java @@ -0,0 +1,69 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorActionStateMessage; +import us.ihmc.behaviors.sequence.ActionNodeState; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.crdt.CRDTUnidirectionalEnumField; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorActionState extends ActionNodeState +{ + private final ReferenceFrame parentFrame; + private final KickDoorActionDefinition definition; + private final ReferenceFrameLibrary referenceFrameLibrary; + private CRDTUnidirectionalEnumField executionState; + + public KickDoorActionState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, ReferenceFrameLibrary referenceFrameLibrary) + { + super(id, new KickDoorActionDefinition(crdtInfo, saveFileDirectory), crdtInfo); + + definition = getDefinition(); + + this.referenceFrameLibrary = referenceFrameLibrary; + + parentFrame = referenceFrameLibrary.findFrameByName(definition.getParentFrameName()); + + executionState = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.ROBOT, crdtInfo, KickDoorActionExecutionState.STANDING); + } + + @Override + public void update() + { + } + + public void toMessage(KickDoorActionStateMessage message) + { + getDefinition().toMessage(message.getDefinition()); + + super.toMessage(message.getState()); + + message.setExecutionState(executionState.toMessage().toByte()); + } + + public void fromMessage(KickDoorActionStateMessage message) + { + super.fromMessage(message.getState()); + + getDefinition().fromMessage(message.getDefinition()); + + executionState.fromMessage(KickDoorActionExecutionState.fromByte(message.getExecutionState())); + } + + public boolean areFramesInWorld() + { + return referenceFrameLibrary.containsFrame(definition.getParentFrameName()) && parentFrame.getRootFrame() == ReferenceFrame.getWorldFrame(); + } + + public ReferenceFrame getParentFrame() + { + return parentFrame; + } + + public CRDTUnidirectionalEnumField getExecutionState() + { + return executionState; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java new file mode 100644 index 000000000000..22bbde0bd9b0 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -0,0 +1,287 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.node.ObjectNode; +import us.ihmc.behaviors.sequence.ActionNodeDefinition; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.crdt.CRDTUnidirectionalDouble; +import us.ihmc.communication.crdt.CRDTUnidirectionalEnumField; +import us.ihmc.communication.crdt.CRDTUnidirectionalString; +import us.ihmc.communication.packets.ExecutionMode; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SidedObject; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition implements SidedObject +{ + public static final double KICK_IMPULSE = 55.0; + public static final double KICK_HEIGHT = 0.55; + public static final double KICK_TARGET_DISTANCE = 0.75; + public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; + public static final double HORIZONTAL_DISTANCE_FROM_HANDLE = 0.1; + public static final double STANCE_FOOT_WIDTH = 0.23; + + private final CRDTUnidirectionalDouble swingDuration; + private final CRDTUnidirectionalDouble transferDuration; + private final CRDTUnidirectionalEnumField executionMode; + private final CRDTUnidirectionalString parentFrameName; + + private final CRDTUnidirectionalEnumField kickSide; + private final CRDTUnidirectionalDouble kickImpulse; + private final CRDTUnidirectionalDouble kickHeight; + private final CRDTUnidirectionalDouble kickTargetDistance; + private final CRDTUnidirectionalDouble prekickWeightDistribution; + private final CRDTUnidirectionalDouble horizontalDistanceFromHandle; + private final CRDTUnidirectionalDouble stanceFootWidth; + + // On disk fields + private double onDiskSwingDuration; + private double onDiskTransferDuration; + private ExecutionMode onDiskExecutionMode; + private String onDiskParentFrameName; + private RobotSide onDiskKickSide; + private double onDiskKickImpulse; + private double onDiskKickHeight; + private double onDiskKickTargetDistance; + private double onDiskPrekickWeightDistribution; + private double onDiskHorizontalDistanceFromHandle; + private double onDiskStanceFootWidth; + + + public KickDoorApproachPlanActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) + { + super(crdtInfo, saveFileDirectory); + + swingDuration = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 1.2); + transferDuration = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.8); + executionMode = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, ExecutionMode.OVERRIDE); + parentFrameName = new CRDTUnidirectionalString(ROS2ActorDesignation.OPERATOR, crdtInfo, ReferenceFrame.getWorldFrame().getName()); + + kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); + kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); + kickHeight = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_HEIGHT); + kickTargetDistance = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_TARGET_DISTANCE); + prekickWeightDistribution = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, PREKICK_WEIGHT_DISTRIBUTION); + horizontalDistanceFromHandle = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, HORIZONTAL_DISTANCE_FROM_HANDLE); + stanceFootWidth = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, STANCE_FOOT_WIDTH); + } + + @Override + public void saveToFile(ObjectNode jsonNode) + { + super.saveToFile(jsonNode); + + jsonNode.put("swingDuration", swingDuration.getValue()); + jsonNode.put("transferDuration", transferDuration.getValue()); + jsonNode.put("executionMode", executionMode.getValue().name()); + jsonNode.put("parentFrame", parentFrameName.getValue()); + + jsonNode.put("kickImpulse", kickImpulse.getValue()); + jsonNode.put("kickHeight", kickHeight.getValue()); + jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); + jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); + jsonNode.put("horizontalDistanceFromHandle", horizontalDistanceFromHandle.getValue()); + jsonNode.put("stanceFootWidth", stanceFootWidth.getValue()); + jsonNode.put("kickSide", kickSide.getValue().getLowerCaseName()); + } + + @Override + public void loadFromFile(JsonNode jsonNode) + { + super.loadFromFile(jsonNode); + + swingDuration.setValue(jsonNode.get("swingDuration").asDouble()); + transferDuration.setValue(jsonNode.get("transferDuration").asDouble()); + executionMode.setValue(ExecutionMode.valueOf(jsonNode.get("executionMode").textValue())); + parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); + + kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); + kickHeight.setValue(jsonNode.get("kickHeight").asDouble()); + kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); + prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); + horizontalDistanceFromHandle.setValue(jsonNode.get("horizontalDistanceFromHandle").asDouble()); + stanceFootWidth.setValue(jsonNode.get("stanceFootWidth").asDouble()); + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").textValue())); + } + + @Override + public void setOnDiskFields() + { + super.setOnDiskFields(); + + onDiskSwingDuration = swingDuration.getValue(); + onDiskTransferDuration = transferDuration.getValue(); + onDiskExecutionMode = executionMode.getValue(); + onDiskParentFrameName = parentFrameName.getValue(); + onDiskKickSide = kickSide.getValue(); + onDiskKickImpulse = kickImpulse.getValue(); + onDiskKickHeight = kickHeight.getValue(); + onDiskKickTargetDistance = kickTargetDistance.getValue(); + onDiskPrekickWeightDistribution = prekickWeightDistribution.getValue(); + onDiskHorizontalDistanceFromHandle = horizontalDistanceFromHandle.getValue(); + onDiskStanceFootWidth = stanceFootWidth.getValue(); + } + + @Override + public void undoAllNontopologicalChanges() + { + super.undoAllNontopologicalChanges(); + + swingDuration.setValue(onDiskSwingDuration); + transferDuration.setValue(onDiskTransferDuration); + executionMode.setValue(onDiskExecutionMode); + parentFrameName.setValue(onDiskParentFrameName); + kickSide.setValue(onDiskKickSide); + kickImpulse.setValue(onDiskKickImpulse); + kickHeight.setValue(onDiskKickHeight); + kickTargetDistance.setValue(onDiskKickTargetDistance); + prekickWeightDistribution.setValue(onDiskPrekickWeightDistribution); + horizontalDistanceFromHandle.setValue(onDiskHorizontalDistanceFromHandle); + stanceFootWidth.setValue(onDiskStanceFootWidth); + } + + @Override + public boolean hasChanges() + { + boolean unchanged = !super.hasChanges(); + + unchanged &= swingDuration.getValue() == onDiskSwingDuration; + unchanged &= transferDuration.getValue() == onDiskTransferDuration; + unchanged &= executionMode.getValue() == onDiskExecutionMode; + unchanged &= parentFrameName.getValue().equals(onDiskParentFrameName); + unchanged &= kickSide.getValue() == onDiskKickSide; + unchanged &= kickImpulse.getValue() == onDiskKickImpulse; + unchanged &= kickHeight.getValue() == onDiskKickHeight; + unchanged &= kickTargetDistance.getValue() == onDiskKickTargetDistance; + unchanged &= prekickWeightDistribution.getValue() == onDiskPrekickWeightDistribution; + unchanged &= horizontalDistanceFromHandle.getValue() == onDiskHorizontalDistanceFromHandle; + unchanged &= stanceFootWidth.getValue() == onDiskStanceFootWidth; + + return !unchanged; + } + + public void toMessage(KickDoorApproachPlanDefinitionMessage message) + { + super.toMessage(message.getDefinition()); + + message.setSwingDuration(swingDuration.toMessage()); + message.setTransferDuration(transferDuration.toMessage()); + message.setExecutionMode(executionMode.toMessageOrdinal()); + message.setParentFrameName(parentFrameName.toMessage()); + + message.setRobotSide(kickSide.toMessage().toByte()); + message.setKickImpulse(kickImpulse.toMessage()); + message.setKickHeight(kickHeight.toMessage()); + message.setKickTargetDistance(kickTargetDistance.toMessage()); + message.setPrekickWeightDistribution(prekickWeightDistribution.toMessage()); + message.setHorizontalDistanceFromHandle(horizontalDistanceFromHandle.toMessage()); + message.setStanceFootWidth(stanceFootWidth.toMessage()); + } + + public void fromMessage(KickDoorApproachPlanDefinitionMessage message) + { + super.fromMessage(message.getDefinition()); + + swingDuration.fromMessage(message.getSwingDuration()); + transferDuration.fromMessage(message.getTransferDuration()); + executionMode.fromMessageOrdinal(message.getExecutionMode(), ExecutionMode.values); + parentFrameName.fromMessage(message.getParentFrameNameAsString()); + + kickSide.fromMessage(RobotSide.fromByte(message.getRobotSide())); + kickHeight.fromMessage(message.getKickHeight()); + kickImpulse.fromMessage(message.getKickImpulse()); + kickTargetDistance.fromMessage(message.getKickTargetDistance()); + prekickWeightDistribution.fromMessage(message.getPrekickWeightDistribution()); + horizontalDistanceFromHandle.fromMessage(message.getHorizontalDistanceFromHandle()); + stanceFootWidth.fromMessage(message.getStanceFootWidth()); + } + + @Override + public RobotSide getSide() + { + return kickSide.getValue(); + } + + public void setSide(RobotSide kickSide) + { + this.kickSide.setValue(kickSide); + } + + public double getSwingDuration() + { + return swingDuration.getValue(); + } + + public void setSwingDuration(double swingDuration) + { + this.swingDuration.setValue(swingDuration); + } + + public double getTransferDuration() + { + return transferDuration.getValue(); + } + + public void setTransferDuration(double transferDuration) + { + this.transferDuration.setValue(transferDuration); + } + + public CRDTUnidirectionalEnumField getExecutionMode() + { + return executionMode; + } + + public String getParentFrameName() + { + return parentFrameName.getValue(); + } + + public void setParentFrameName(String parentFrameName) + { + this.parentFrameName.setValue(parentFrameName); + } + + public CRDTUnidirectionalString getCRDTParentFrameName() + { + return parentFrameName; + } + + public CRDTUnidirectionalEnumField getKickSide() + { + return kickSide; + } + + public CRDTUnidirectionalDouble getKickImpulse() + { + return kickImpulse; + } + + public CRDTUnidirectionalDouble getKickTargetDistance() + { + return kickTargetDistance; + } + + public CRDTUnidirectionalDouble getPrekickWeightDistribution() + { + return prekickWeightDistribution; + } + + public CRDTUnidirectionalDouble getHorizontalDistanceFromHandle() + { + return horizontalDistanceFromHandle; + } + + public CRDTUnidirectionalDouble getStanceFootWidth() + { + return stanceFootWidth; + } + + public CRDTUnidirectionalDouble getKickHeight() + { + return kickHeight; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java new file mode 100644 index 000000000000..3d210dd065a3 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -0,0 +1,493 @@ +package us.ihmc.behaviors.sequence.actions; + +import controller_msgs.msg.dds.FootstepDataListMessage; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.behaviors.sequence.ActionNodeExecutor; +import us.ihmc.behaviors.sequence.TaskspaceTrajectoryTrackingErrorCalculator; +import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickDynamicPlanner; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickInputParameters; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickParameters; +import us.ihmc.commons.Conversions; +import us.ihmc.commons.FormattingTools; +import us.ihmc.commons.exception.DefaultExceptionHandler; +import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.footstepPlanning.*; +import us.ihmc.footstepPlanning.graphSearch.graph.visualization.BipedalFootstepPlannerNodeRejectionReason; +import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; +import us.ihmc.footstepPlanning.log.FootstepPlannerLogger; +import us.ihmc.footstepPlanning.tools.FootstepPlannerRejectionReasonReport; +import us.ihmc.footstepPlanning.tools.PlannerTools; +import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; +import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; +import us.ihmc.robotics.referenceFrames.PoseReferenceFrame; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.WorkspaceResourceDirectory; +import us.ihmc.tools.thread.MissingThreadTools; +import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; + +import java.util.UUID; + +public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor +{ + public static final double POSITION_TOLERANCE = 0.15; + public static final double ORIENTATION_TOLERANCE = Math.toRadians(10.0); + + private final KickDoorApproachPlanActionState state; + private final KickDoorApproachPlanActionDefinition definition; + private final ROS2ControllerHelper ros2ControllerHelper; + private final ROS2SyncedRobotModel syncedRobot; + private final ControllerStatusTracker controllerStatusTracker; + private final WalkingControllerParameters walkingControllerParameters; + private final SideDependentList commandedGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final SideDependentList syncedFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final SideDependentList indexOfLastFoot = new SideDependentList<>(); + private final ReferenceFrameLibrary referenceFrameLibrary; + private double nominalExecutionDuration; + private final SideDependentList trackingCalculators = new SideDependentList<>( + TaskspaceTrajectoryTrackingErrorCalculator::new); + private final FootstepPlan footstepPlanToExecute = new FootstepPlan(); + private final FootstepPlanningModule footstepPlanner; + private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; + private final ResettableExceptionHandlingExecutorService footstepPlanningThread = MissingThreadTools.newSingleThreadExecutor("FootstepPlanning", true, 1); + private final TypedNotification footstepPlanNotification = new TypedNotification<>(); + private final SideDependentList liveGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final FramePose3D liveKickPose = new FramePose3D(); + private final SideDependentList startFootPosesForThread = new SideDependentList<>(new FramePose3D(), new FramePose3D()); + private final SideDependentList goalFootPosesForThread = new SideDependentList<>(new FramePose3D(), new FramePose3D()); + + private final KickParameters kickParameters = new KickParameters(); + private final KickInputParameters kickInputParameters = new KickInputParameters(null); + private final SideDependentList soleFramesForPlanning = new SideDependentList<>(); + private final PoseReferenceFrame centerOfMassControlFrameForPlanning = new PoseReferenceFrame("CenterOfMassControlFrameForPlanning", + ReferenceFrame.getWorldFrame()); + private static final double gravityZ = 9.81; // This needs to be positive for the planner to work + private final KickDynamicPlanner kickDynamicPlanner; + + private final DetachableReferenceFrame doorHandleFrame; + private ReferenceFrame worldFrame; + + public KickDoorApproachPlanActionExecutor(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + ROS2ControllerHelper ros2ControllerHelper, + ROS2SyncedRobotModel syncedRobot, + ControllerStatusTracker controllerStatusTracker, + ReferenceFrameLibrary referenceFrameLibrary, + WalkingControllerParameters walkingControllerParameters, + FootstepPlanningModule footstepPlanner, + DefaultFootstepPlannerParametersBasics footstepPlannerParameters) + { + super(new KickDoorApproachPlanActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + this.referenceFrameLibrary = referenceFrameLibrary; + this.ros2ControllerHelper = ros2ControllerHelper; + this.syncedRobot = syncedRobot; + this.controllerStatusTracker = controllerStatusTracker; + this.walkingControllerParameters = walkingControllerParameters; + this.footstepPlanner = footstepPlanner; + this.footstepPlannerParameters = footstepPlannerParameters; + + doorHandleFrame = state.getDoorHandleFrame(); + doorHandleFrame.update(definition.getParentFrameName()); + worldFrame = doorHandleFrame.getReferenceFrame().getRootFrame(); + + for (RobotSide robotSide : RobotSide.values) + { + PoseReferenceFrame soleFrameForPlanning = new PoseReferenceFrame(robotSide.getLowerCaseName() + "SoleFrameForPlanning", + ReferenceFrame.getWorldFrame()); + soleFramesForPlanning.put(robotSide, soleFrameForPlanning); + } + + this.kickDynamicPlanner = new KickDynamicPlanner(kickParameters, + soleFramesForPlanning, + centerOfMassControlFrameForPlanning, + gravityZ, + syncedRobot.getFullRobotModel().getTotalMass(), + null); + } + + @Override + public void update() + { + super.update(); + + worldFrame = doorHandleFrame.getReferenceFrame().getRootFrame(); + + boolean invalidDefinition = false; + + double kickImpulse = definition.getKickImpulse().getValue(); + double kickHeight = definition.getKickHeight().getValue(); + double kickTargetDistance = definition.getKickTargetDistance().getValue(); + double prekickWeightDistribution = definition.getPrekickWeightDistribution().getValue(); + double kickStanceWidth = definition.getStanceFootWidth().getValue(); + double kickOffsetFromHandle = definition.getHorizontalDistanceFromHandle().getValue(); + + if (Double.isNaN(kickImpulse) || kickImpulse < 0.0) + { + state.getLogger().error("Kick impulse must be greater than 0.0"); + invalidDefinition = true; + } + + if (Double.isNaN(kickTargetDistance) || kickTargetDistance < 0.0) + { + state.getLogger().error("Kick target distance must be greater than 0.0"); + invalidDefinition = true; + } + + if (Double.isNaN(prekickWeightDistribution) || prekickWeightDistribution < 0.4) + { + state.getLogger().error("Pre kick weight distribution must be greater than 0.4"); + invalidDefinition = true; + } + + if (Double.isNaN(kickStanceWidth) || kickStanceWidth < 0.15) + { + state.getLogger().error("Kick stance width must be greater than 0.15"); + invalidDefinition = true; + } + + if (Double.isNaN(kickOffsetFromHandle) || kickOffsetFromHandle < 0.0) + { + state.getLogger().error("Kick offset from handle must be greater than 0.0"); + invalidDefinition = true; + } + + state.setCanExecute(state.areFramesInWorld() && !invalidDefinition); + computeGoalFootPosesForKick(); + + state.getLeftFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.LEFT)); + state.getRightFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.RIGHT)); + state.getKickGoalPose().getValue().set(liveKickPose); + + for (RobotSide side : RobotSide.values) + { + trackingCalculators.get(side).update(Conversions.nanosecondsToSeconds(syncedRobot.getTimestamp())); + syncedFeetPoses.get(side).setFromReferenceFrame(syncedRobot.getReferenceFrames().getSoleFrame(side)); + } + + if (state.getCanExecute()) + { + } + } + + private void computeGoalFootPosesForKick() + { + // update the input parameters from the definition + kickInputParameters.setKickImpulse(definition.getKickImpulse().getValue()); + kickInputParameters.setKickTargetDistance(definition.getKickTargetDistance().getValue()); + kickInputParameters.setPrekickWeightDistribution(definition.getPrekickWeightDistribution().getValue()); + kickInputParameters.setKickFootSide(definition.getKickSide().getValue()); + + computeSquaredUpGoalFootPosesFromDoorHandle(); + + // call compute here to get the initial kick foot pose + kickDynamicPlanner.compute(kickInputParameters); + + updateGoalPosesFromDynamicPlanner(); + } + + private void computeSquaredUpGoalFootPosesFromDoorHandle() + { + RobotSide kickSide = definition.getKickSide().getValue(); + + FramePose3D preKickFootPose = new FramePose3D(); + preKickFootPose.setToZero(doorHandleFrame.getReferenceFrame()); + preKickFootPose.setX(definition.getKickTargetDistance().getValue()); + preKickFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); + preKickFootPose.changeFrame(worldFrame); + preKickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + + + FramePose3D kickPose = new FramePose3D(preKickFootPose); + kickPose.changeFrame(syncedRobot.getReferenceFrames().getMidFeetZUpFrame()); + kickPose.setZ(definition.getKickHeight().getValue()); + kickPose.changeFrame(doorHandleFrame.getReferenceFrame()); + kickPose.setX(0.0); + kickPose.appendPitchRotation(Math.PI / 2.0); + kickPose.changeFrame(worldFrame); + + FramePose3D stanceFootPose = new FramePose3D(); + stanceFootPose.setToZero(doorHandleFrame.getReferenceFrame()); + stanceFootPose.setX(definition.getKickTargetDistance().getValue()); + stanceFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); + stanceFootPose.changeFrame(worldFrame); + stanceFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + + FramePose3D centerOfMassPose = new FramePose3D(worldFrame); + centerOfMassPose.interpolate(preKickFootPose, stanceFootPose, 0.5); + + soleFramesForPlanning.get(kickSide).setPoseAndUpdate(preKickFootPose); + soleFramesForPlanning.get(kickSide.getOppositeSide()).setPoseAndUpdate(stanceFootPose); + + liveKickPose.set(kickPose); + + centerOfMassControlFrameForPlanning.setPoseAndUpdate(centerOfMassPose); + } + + private void updateGoalPosesFromDynamicPlanner() + { + RobotSide kickSide = definition.getKickSide().getValue(); + RobotSide stanceSide = kickSide.getOppositeSide(); + + FramePose3D stanceGoalPose = new FramePose3D(soleFramesForPlanning.get(stanceSide)); + stanceGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); + + liveGoalFeetPoses.get(stanceSide).setIncludingFrame(stanceGoalPose); + + FramePose3D kickStartPose = new FramePose3D(soleFramesForPlanning.get(kickSide)); + kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); + kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); + + liveGoalFeetPoses.get(kickSide).setIncludingFrame(kickStartPose); + } + + @Override + public void triggerActionExecution() + { + super.triggerActionExecution(); + + // Reset state + state.setTotalNumberOfFootsteps(0); + state.setNumberOfIncompleteFootsteps(0); + for (RobotSide side : RobotSide.values) + { + state.getCurrentFootPoses().get(side).getValue().set(syncedFeetPoses.get(side)); + state.getDesiredFootPoses().get(side).getValue().clear(); + } + + state.setPositionDistanceToGoalTolerance(POSITION_TOLERANCE); + state.setOrientationDistanceToGoalTolerance(ORIENTATION_TOLERANCE); + + if (state.areFramesInWorld()) + { + startFootstepPlanningAsync(); + state.getExecutionState().setValue(FootstepPlanActionExecutionState.FOOTSTEP_PLANNING); + } + else + { + state.getLogger().error("Cannot execute. Frame is not a child of World frame."); + } + } + + @Override + public void updateCurrentlyExecuting() + { + switch (state.getExecutionState().getValue()) + { + case FOOTSTEP_PLANNING -> + { + state.setIsExecuting(true); + // TODO: Maybe report planning elapsed time or something + if (footstepPlanNotification.poll()) + { + footstepPlanToExecute.clear(); + footstepPlanToExecute.set(footstepPlanNotification.read()); + if (footstepPlanToExecute.isEmpty()) + { + state.getExecutionState().setValue(FootstepPlanActionExecutionState.PLANNING_FAILED); + } + else + { + state.getExecutionState().setValue(FootstepPlanActionExecutionState.PLANNING_SUCCEEDED); + } + } + } + case PLANNING_FAILED -> + { + state.getLogger().error("No planned steps to execute!"); + state.setIsExecuting(false); + state.setFailed(true); + } + case PLANNING_SUCCEEDED -> + { + state.setIsExecuting(true); + buildAndSendCommandAndSetDesiredState(); + state.getExecutionState().setValue(FootstepPlanActionExecutionState.PLAN_COMMANDED); + } + case PLAN_COMMANDED -> + { + updateProgress(); + } + } + } + + private void startFootstepPlanningAsync() + { + for (RobotSide side : RobotSide.values) + { + startFootPosesForThread.get(side).setFromReferenceFrame(syncedRobot.getReferenceFrames().getSoleFrame(side)); + goalFootPosesForThread.get(side).set(liveGoalFeetPoses.get(side)); + } + + footstepPlanNotification.poll(); // Make sure it's cleared + footstepPlanningThread.execute(() -> + { + footstepPlannerParameters.setFinalTurnProximity(1.0); + + FootstepPlannerRequest footstepPlannerRequest = new FootstepPlannerRequest(); + footstepPlannerRequest.setPlanBodyPath(false); + footstepPlannerRequest.setStartFootPoses(startFootPosesForThread.get(RobotSide.LEFT), + startFootPosesForThread.get(RobotSide.RIGHT)); + // TODO: Set start footholds!! + for (RobotSide side : RobotSide.values) + { + footstepPlannerRequest.setGoalFootPose(side, goalFootPosesForThread.get(side)); + } + + footstepPlannerRequest.setAssumeFlatGround(true); // TODO: Incorporate height map + + footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); + double idealFootstepLength = 0.5; + footstepPlanner.getFootstepPlannerParameters().setIdealFootstepLength(idealFootstepLength); + footstepPlanner.getFootstepPlannerParameters().setMaxStepReach(idealFootstepLength); + state.getLogger().info("Planning footsteps..."); + FootstepPlannerOutput footstepPlannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); + FootstepPlan footstepPlan = footstepPlannerOutput.getFootstepPlan(); + state.getLogger() + .info("Footstep planner completed with {}, {} step(s)", + footstepPlannerOutput.getFootstepPlanningResult(), + footstepPlan.getNumberOfSteps()); + + if (footstepPlan.getNumberOfSteps() < 1) // failed + { + FootstepPlannerRejectionReasonReport rejectionReasonReport = new FootstepPlannerRejectionReasonReport(footstepPlanner); + rejectionReasonReport.update(); + for (BipedalFootstepPlannerNodeRejectionReason reason : rejectionReasonReport.getSortedReasons()) + { + double rejectionPercentage = rejectionReasonReport.getRejectionReasonPercentage(reason); + state.getLogger() + .info("Rejection {}%: {}", FormattingTools.getFormattedToSignificantFigures(rejectionPercentage, 3), reason); + } + state.getLogger().info("Footstep planning failure..."); + footstepPlanNotification.set(new FootstepPlan()); + } + else + { + for (int i = 0; i < footstepPlan.getNumberOfSteps(); i++) + { + if (i == 0) + footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration() / 2.0); + else + footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration()); + + footstepPlan.getFootstep(i).setSwingDuration(getDefinition().getSwingDuration()); + } + footstepPlanNotification.set(new FootstepPlan(footstepPlan)); // Copy of the output to be safe + } + + FootstepPlannerLogger footstepPlannerLogger = new FootstepPlannerLogger(footstepPlanner); + footstepPlannerLogger.logSession(); + FootstepPlannerLogger.deleteOldLogs(); + }, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE); + } + + private void buildAndSendCommandAndSetDesiredState() + { + FootstepDataListMessage footstepDataListMessage = FootstepDataMessageConverter.createFootstepDataListFromPlan(footstepPlanToExecute, + definition.getSwingDuration(), + definition.getTransferDuration()); + double finalTransferDuration = 0.01; // We don't want any unecessary pauses at the end; but it can't be 0 + footstepDataListMessage.setFinalTransferDuration(finalTransferDuration); + footstepDataListMessage.getQueueingProperties().setExecutionMode(definition.getExecutionMode().getValue().toByte()); + footstepDataListMessage.getQueueingProperties().setMessageId(UUID.randomUUID().getLeastSignificantBits()); + state.getLogger().info("Commanding {} footsteps", footstepDataListMessage.getFootstepDataList().size()); + ros2ControllerHelper.publishToController(footstepDataListMessage); + for (RobotSide side : RobotSide.values) + { + trackingCalculators.get(side).reset(); + } + + nominalExecutionDuration = PlannerTools.calculateNominalTotalPlanExecutionDuration(footstepPlanToExecute, + definition.getSwingDuration(), + walkingControllerParameters.getDefaultInitialTransferTime(), + definition.getTransferDuration(), + finalTransferDuration); + for (RobotSide side : RobotSide.values) + { + indexOfLastFoot.put(side, -1); + } + for (int i = 0; i < footstepPlanToExecute.getNumberOfSteps(); i++) + { + indexOfLastFoot.put(footstepPlanToExecute.getFootstep(i).getRobotSide(), i); + } + + for (RobotSide side : RobotSide.values) + { + int indexOfLastFootSide = indexOfLastFoot.get(side); + if (indexOfLastFootSide >= 0) + { + commandedGoalFeetPoses.get(side).setIncludingFrame(footstepPlanToExecute.getFootstep(indexOfLastFootSide).getFootstepPose()); + } + else + { + commandedGoalFeetPoses.get(side).setIncludingFrame(syncedFeetPoses.get(side)); + } + + state.getDesiredFootPoses().get(side).getValue().clear(); + state.getDesiredFootPoses().get(side).addTrajectoryPoint(syncedFeetPoses.get(side), 0.0); + } + + for (int i = 0; i < footstepPlanToExecute.getNumberOfSteps(); i++) + { + PlannedFootstep footstep = footstepPlanToExecute.getFootstep(i); + double stepCompletionTime = PlannerTools.calculateFootstepCompletionTime(footstepPlanToExecute, + definition.getSwingDuration(), + walkingControllerParameters.getDefaultInitialTransferTime(), + definition.getTransferDuration(), + walkingControllerParameters.getDefaultFinalTransferTime(), + i + 1); + state.getDesiredFootPoses().get(footstep.getRobotSide()).addTrajectoryPoint(footstep.getFootstepPose(), stepCompletionTime); + } + } + + private void updateProgress() + { + boolean hitTimeLimit = false; + boolean meetsDesiredCompletionCriteria = true; + + for (RobotSide side : RobotSide.values) + { + trackingCalculators.get(side).computeExecutionTimings(nominalExecutionDuration); + trackingCalculators.get(side).computePoseTrackingData(commandedGoalFeetPoses.get(side), syncedFeetPoses.get(side)); + trackingCalculators.get(side).factorInR3Errors(POSITION_TOLERANCE); + trackingCalculators.get(side).factoryInSO3Errors(ORIENTATION_TOLERANCE); + meetsDesiredCompletionCriteria &= trackingCalculators.get(side).isWithinPositionTolerance(); + meetsDesiredCompletionCriteria &= trackingCalculators.get(side).getTimeIsUp(); + hitTimeLimit |= trackingCalculators.get(side).getHitTimeLimit(); + } + + int incompleteFootsteps = controllerStatusTracker.getFootstepTracker().getNumberOfIncompleteFootsteps(); + boolean isWalking = controllerStatusTracker.isWalking(); + meetsDesiredCompletionCriteria &= incompleteFootsteps == 0; + meetsDesiredCompletionCriteria &= !isWalking; + + if (meetsDesiredCompletionCriteria || hitTimeLimit) + { + state.setIsExecuting(false); + } + if (hitTimeLimit) + { + state.setFailed(true); + state.getLogger().info("Walking failed. (time limit)"); + } + state.setNominalExecutionDuration(nominalExecutionDuration); + state.setElapsedExecutionTime(trackingCalculators.get(RobotSide.LEFT).getElapsedTime()); + state.setTotalNumberOfFootsteps(footstepPlanToExecute.getNumberOfSteps()); + state.setNumberOfIncompleteFootsteps(incompleteFootsteps); + for (RobotSide side : RobotSide.values) + { + state.getCurrentFootPoses().get(side).getValue().set(syncedFeetPoses.get(side)); + } + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java new file mode 100644 index 000000000000..df3176848e7a --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java @@ -0,0 +1,165 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage; +import us.ihmc.behaviors.sequence.ActionNodeState; +import us.ihmc.communication.crdt.*; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.referenceFrames.ZUpFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorApproachPlanActionState extends ActionNodeState +{ + private final KickDoorApproachPlanActionDefinition definition; + private final ReferenceFrameLibrary referenceFrameLibrary; + private final CRDTUnidirectionalPose3D kickGoalPose; + private final CRDTUnidirectionalPose3D leftFootGoalPose; + private final CRDTUnidirectionalPose3D rightFootGoalPose; + private final DetachableReferenceFrame doorHandleFrame; + private ReferenceFrame parentFrame; + private final CRDTUnidirectionalInteger totalNumberOfFootsteps; + private final CRDTUnidirectionalInteger numberOfIncompleteFootsteps; + private final SideDependentList desiredFootPoses = new SideDependentList<>(); + private final SideDependentList currentFootPoses = new SideDependentList<>(); + private final CRDTUnidirectionalEnumField executionState; + + public KickDoorApproachPlanActionState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, ReferenceFrameLibrary referenceFrameLibrary) + { + super(id, new KickDoorApproachPlanActionDefinition(crdtInfo, saveFileDirectory), crdtInfo); + + definition = getDefinition(); + + this.referenceFrameLibrary = referenceFrameLibrary; + + ZUpFrame stateParentZUpFrame = new ZUpFrame(referenceFrameLibrary.findFrameByName(definition.getParentFrameName()), "StateParentZUpFrame"); + RigidBodyTransform transformToParent = stateParentZUpFrame.getTransformToParent(); + doorHandleFrame = new DetachableReferenceFrame(referenceFrameLibrary, transformToParent); + + kickGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); + leftFootGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); + rightFootGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); + totalNumberOfFootsteps = new CRDTUnidirectionalInteger(ROS2ActorDesignation.ROBOT, crdtInfo, 0); + numberOfIncompleteFootsteps = new CRDTUnidirectionalInteger(ROS2ActorDesignation.ROBOT, crdtInfo, 0); + for (RobotSide side : RobotSide.values) + { + desiredFootPoses.set(side, new CRDTUnidirectionalSE3Trajectory(ROS2ActorDesignation.ROBOT, crdtInfo)); + currentFootPoses.set(side, new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo)); + } + executionState = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.ROBOT, crdtInfo, FootstepPlanActionExecutionState.PLANNING_SUCCEEDED); + } + + @Override + public void update() + { + doorHandleFrame.update(definition.getParentFrameName()); + parentFrame = doorHandleFrame.getReferenceFrame().getParent(); + } + + public void toMessage(KickDoorApproachPlanStateMessage message) + { + definition.toMessage(message.getDefinition()); + + super.toMessage(message.getState()); + + kickGoalPose.toMessage(message.getKickGoalPose()); + leftFootGoalPose.toMessage(message.getLeftFootGoalPose()); + rightFootGoalPose.toMessage(message.getRightFootGoalPose()); + message.setTotalNumberOfFootsteps(totalNumberOfFootsteps.toMessage()); + message.setNumberOfIncompleteFootsteps(numberOfIncompleteFootsteps.toMessage()); + desiredFootPoses.get(RobotSide.LEFT).toMessage(message.getDesiredLeftFootsteps()); + desiredFootPoses.get(RobotSide.RIGHT).toMessage(message.getDesiredRightFootsteps()); + currentFootPoses.get(RobotSide.LEFT).toMessage(message.getCurrentLeftFootPose()); + currentFootPoses.get(RobotSide.RIGHT).toMessage(message.getCurrentRightFootPose()); + + message.setExecutionState(executionState.toMessage().toByte()); + } + + public void fromMessage(KickDoorApproachPlanStateMessage message) + { + super.fromMessage(message.getState()); + + definition.fromMessage(message.getDefinition()); + + kickGoalPose.fromMessage(message.getKickGoalPose()); + leftFootGoalPose.fromMessage(message.getLeftFootGoalPose()); + rightFootGoalPose.fromMessage(message.getRightFootGoalPose()); + totalNumberOfFootsteps.fromMessage(message.getTotalNumberOfFootsteps()); + numberOfIncompleteFootsteps.fromMessage(message.getNumberOfIncompleteFootsteps()); + desiredFootPoses.get(RobotSide.LEFT).fromMessage(message.getDesiredLeftFootsteps()); + desiredFootPoses.get(RobotSide.RIGHT).fromMessage(message.getDesiredRightFootsteps()); + currentFootPoses.get(RobotSide.LEFT).fromMessage(message.getCurrentLeftFootPose()); + currentFootPoses.get(RobotSide.RIGHT).fromMessage(message.getCurrentRightFootPose()); + + executionState.fromMessage(FootstepPlanActionExecutionState.fromByte(message.getExecutionState())); + } + + public boolean areFramesInWorld() + { + return referenceFrameLibrary.containsFrame(definition.getParentFrameName()) && parentFrame.getRootFrame() == ReferenceFrame.getWorldFrame(); + } + + public DetachableReferenceFrame getDoorHandleFrame() + { + return doorHandleFrame; + } + + public ReferenceFrame getParentFrame() + { + return parentFrame; + } + + public int getTotalNumberOfFootsteps() + { + return totalNumberOfFootsteps.getValue(); + } + + public void setTotalNumberOfFootsteps(int totalNumberOfFootsteps) + { + this.totalNumberOfFootsteps.setValue(totalNumberOfFootsteps); + } + + public int getNumberOfIncompleteFootsteps() + { + return numberOfIncompleteFootsteps.getValue(); + } + + public void setNumberOfIncompleteFootsteps(int numberOfIncompleteFootsteps) + { + this.numberOfIncompleteFootsteps.setValue(numberOfIncompleteFootsteps); + } + + public CRDTUnidirectionalPose3D getKickGoalPose() + { + return kickGoalPose; + } + + public CRDTUnidirectionalPose3D getLeftFootGoalPose() + { + return leftFootGoalPose; + } + + public CRDTUnidirectionalPose3D getRightFootGoalPose() + { + return rightFootGoalPose; + } + + public SideDependentList getDesiredFootPoses() + { + return desiredFootPoses; + } + + public SideDependentList getCurrentFootPoses() + { + return currentFootPoses; + } + + public CRDTUnidirectionalEnumField getExecutionState() + { + return executionState; + } +} diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/KickDoorCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/KickDoorCommand.java new file mode 100644 index 000000000000..77f5bd4f2b02 --- /dev/null +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/KickDoorCommand.java @@ -0,0 +1,140 @@ +package us.ihmc.humanoidRobotics.communication.controllerAPI.command; + +import controller_msgs.msg.dds.KickDoorMessage; +import us.ihmc.communication.controllerAPI.command.Command; +import us.ihmc.robotics.robotSide.RobotSide; + +public class KickDoorCommand implements Command +{ + private long sequenceId; + private boolean triggerKickRequested = false; + private RobotSide robotSide; + private double kickHeight; + private double kickImpulse; + private double kickTargetDistance; + private double prekickWeightDistribution; + + @Override + public void clear() + { + sequenceId = 0; + triggerKickRequested = false; + robotSide = null; + kickHeight = 0; + kickImpulse = 0; + kickTargetDistance = 0; + prekickWeightDistribution = 0.5; + } + + @Override + public void set(KickDoorCommand other) + { + sequenceId = other.sequenceId; + triggerKickRequested = other.triggerKickRequested; + robotSide = other.robotSide; + kickHeight = other.kickHeight; + kickImpulse = other.kickImpulse; + kickTargetDistance = other.kickTargetDistance; + prekickWeightDistribution = other.prekickWeightDistribution; + } + + @Override + public void setFromMessage(KickDoorMessage message) + { + sequenceId = message.getSequenceId(); + triggerKickRequested = true; + robotSide = RobotSide.fromByte(message.getRobotSide()); + kickHeight = message.getKickHeight(); + kickImpulse = message.getKickImpulse(); + kickTargetDistance = message.getKickTargetDistance(); + prekickWeightDistribution = message.getPrekickWeightDistribution(); + } + + public RobotSide getRobotSide() + { + return robotSide; + } + + public void setRobotSide(RobotSide robotSide) + { + this.robotSide = robotSide; + } + + public double getKickHeight() + { + return kickHeight; + } + + public void setKickHeight(double kickHeight) + { + this.kickHeight = kickHeight; + } + + public double getKickImpulse() + { + return kickImpulse; + } + + public boolean isTriggerKickRequested() + { + return triggerKickRequested; + } + + public void setKickImpulse(double kickImpulse) + { + this.kickImpulse = kickImpulse; + } + + public double getKickTargetDistance() + { + return kickTargetDistance; + } + + public void setKickTargetDistance(double kickTargetDistance) + { + this.kickTargetDistance = kickTargetDistance; + } + + public double getPrekickWeightDistribution() + { + return prekickWeightDistribution; + } + + public void setPrekickWeightDistribution(double prekickWeightDistribution) + { + this.prekickWeightDistribution = prekickWeightDistribution; + } + + @Override + public Class getMessageClass() + { + return KickDoorMessage.class; + } + + @Override + public boolean isCommandValid() + { + if (robotSide == null) + { + return false; + } + + if (kickHeight < 0 || kickImpulse < 0 || kickTargetDistance < 0) + { + return false; + } + + if (prekickWeightDistribution < 0 || prekickWeightDistribution > 1) + { + return false; + } + + return true; + } + + @Override + public long getSequenceId() + { + return sequenceId; + } +} \ No newline at end of file diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl index e09975260efe..75066c9e42cd 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl @@ -12,6 +12,8 @@ #include "behavior_msgs/msg/./HandPoseActionStateMessage_.idl" #include "behavior_msgs/msg/./HandWrenchActionStateMessage_.idl" #include "behavior_msgs/msg/./PelvisHeightOrientationActionStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorActionStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorApproachPlanStateMessage_.idl" #include "behavior_msgs/msg/./SakeHandCommandActionStateMessage_.idl" #include "behavior_msgs/msg/./ScrewPrimitiveActionStateMessage_.idl" #include "behavior_msgs/msg/./TrashCanInteractionStateMessage_.idl" @@ -68,6 +70,11 @@ module behavior_msgs const octet FOOT_POSE_ACTION = 18; + const octet KICK_DOOR_ACTION = + 19; + + const octet KICK_DOOR_APPROACH_ACTION = + 20; /** * Gives the current state of the complete collection of behavior tree nodes. @@ -115,6 +122,8 @@ module behavior_msgs sequence pelvis_height_actions; sequence wait_duration_actions; sequence foot_pose_actions; + sequence kick_door_actions; + sequence kick_door_approach_actions; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl new file mode 100644 index 000000000000..c9ddb192ba18 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl @@ -0,0 +1,53 @@ +#ifndef __behavior_msgs__msg__KickDoorActionDefinitionMessage__idl__ +#define __behavior_msgs__msg__KickDoorActionDefinitionMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeDefinitionMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorActionDefinitionMessage_") + struct KickDoorActionDefinitionMessage + { + /** + * Parent definition fields + */ + behavior_msgs::msg::dds::ActionNodeDefinitionMessage definition; + /** + * Name of the parent frame the footsteps are expressed in + */ + string parent_frame_name; + /** + * Specifies the side of the robot that will execute the kick. + */ + @defaultValue(value=255) + octet robot_side; + /** + * The height at which the kick should be targeted. + */ + double kick_height; + /** + * The impulse with which the kick should be executed. + */ + double kick_impulse; + /** + * The target distance from the robot to where the kick should be aimed. + */ + double kick_target_distance; + /** + * The stance foot width. + */ + double stance_foot_width; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + double prekick_weight_distribution; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl new file mode 100644 index 000000000000..8c3a6b26bdfa --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl @@ -0,0 +1,33 @@ +#ifndef __behavior_msgs__msg__KickDoorActionStateMessage__idl__ +#define __behavior_msgs__msg__KickDoorActionStateMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorActionDefinitionMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorActionStateMessage_") + struct KickDoorActionStateMessage + { + /** + * Parent state fields + */ + behavior_msgs::msg::dds::ActionNodeStateMessage state; + /** + * Definition + */ + behavior_msgs::msg::dds::KickDoorActionDefinitionMessage definition; + /** + * Execution state + */ + octet execution_state; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl new file mode 100644 index 000000000000..74c50d717d18 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl @@ -0,0 +1,69 @@ +#ifndef __behavior_msgs__msg__KickDoorApproachPlanDefinitionMessage__idl__ +#define __behavior_msgs__msg__KickDoorApproachPlanDefinitionMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeDefinitionMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorApproachPlanDefinitionMessage_") + struct KickDoorApproachPlanDefinitionMessage + { + /** + * Parent definition fields + */ + behavior_msgs::msg::dds::ActionNodeDefinitionMessage definition; + /** + * Name of the parent frame the footsteps are expressed in + */ + string parent_frame_name; + /** + * Swing duration + */ + double swing_duration; + /** + * Transfer duration + */ + double transfer_duration; + /** + * OVERRIDE (0) or QUEUE (1) + */ + long execution_mode; + /** + * Specifies the side of the robot that will execute the kick. + */ + @defaultValue(value=255) + octet robot_side; + /** + * The impulse with which the kick should be executed. + */ + double kick_height; + /** + * The impulse with which the kick should be executed. + */ + double kick_impulse; + /** + * The target distance from the robot to where the kick should be aimed. + */ + double kick_target_distance; + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + double horizontal_distance_from_handle; + /** + * The stance foot width. + */ + double stance_foot_width; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + double prekick_weight_distribution; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl new file mode 100644 index 000000000000..f654785e81b5 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl @@ -0,0 +1,83 @@ +#ifndef __behavior_msgs__msg__KickDoorApproachPlanStateMessage__idl__ +#define __behavior_msgs__msg__KickDoorApproachPlanStateMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorApproachPlanDefinitionMessage_.idl" +#include "geometry_msgs/msg/./Pose_.idl" +#include "ihmc_common_msgs/msg/./SE3TrajectoryPointMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + const octet FOOTSTEP_PLANNING = + 0; + + const octet PLANNING_FAILED = + 1; + + const octet PLANNING_SUCCEEDED = + 2; + + const octet PLAN_COMMANDED = + 3; + + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorApproachPlanStateMessage_") + struct KickDoorApproachPlanStateMessage + { + /** + * Parent state fields + */ + behavior_msgs::msg::dds::ActionNodeStateMessage state; + /** + * Definition + */ + behavior_msgs::msg::dds::KickDoorApproachPlanDefinitionMessage definition; + /** + * Kick goal pose + */ + geometry_msgs::msg::dds::Pose kick_goal_pose; + /** + * Left foot goal pose + */ + geometry_msgs::msg::dds::Pose left_foot_goal_pose; + /** + * Right foot goal pose + */ + geometry_msgs::msg::dds::Pose right_foot_goal_pose; + /** + * Execution state + */ + octet execution_state; + /** + * Total number of footsteps; used for walking actions + */ + unsigned short total_number_of_footsteps; + /** + * Incomplete footsteps; used for walking actions + */ + unsigned short number_of_incomplete_footsteps; + /** + * Desired left footsteps + */ + sequence desired_left_footsteps; + /** + * Desired right footsteps + */ + sequence desired_right_footsteps; + /** + * Current left pose + */ + geometry_msgs::msg::dds::Pose current_left_foot_pose; + /** + * Current right pose + */ + geometry_msgs::msg::dds::Pose current_right_foot_pose; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/KickDoorMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/KickDoorMessage_.idl new file mode 100644 index 000000000000..3283fe419ae7 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/KickDoorMessage_.idl @@ -0,0 +1,57 @@ +#ifndef __controller_msgs__msg__KickDoorMessage__idl__ +#define __controller_msgs__msg__KickDoorMessage__idl__ + +module controller_msgs +{ + module msg + { + module dds + { + const octet ROBOT_SIDE_LEFT = + 0; + + const octet ROBOT_SIDE_RIGHT = + 1; + + + /** + * This message is part of the IHMC whole-body controller API. + * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + */ + @TypeCode(type="controller_msgs::msg::dds_::KickDoorMessage_") + struct KickDoorMessage + { + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + unsigned long sequence_id; + /** + * Specifies the side of the robot that will execute the kick. + */ + octet robot_side; + /** + * The height at which the kick should be targeted. + */ + double kick_height; + /** + * The impulse with which the kick should be executed. + */ + double kick_impulse; + /** + * The target distance from the robot to where the kick should be aimed. + */ + double kick_target_distance; + /** + * A boolean for tracking whether a kick has been requested. + */ + boolean trigger_kick_request; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + double prekick_weight_distribution; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java index 8aa1d24a5fe0..7dc7f31c3c27 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java @@ -29,6 +29,8 @@ public class BehaviorTreeStateMessage extends Packet i public static final byte PELVIS_HEIGHT_ORIENTATION_ACTION = (byte) 16; public static final byte WAIT_DURATION_ACTION = (byte) 17; public static final byte FOOT_POSE_ACTION = (byte) 18; + public static final byte KICK_DOOR_ACTION = (byte) 19; + public static final byte KICK_DOOR_APPROACH_ACTION = (byte) 20; /** * Monotonically increasing message ID that matches the CRDTInfo update number */ @@ -66,6 +68,8 @@ public class BehaviorTreeStateMessage extends Packet i public us.ihmc.idl.IDLSequence.Object pelvis_height_actions_; public us.ihmc.idl.IDLSequence.Object wait_duration_actions_; public us.ihmc.idl.IDLSequence.Object foot_pose_actions_; + public us.ihmc.idl.IDLSequence.Object kick_door_actions_; + public us.ihmc.idl.IDLSequence.Object kick_door_approach_actions_; public BehaviorTreeStateMessage() { @@ -89,6 +93,8 @@ public BehaviorTreeStateMessage() pelvis_height_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.PelvisHeightOrientationActionStateMessagePubSubType()); wait_duration_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType()); foot_pose_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.FootPoseActionStateMessagePubSubType()); + kick_door_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType()); + kick_door_approach_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.KickDoorApproachPlanStateMessagePubSubType()); } @@ -122,6 +128,8 @@ public void set(BehaviorTreeStateMessage other) pelvis_height_actions_.set(other.pelvis_height_actions_); wait_duration_actions_.set(other.wait_duration_actions_); foot_pose_actions_.set(other.foot_pose_actions_); + kick_door_actions_.set(other.kick_door_actions_); + kick_door_approach_actions_.set(other.kick_door_approach_actions_); } /** @@ -274,6 +282,18 @@ public us.ihmc.idl.IDLSequence.Object getKickDoorActions() + { + return kick_door_actions_; + } + + + public us.ihmc.idl.IDLSequence.Object getKickDoorApproachActions() + { + return kick_door_approach_actions_; + } + + public static Supplier getPubSubType() { return BehaviorTreeStateMessagePubSubType::new; @@ -405,6 +425,20 @@ public boolean epsilonEquals(BehaviorTreeStateMessage other, double epsilon) { if (!this.foot_pose_actions_.get(i).epsilonEquals(other.foot_pose_actions_.get(i), epsilon)) return false; } } + if (this.kick_door_actions_.size() != other.kick_door_actions_.size()) { return false; } + else + { + for (int i = 0; i < this.kick_door_actions_.size(); i++) + { if (!this.kick_door_actions_.get(i).epsilonEquals(other.kick_door_actions_.get(i), epsilon)) return false; } + } + + if (this.kick_door_approach_actions_.size() != other.kick_door_approach_actions_.size()) { return false; } + else + { + for (int i = 0; i < this.kick_door_approach_actions_.size(); i++) + { if (!this.kick_door_approach_actions_.get(i).epsilonEquals(other.kick_door_approach_actions_.get(i), epsilon)) return false; } + } + return true; } @@ -440,6 +474,8 @@ public boolean equals(Object other) if (!this.pelvis_height_actions_.equals(otherMyClass.pelvis_height_actions_)) return false; if (!this.wait_duration_actions_.equals(otherMyClass.wait_duration_actions_)) return false; if (!this.foot_pose_actions_.equals(otherMyClass.foot_pose_actions_)) return false; + if (!this.kick_door_actions_.equals(otherMyClass.kick_door_actions_)) return false; + if (!this.kick_door_approach_actions_.equals(otherMyClass.kick_door_approach_actions_)) return false; return true; } @@ -490,6 +526,11 @@ public java.lang.String toString() builder.append(this.wait_duration_actions_); builder.append(", "); builder.append("foot_pose_actions="); builder.append(this.foot_pose_actions_); + builder.append(this.wait_duration_actions_); builder.append(", "); + builder.append("kick_door_actions="); + builder.append(this.kick_door_actions_); builder.append(", "); + builder.append("kick_door_approach_actions="); + builder.append(this.kick_door_approach_actions_); builder.append("}"); return builder.toString(); } diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java index 06f689c65551..11de1cf10578 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java @@ -107,6 +107,12 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) { current_alignment += behavior_msgs.msg.dds.FootPoseActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} return current_alignment - initial_alignment; } @@ -211,6 +217,16 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeS { current_alignment += behavior_msgs.msg.dds.FootPoseActionStateMessagePubSubType.getCdrSerializedSize(data.getFootPoseActions().get(i0), current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getKickDoorActions().size(); ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getCdrSerializedSize(data.getKickDoorActions().get(i0), current_alignment);} + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getKickDoorApproachActions().size(); ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanStateMessagePubSubType.getCdrSerializedSize(data.getKickDoorApproachActions().get(i0), current_alignment);} + return current_alignment - initial_alignment; } @@ -290,6 +306,14 @@ public static void write(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us cdr.write_type_e(data.getFootPoseActions());else throw new RuntimeException("foot_pose_actions field exceeds the maximum length"); + if(data.getKickDoorActions().size() <= 200) + cdr.write_type_e(data.getKickDoorActions());else + throw new RuntimeException("kick_door_actions field exceeds the maximum length"); + + if(data.getKickDoorApproachActions().size() <= 200) + cdr.write_type_e(data.getKickDoorApproachActions());else + throw new RuntimeException("kick_door_approach_actions field exceeds the maximum length"); + } public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us.ihmc.idl.CDR cdr) @@ -301,13 +325,13 @@ public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us. ihmc_common_msgs.msg.dds.ConfirmableRequestMessagePubSubType.read(data.getConfirmableRequest(), cdr); cdr.read_type_e(data.getBehaviorTreeTypes()); cdr.read_type_e(data.getBehaviorTreeIndices()); - cdr.read_type_e(data.getRootNodes()); - cdr.read_type_e(data.getBasicNodes()); + cdr.read_type_e(data.getRootNodes()); + cdr.read_type_e(data.getBasicNodes()); cdr.read_type_e(data.getActionSequences()); cdr.read_type_e(data.getDoorTraversals()); - cdr.read_type_e(data.getTrashCanInteractions()); - cdr.read_type_e(data.getBuildingExplorations()); - cdr.read_type_e(data.getChestOrientationActions()); + cdr.read_type_e(data.getTrashCanInteractions()); + cdr.read_type_e(data.getBuildingExplorations()); + cdr.read_type_e(data.getChestOrientationActions()); cdr.read_type_e(data.getFootstepPlanActions()); cdr.read_type_e(data.getSakeHandCommandActions()); cdr.read_type_e(data.getHandPoseActions()); @@ -315,7 +339,9 @@ public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us. cdr.read_type_e(data.getScrewPrimitiveActions()); cdr.read_type_e(data.getPelvisHeightActions()); cdr.read_type_e(data.getWaitDurationActions()); - cdr.read_type_e(data.getFootPoseActions()); + cdr.read_type_e(data.getFootPoseActions()); + cdr.read_type_e(data.getKickDoorActions()); + cdr.read_type_e(data.getKickDoorApproachActions()); } @@ -343,6 +369,8 @@ public final void serialize(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, ser.write_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.write_type_e("wait_duration_actions", data.getWaitDurationActions()); ser.write_type_e("foot_pose_actions", data.getFootPoseActions()); + ser.write_type_e("kick_door_actions", data.getKickDoorActions()); + ser.write_type_e("kick_door_approach_actions", data.getKickDoorApproachActions()); } @Override @@ -369,6 +397,8 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.read_type_e("wait_duration_actions", data.getWaitDurationActions()); ser.read_type_e("foot_pose_actions", data.getFootPoseActions()); + ser.read_type_e("kick_door_actions", data.getKickDoorActions()); + ser.read_type_e("kick_door_approach_actions", data.getKickDoorApproachActions()); } public static void staticCopy(behavior_msgs.msg.dds.BehaviorTreeStateMessage src, behavior_msgs.msg.dds.BehaviorTreeStateMessage dest) diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java new file mode 100644 index 000000000000..bfd2d0075086 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java @@ -0,0 +1,289 @@ +package behavior_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +public class KickDoorActionDefinitionMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage definition_; + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.StringBuilder parent_frame_name_; + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte robot_side_ = (byte) 255; + /** + * The height at which the kick should be targeted. + */ + public double kick_height_; + /** + * The impulse with which the kick should be executed. + */ + public double kick_impulse_; + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double kick_target_distance_; + /** + * The stance foot width. + */ + public double stance_foot_width_; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double prekick_weight_distribution_; + + public KickDoorActionDefinitionMessage() + { + definition_ = new behavior_msgs.msg.dds.ActionNodeDefinitionMessage(); + parent_frame_name_ = new java.lang.StringBuilder(255); + } + + public KickDoorActionDefinitionMessage(KickDoorActionDefinitionMessage other) + { + this(); + set(other); + } + + public void set(KickDoorActionDefinitionMessage other) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + parent_frame_name_.setLength(0); + parent_frame_name_.append(other.parent_frame_name_); + + robot_side_ = other.robot_side_; + + kick_height_ = other.kick_height_; + + kick_impulse_ = other.kick_impulse_; + + kick_target_distance_ = other.kick_target_distance_; + + stance_foot_width_ = other.stance_foot_width_; + + prekick_weight_distribution_ = other.prekick_weight_distribution_; + + } + + + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage getDefinition() + { + return definition_; + } + + /** + * Name of the parent frame the footsteps are expressed in + */ + public void setParentFrameName(java.lang.String parent_frame_name) + { + parent_frame_name_.setLength(0); + parent_frame_name_.append(parent_frame_name); + } + + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.String getParentFrameNameAsString() + { + return getParentFrameName().toString(); + } + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.StringBuilder getParentFrameName() + { + return parent_frame_name_; + } + + /** + * Specifies the side of the robot that will execute the kick. + */ + public void setRobotSide(byte robot_side) + { + robot_side_ = robot_side; + } + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte getRobotSide() + { + return robot_side_; + } + + /** + * The height at which the kick should be targeted. + */ + public void setKickHeight(double kick_height) + { + kick_height_ = kick_height; + } + /** + * The height at which the kick should be targeted. + */ + public double getKickHeight() + { + return kick_height_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickImpulse(double kick_impulse) + { + kick_impulse_ = kick_impulse; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickImpulse() + { + return kick_impulse_; + } + + /** + * The target distance from the robot to where the kick should be aimed. + */ + public void setKickTargetDistance(double kick_target_distance) + { + kick_target_distance_ = kick_target_distance; + } + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double getKickTargetDistance() + { + return kick_target_distance_; + } + + /** + * The stance foot width. + */ + public void setStanceFootWidth(double stance_foot_width) + { + stance_foot_width_ = stance_foot_width; + } + /** + * The stance foot width. + */ + public double getStanceFootWidth() + { + return stance_foot_width_; + } + + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public void setPrekickWeightDistribution(double prekick_weight_distribution) + { + prekick_weight_distribution_ = prekick_weight_distribution; + } + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double getPrekickWeightDistribution() + { + return prekick_weight_distribution_; + } + + + public static Supplier getPubSubType() + { + return KickDoorActionDefinitionMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorActionDefinitionMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorActionDefinitionMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.parent_frame_name_, other.parent_frame_name_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.stance_foot_width_, other.stance_foot_width_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorActionDefinitionMessage)) return false; + + KickDoorActionDefinitionMessage otherMyClass = (KickDoorActionDefinitionMessage) other; + + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if (!us.ihmc.idl.IDLTools.equals(this.parent_frame_name_, otherMyClass.parent_frame_name_)) return false; + + if(this.robot_side_ != otherMyClass.robot_side_) return false; + + if(this.kick_height_ != otherMyClass.kick_height_) return false; + + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + + if(this.stance_foot_width_ != otherMyClass.stance_foot_width_) return false; + + if(this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorActionDefinitionMessage {"); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("parent_frame_name="); + builder.append(this.parent_frame_name_); builder.append(", "); + builder.append("robot_side="); + builder.append(this.robot_side_); builder.append(", "); + builder.append("kick_height="); + builder.append(this.kick_height_); builder.append(", "); + builder.append("kick_impulse="); + builder.append(this.kick_impulse_); builder.append(", "); + builder.append("kick_target_distance="); + builder.append(this.kick_target_distance_); builder.append(", "); + builder.append("stance_foot_width="); + builder.append(this.stance_foot_width_); builder.append(", "); + builder.append("prekick_weight_distribution="); + builder.append(this.prekick_weight_distribution_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java new file mode 100644 index 000000000000..c12b21a1966f --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java @@ -0,0 +1,219 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorActionDefinitionMessage" defined in "KickDoorActionDefinitionMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorActionDefinitionMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorActionDefinitionMessage_.idl instead. +* +*/ +public class KickDoorActionDefinitionMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorActionDefinitionMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "9478ac2d9badffabc50eaeb6d9af80bd635df624a5f6a9972035245abb6a67c6"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1; + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getParentFrameName().length() + 1; + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + if(data.getParentFrameName().length() <= 255) + cdr.write_type_d(data.getParentFrameName());else + throw new RuntimeException("parent_frame_name field exceeds the maximum length"); + + cdr.write_type_9(data.getRobotSide()); + + cdr.write_type_6(data.getKickHeight()); + + cdr.write_type_6(data.getKickImpulse()); + + cdr.write_type_6(data.getKickTargetDistance()); + + cdr.write_type_6(data.getStanceFootWidth()); + + cdr.write_type_6(data.getPrekickWeightDistribution()); + + } + + public static void read(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + cdr.read_type_d(data.getParentFrameName()); + data.setRobotSide(cdr.read_type_9()); + + data.setKickHeight(cdr.read_type_6()); + + data.setKickImpulse(cdr.read_type_6()); + + data.setKickTargetDistance(cdr.read_type_6()); + + data.setStanceFootWidth(cdr.read_type_6()); + + data.setPrekickWeightDistribution(cdr.read_type_6()); + + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_d("parent_frame_name", data.getParentFrameName()); + ser.write_type_9("robot_side", data.getRobotSide()); + ser.write_type_6("kick_height", data.getKickHeight()); + ser.write_type_6("kick_impulse", data.getKickImpulse()); + ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); + ser.write_type_6("stance_foot_width", data.getStanceFootWidth()); + ser.write_type_6("prekick_weight_distribution", data.getPrekickWeightDistribution()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data) + { + ser.read_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.read_type_d("parent_frame_name", data.getParentFrameName()); + data.setRobotSide(ser.read_type_9("robot_side")); + data.setKickHeight(ser.read_type_6("kick_height")); + data.setKickImpulse(ser.read_type_6("kick_impulse")); + data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); + data.setStanceFootWidth(ser.read_type_6("stance_foot_width")); + data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage src, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorActionDefinitionMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorActionDefinitionMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage src, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorActionDefinitionMessagePubSubType newInstance() + { + return new KickDoorActionDefinitionMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java new file mode 100644 index 000000000000..dc76a9807df0 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java @@ -0,0 +1,135 @@ +package behavior_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +public class KickDoorActionStateMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage state_; + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorActionDefinitionMessage definition_; + /** + * Execution state + */ + public byte execution_state_; + + public KickDoorActionStateMessage() + { + state_ = new behavior_msgs.msg.dds.ActionNodeStateMessage(); + definition_ = new behavior_msgs.msg.dds.KickDoorActionDefinitionMessage(); + } + + public KickDoorActionStateMessage(KickDoorActionStateMessage other) + { + this(); + set(other); + } + + public void set(KickDoorActionStateMessage other) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.staticCopy(other.state_, state_); + behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + execution_state_ = other.execution_state_; + + } + + + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage getState() + { + return state_; + } + + + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorActionDefinitionMessage getDefinition() + { + return definition_; + } + + /** + * Execution state + */ + public void setExecutionState(byte execution_state) + { + execution_state_ = execution_state; + } + /** + * Execution state + */ + public byte getExecutionState() + { + return execution_state_; + } + + + public static Supplier getPubSubType() + { + return KickDoorActionStateMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorActionStateMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorActionStateMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_state_, other.execution_state_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorActionStateMessage)) return false; + + KickDoorActionStateMessage otherMyClass = (KickDoorActionStateMessage) other; + + if (!this.state_.equals(otherMyClass.state_)) return false; + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if(this.execution_state_ != otherMyClass.execution_state_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorActionStateMessage {"); + builder.append("state="); + builder.append(this.state_); builder.append(", "); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("execution_state="); + builder.append(this.execution_state_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java new file mode 100644 index 000000000000..860b48cf0451 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java @@ -0,0 +1,164 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorActionStateMessage" defined in "KickDoorActionStateMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorActionStateMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorActionStateMessage_.idl instead. +* +*/ +public class KickDoorActionStateMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorActionStateMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "ede1f590238e3157f15183617320a585334fc38d88c45b96af7504fa55a7daaf"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.KickDoorActionStateMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionStateMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getCdrSerializedSize(data.getState(), current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + cdr.write_type_9(data.getExecutionState()); + + } + + public static void read(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + data.setExecutionState(cdr.read_type_9()); + + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_9("execution_state", data.getExecutionState()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorActionStateMessage data) + { + ser.read_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType(), data.getDefinition()); + + data.setExecutionState(ser.read_type_9("execution_state")); + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorActionStateMessage src, behavior_msgs.msg.dds.KickDoorActionStateMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorActionStateMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorActionStateMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorActionStateMessage src, behavior_msgs.msg.dds.KickDoorActionStateMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorActionStateMessagePubSubType newInstance() + { + return new KickDoorActionStateMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java new file mode 100644 index 000000000000..24cce3eac110 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java @@ -0,0 +1,397 @@ +package behavior_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +public class KickDoorApproachPlanDefinitionMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage definition_; + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.StringBuilder parent_frame_name_; + /** + * Swing duration + */ + public double swing_duration_; + /** + * Transfer duration + */ + public double transfer_duration_; + /** + * OVERRIDE (0) or QUEUE (1) + */ + public int execution_mode_; + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte robot_side_ = (byte) 255; + /** + * The impulse with which the kick should be executed. + */ + public double kick_height_; + /** + * The impulse with which the kick should be executed. + */ + public double kick_impulse_; + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double kick_target_distance_; + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + public double horizontal_distance_from_handle_; + /** + * The stance foot width. + */ + public double stance_foot_width_; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double prekick_weight_distribution_; + + public KickDoorApproachPlanDefinitionMessage() + { + definition_ = new behavior_msgs.msg.dds.ActionNodeDefinitionMessage(); + parent_frame_name_ = new java.lang.StringBuilder(255); + } + + public KickDoorApproachPlanDefinitionMessage(KickDoorApproachPlanDefinitionMessage other) + { + this(); + set(other); + } + + public void set(KickDoorApproachPlanDefinitionMessage other) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + parent_frame_name_.setLength(0); + parent_frame_name_.append(other.parent_frame_name_); + + swing_duration_ = other.swing_duration_; + + transfer_duration_ = other.transfer_duration_; + + execution_mode_ = other.execution_mode_; + + robot_side_ = other.robot_side_; + + kick_height_ = other.kick_height_; + + kick_impulse_ = other.kick_impulse_; + + kick_target_distance_ = other.kick_target_distance_; + + horizontal_distance_from_handle_ = other.horizontal_distance_from_handle_; + + stance_foot_width_ = other.stance_foot_width_; + + prekick_weight_distribution_ = other.prekick_weight_distribution_; + + } + + + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage getDefinition() + { + return definition_; + } + + /** + * Name of the parent frame the footsteps are expressed in + */ + public void setParentFrameName(java.lang.String parent_frame_name) + { + parent_frame_name_.setLength(0); + parent_frame_name_.append(parent_frame_name); + } + + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.String getParentFrameNameAsString() + { + return getParentFrameName().toString(); + } + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.StringBuilder getParentFrameName() + { + return parent_frame_name_; + } + + /** + * Swing duration + */ + public void setSwingDuration(double swing_duration) + { + swing_duration_ = swing_duration; + } + /** + * Swing duration + */ + public double getSwingDuration() + { + return swing_duration_; + } + + /** + * Transfer duration + */ + public void setTransferDuration(double transfer_duration) + { + transfer_duration_ = transfer_duration; + } + /** + * Transfer duration + */ + public double getTransferDuration() + { + return transfer_duration_; + } + + /** + * OVERRIDE (0) or QUEUE (1) + */ + public void setExecutionMode(int execution_mode) + { + execution_mode_ = execution_mode; + } + /** + * OVERRIDE (0) or QUEUE (1) + */ + public int getExecutionMode() + { + return execution_mode_; + } + + /** + * Specifies the side of the robot that will execute the kick. + */ + public void setRobotSide(byte robot_side) + { + robot_side_ = robot_side; + } + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte getRobotSide() + { + return robot_side_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickHeight(double kick_height) + { + kick_height_ = kick_height; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickHeight() + { + return kick_height_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickImpulse(double kick_impulse) + { + kick_impulse_ = kick_impulse; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickImpulse() + { + return kick_impulse_; + } + + /** + * The target distance from the robot to where the kick should be aimed. + */ + public void setKickTargetDistance(double kick_target_distance) + { + kick_target_distance_ = kick_target_distance; + } + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double getKickTargetDistance() + { + return kick_target_distance_; + } + + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + public void setHorizontalDistanceFromHandle(double horizontal_distance_from_handle) + { + horizontal_distance_from_handle_ = horizontal_distance_from_handle; + } + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + public double getHorizontalDistanceFromHandle() + { + return horizontal_distance_from_handle_; + } + + /** + * The stance foot width. + */ + public void setStanceFootWidth(double stance_foot_width) + { + stance_foot_width_ = stance_foot_width; + } + /** + * The stance foot width. + */ + public double getStanceFootWidth() + { + return stance_foot_width_; + } + + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public void setPrekickWeightDistribution(double prekick_weight_distribution) + { + prekick_weight_distribution_ = prekick_weight_distribution; + } + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double getPrekickWeightDistribution() + { + return prekick_weight_distribution_; + } + + + public static Supplier getPubSubType() + { + return KickDoorApproachPlanDefinitionMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorApproachPlanDefinitionMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorApproachPlanDefinitionMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.parent_frame_name_, other.parent_frame_name_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.swing_duration_, other.swing_duration_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.transfer_duration_, other.transfer_duration_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_mode_, other.execution_mode_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.horizontal_distance_from_handle_, other.horizontal_distance_from_handle_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.stance_foot_width_, other.stance_foot_width_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorApproachPlanDefinitionMessage)) return false; + + KickDoorApproachPlanDefinitionMessage otherMyClass = (KickDoorApproachPlanDefinitionMessage) other; + + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if (!us.ihmc.idl.IDLTools.equals(this.parent_frame_name_, otherMyClass.parent_frame_name_)) return false; + + if(this.swing_duration_ != otherMyClass.swing_duration_) return false; + + if(this.transfer_duration_ != otherMyClass.transfer_duration_) return false; + + if(this.execution_mode_ != otherMyClass.execution_mode_) return false; + + if(this.robot_side_ != otherMyClass.robot_side_) return false; + + if(this.kick_height_ != otherMyClass.kick_height_) return false; + + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + + if(this.horizontal_distance_from_handle_ != otherMyClass.horizontal_distance_from_handle_) return false; + + if(this.stance_foot_width_ != otherMyClass.stance_foot_width_) return false; + + if(this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorApproachPlanDefinitionMessage {"); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("parent_frame_name="); + builder.append(this.parent_frame_name_); builder.append(", "); + builder.append("swing_duration="); + builder.append(this.swing_duration_); builder.append(", "); + builder.append("transfer_duration="); + builder.append(this.transfer_duration_); builder.append(", "); + builder.append("execution_mode="); + builder.append(this.execution_mode_); builder.append(", "); + builder.append("robot_side="); + builder.append(this.robot_side_); builder.append(", "); + builder.append("kick_height="); + builder.append(this.kick_height_); builder.append(", "); + builder.append("kick_impulse="); + builder.append(this.kick_impulse_); builder.append(", "); + builder.append("kick_target_distance="); + builder.append(this.kick_target_distance_); builder.append(", "); + builder.append("horizontal_distance_from_handle="); + builder.append(this.horizontal_distance_from_handle_); builder.append(", "); + builder.append("stance_foot_width="); + builder.append(this.stance_foot_width_); builder.append(", "); + builder.append("prekick_weight_distribution="); + builder.append(this.prekick_weight_distribution_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java new file mode 100644 index 000000000000..7d98d75e7892 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java @@ -0,0 +1,263 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorApproachPlanDefinitionMessage" defined in "KickDoorApproachPlanDefinitionMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorApproachPlanDefinitionMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorApproachPlanDefinitionMessage_.idl instead. +* +*/ +public class KickDoorApproachPlanDefinitionMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorApproachPlanDefinitionMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "8f2dcdf800051a6b949b2be35689079f9520d93925ff6d1311a90b78b9b4f312"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1; + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getParentFrameName().length() + 1; + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + if(data.getParentFrameName().length() <= 255) + cdr.write_type_d(data.getParentFrameName());else + throw new RuntimeException("parent_frame_name field exceeds the maximum length"); + + cdr.write_type_6(data.getSwingDuration()); + + cdr.write_type_6(data.getTransferDuration()); + + cdr.write_type_2(data.getExecutionMode()); + + cdr.write_type_9(data.getRobotSide()); + + cdr.write_type_6(data.getKickHeight()); + + cdr.write_type_6(data.getKickImpulse()); + + cdr.write_type_6(data.getKickTargetDistance()); + + cdr.write_type_6(data.getHorizontalDistanceFromHandle()); + + cdr.write_type_6(data.getStanceFootWidth()); + + cdr.write_type_6(data.getPrekickWeightDistribution()); + + } + + public static void read(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + cdr.read_type_d(data.getParentFrameName()); + data.setSwingDuration(cdr.read_type_6()); + + data.setTransferDuration(cdr.read_type_6()); + + data.setExecutionMode(cdr.read_type_2()); + + data.setRobotSide(cdr.read_type_9()); + + data.setKickHeight(cdr.read_type_6()); + + data.setKickImpulse(cdr.read_type_6()); + + data.setKickTargetDistance(cdr.read_type_6()); + + data.setHorizontalDistanceFromHandle(cdr.read_type_6()); + + data.setStanceFootWidth(cdr.read_type_6()); + + data.setPrekickWeightDistribution(cdr.read_type_6()); + + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_d("parent_frame_name", data.getParentFrameName()); + ser.write_type_6("swing_duration", data.getSwingDuration()); + ser.write_type_6("transfer_duration", data.getTransferDuration()); + ser.write_type_2("execution_mode", data.getExecutionMode()); + ser.write_type_9("robot_side", data.getRobotSide()); + ser.write_type_6("kick_height", data.getKickHeight()); + ser.write_type_6("kick_impulse", data.getKickImpulse()); + ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); + ser.write_type_6("horizontal_distance_from_handle", data.getHorizontalDistanceFromHandle()); + ser.write_type_6("stance_foot_width", data.getStanceFootWidth()); + ser.write_type_6("prekick_weight_distribution", data.getPrekickWeightDistribution()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data) + { + ser.read_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.read_type_d("parent_frame_name", data.getParentFrameName()); + data.setSwingDuration(ser.read_type_6("swing_duration")); + data.setTransferDuration(ser.read_type_6("transfer_duration")); + data.setExecutionMode(ser.read_type_2("execution_mode")); + data.setRobotSide(ser.read_type_9("robot_side")); + data.setKickHeight(ser.read_type_6("kick_height")); + data.setKickImpulse(ser.read_type_6("kick_impulse")); + data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); + data.setHorizontalDistanceFromHandle(ser.read_type_6("horizontal_distance_from_handle")); + data.setStanceFootWidth(ser.read_type_6("stance_foot_width")); + data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorApproachPlanDefinitionMessagePubSubType newInstance() + { + return new KickDoorApproachPlanDefinitionMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java new file mode 100644 index 000000000000..a89ef889837a --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java @@ -0,0 +1,339 @@ +package behavior_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +public class KickDoorApproachPlanStateMessage extends Packet implements Settable, EpsilonComparable +{ + public static final byte FOOTSTEP_PLANNING = (byte) 0; + public static final byte PLANNING_FAILED = (byte) 1; + public static final byte PLANNING_SUCCEEDED = (byte) 2; + public static final byte PLAN_COMMANDED = (byte) 3; + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage state_; + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage definition_; + /** + * Kick goal pose + */ + public us.ihmc.euclid.geometry.Pose3D kick_goal_pose_; + /** + * Left foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D left_foot_goal_pose_; + /** + * Right foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D right_foot_goal_pose_; + /** + * Execution state + */ + public byte execution_state_; + /** + * Total number of footsteps; used for walking actions + */ + public int total_number_of_footsteps_; + /** + * Incomplete footsteps; used for walking actions + */ + public int number_of_incomplete_footsteps_; + /** + * Desired left footsteps + */ + public us.ihmc.idl.IDLSequence.Object desired_left_footsteps_; + /** + * Desired right footsteps + */ + public us.ihmc.idl.IDLSequence.Object desired_right_footsteps_; + /** + * Current left pose + */ + public us.ihmc.euclid.geometry.Pose3D current_left_foot_pose_; + /** + * Current right pose + */ + public us.ihmc.euclid.geometry.Pose3D current_right_foot_pose_; + + public KickDoorApproachPlanStateMessage() + { + state_ = new behavior_msgs.msg.dds.ActionNodeStateMessage(); + definition_ = new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage(); + kick_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + left_foot_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + right_foot_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + desired_left_footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType()); + desired_right_footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType()); + current_left_foot_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + current_right_foot_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + + } + + public KickDoorApproachPlanStateMessage(KickDoorApproachPlanStateMessage other) + { + this(); + set(other); + } + + public void set(KickDoorApproachPlanStateMessage other) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.staticCopy(other.state_, state_); + behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.kick_goal_pose_, kick_goal_pose_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.left_foot_goal_pose_, left_foot_goal_pose_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.right_foot_goal_pose_, right_foot_goal_pose_); + execution_state_ = other.execution_state_; + + total_number_of_footsteps_ = other.total_number_of_footsteps_; + + number_of_incomplete_footsteps_ = other.number_of_incomplete_footsteps_; + + desired_left_footsteps_.set(other.desired_left_footsteps_); + desired_right_footsteps_.set(other.desired_right_footsteps_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.current_left_foot_pose_, current_left_foot_pose_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.current_right_foot_pose_, current_right_foot_pose_); + } + + + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage getState() + { + return state_; + } + + + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage getDefinition() + { + return definition_; + } + + + /** + * Kick goal pose + */ + public us.ihmc.euclid.geometry.Pose3D getKickGoalPose() + { + return kick_goal_pose_; + } + + + /** + * Left foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D getLeftFootGoalPose() + { + return left_foot_goal_pose_; + } + + + /** + * Right foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D getRightFootGoalPose() + { + return right_foot_goal_pose_; + } + + /** + * Execution state + */ + public void setExecutionState(byte execution_state) + { + execution_state_ = execution_state; + } + /** + * Execution state + */ + public byte getExecutionState() + { + return execution_state_; + } + + /** + * Total number of footsteps; used for walking actions + */ + public void setTotalNumberOfFootsteps(int total_number_of_footsteps) + { + total_number_of_footsteps_ = total_number_of_footsteps; + } + /** + * Total number of footsteps; used for walking actions + */ + public int getTotalNumberOfFootsteps() + { + return total_number_of_footsteps_; + } + + /** + * Incomplete footsteps; used for walking actions + */ + public void setNumberOfIncompleteFootsteps(int number_of_incomplete_footsteps) + { + number_of_incomplete_footsteps_ = number_of_incomplete_footsteps; + } + /** + * Incomplete footsteps; used for walking actions + */ + public int getNumberOfIncompleteFootsteps() + { + return number_of_incomplete_footsteps_; + } + + + /** + * Desired left footsteps + */ + public us.ihmc.idl.IDLSequence.Object getDesiredLeftFootsteps() + { + return desired_left_footsteps_; + } + + + /** + * Desired right footsteps + */ + public us.ihmc.idl.IDLSequence.Object getDesiredRightFootsteps() + { + return desired_right_footsteps_; + } + + + /** + * Current left pose + */ + public us.ihmc.euclid.geometry.Pose3D getCurrentLeftFootPose() + { + return current_left_foot_pose_; + } + + + /** + * Current right pose + */ + public us.ihmc.euclid.geometry.Pose3D getCurrentRightFootPose() + { + return current_right_foot_pose_; + } + + + public static Supplier getPubSubType() + { + return KickDoorApproachPlanStateMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorApproachPlanStateMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorApproachPlanStateMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!this.kick_goal_pose_.epsilonEquals(other.kick_goal_pose_, epsilon)) return false; + if (!this.left_foot_goal_pose_.epsilonEquals(other.left_foot_goal_pose_, epsilon)) return false; + if (!this.right_foot_goal_pose_.epsilonEquals(other.right_foot_goal_pose_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_state_, other.execution_state_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.total_number_of_footsteps_, other.total_number_of_footsteps_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.number_of_incomplete_footsteps_, other.number_of_incomplete_footsteps_, epsilon)) return false; + + if (this.desired_left_footsteps_.size() != other.desired_left_footsteps_.size()) { return false; } + else + { + for (int i = 0; i < this.desired_left_footsteps_.size(); i++) + { if (!this.desired_left_footsteps_.get(i).epsilonEquals(other.desired_left_footsteps_.get(i), epsilon)) return false; } + } + + if (this.desired_right_footsteps_.size() != other.desired_right_footsteps_.size()) { return false; } + else + { + for (int i = 0; i < this.desired_right_footsteps_.size(); i++) + { if (!this.desired_right_footsteps_.get(i).epsilonEquals(other.desired_right_footsteps_.get(i), epsilon)) return false; } + } + + if (!this.current_left_foot_pose_.epsilonEquals(other.current_left_foot_pose_, epsilon)) return false; + if (!this.current_right_foot_pose_.epsilonEquals(other.current_right_foot_pose_, epsilon)) return false; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorApproachPlanStateMessage)) return false; + + KickDoorApproachPlanStateMessage otherMyClass = (KickDoorApproachPlanStateMessage) other; + + if (!this.state_.equals(otherMyClass.state_)) return false; + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if (!this.kick_goal_pose_.equals(otherMyClass.kick_goal_pose_)) return false; + if (!this.left_foot_goal_pose_.equals(otherMyClass.left_foot_goal_pose_)) return false; + if (!this.right_foot_goal_pose_.equals(otherMyClass.right_foot_goal_pose_)) return false; + if(this.execution_state_ != otherMyClass.execution_state_) return false; + + if(this.total_number_of_footsteps_ != otherMyClass.total_number_of_footsteps_) return false; + + if(this.number_of_incomplete_footsteps_ != otherMyClass.number_of_incomplete_footsteps_) return false; + + if (!this.desired_left_footsteps_.equals(otherMyClass.desired_left_footsteps_)) return false; + if (!this.desired_right_footsteps_.equals(otherMyClass.desired_right_footsteps_)) return false; + if (!this.current_left_foot_pose_.equals(otherMyClass.current_left_foot_pose_)) return false; + if (!this.current_right_foot_pose_.equals(otherMyClass.current_right_foot_pose_)) return false; + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorApproachPlanStateMessage {"); + builder.append("state="); + builder.append(this.state_); builder.append(", "); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("kick_goal_pose="); + builder.append(this.kick_goal_pose_); builder.append(", "); + builder.append("left_foot_goal_pose="); + builder.append(this.left_foot_goal_pose_); builder.append(", "); + builder.append("right_foot_goal_pose="); + builder.append(this.right_foot_goal_pose_); builder.append(", "); + builder.append("execution_state="); + builder.append(this.execution_state_); builder.append(", "); + builder.append("total_number_of_footsteps="); + builder.append(this.total_number_of_footsteps_); builder.append(", "); + builder.append("number_of_incomplete_footsteps="); + builder.append(this.number_of_incomplete_footsteps_); builder.append(", "); + builder.append("desired_left_footsteps="); + builder.append(this.desired_left_footsteps_); builder.append(", "); + builder.append("desired_right_footsteps="); + builder.append(this.desired_right_footsteps_); builder.append(", "); + builder.append("current_left_foot_pose="); + builder.append(this.current_left_foot_pose_); builder.append(", "); + builder.append("current_right_foot_pose="); + builder.append(this.current_right_foot_pose_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java new file mode 100644 index 000000000000..e76e734c46cf --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java @@ -0,0 +1,266 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorApproachPlanStateMessage" defined in "KickDoorApproachPlanStateMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorApproachPlanStateMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorApproachPlanStateMessage_.idl instead. +* +*/ +public class KickDoorApproachPlanStateMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorApproachPlanStateMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "468c99e85dede9b13315857e14567916ea49224d68b9f2da5142a0b3e32bbadc"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getCdrSerializedSize(data.getState(), current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getKickGoalPose(), current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getLeftFootGoalPose(), current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getRightFootGoalPose(), current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getDesiredLeftFootsteps().size(); ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getDesiredLeftFootsteps().get(i0), current_alignment);} + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getDesiredRightFootsteps().size(); ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getDesiredRightFootsteps().get(i0), current_alignment);} + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getCurrentLeftFootPose(), current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getCurrentRightFootPose(), current_alignment); + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getKickGoalPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getLeftFootGoalPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getRightFootGoalPose(), cdr); + cdr.write_type_9(data.getExecutionState()); + + cdr.write_type_3(data.getTotalNumberOfFootsteps()); + + cdr.write_type_3(data.getNumberOfIncompleteFootsteps()); + + if(data.getDesiredLeftFootsteps().size() <= 100) + cdr.write_type_e(data.getDesiredLeftFootsteps());else + throw new RuntimeException("desired_left_footsteps field exceeds the maximum length"); + + if(data.getDesiredRightFootsteps().size() <= 100) + cdr.write_type_e(data.getDesiredRightFootsteps());else + throw new RuntimeException("desired_right_footsteps field exceeds the maximum length"); + + geometry_msgs.msg.dds.PosePubSubType.write(data.getCurrentLeftFootPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getCurrentRightFootPose(), cdr); + } + + public static void read(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getKickGoalPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getLeftFootGoalPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getRightFootGoalPose(), cdr); + data.setExecutionState(cdr.read_type_9()); + + data.setTotalNumberOfFootsteps(cdr.read_type_3()); + + data.setNumberOfIncompleteFootsteps(cdr.read_type_3()); + + cdr.read_type_e(data.getDesiredLeftFootsteps()); + cdr.read_type_e(data.getDesiredRightFootsteps()); + geometry_msgs.msg.dds.PosePubSubType.read(data.getCurrentLeftFootPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getCurrentRightFootPose(), cdr); + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_a("kick_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getKickGoalPose()); + + ser.write_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); + + ser.write_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); + + ser.write_type_9("execution_state", data.getExecutionState()); + ser.write_type_3("total_number_of_footsteps", data.getTotalNumberOfFootsteps()); + ser.write_type_3("number_of_incomplete_footsteps", data.getNumberOfIncompleteFootsteps()); + ser.write_type_e("desired_left_footsteps", data.getDesiredLeftFootsteps()); + ser.write_type_e("desired_right_footsteps", data.getDesiredRightFootsteps()); + ser.write_type_a("current_left_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentLeftFootPose()); + + ser.write_type_a("current_right_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentRightFootPose()); + + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data) + { + ser.read_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); + + ser.read_type_a("kick_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getKickGoalPose()); + + ser.read_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); + + ser.read_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); + + data.setExecutionState(ser.read_type_9("execution_state")); + data.setTotalNumberOfFootsteps(ser.read_type_3("total_number_of_footsteps")); + data.setNumberOfIncompleteFootsteps(ser.read_type_3("number_of_incomplete_footsteps")); + ser.read_type_e("desired_left_footsteps", data.getDesiredLeftFootsteps()); + ser.read_type_e("desired_right_footsteps", data.getDesiredRightFootsteps()); + ser.read_type_a("current_left_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentLeftFootPose()); + + ser.read_type_a("current_right_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentRightFootPose()); + + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorApproachPlanStateMessagePubSubType newInstance() + { + return new KickDoorApproachPlanStateMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessage.java new file mode 100644 index 000000000000..938b42ded91e --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessage.java @@ -0,0 +1,265 @@ +package controller_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * This message is part of the IHMC whole-body controller API. + * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + */ +public class KickDoorMessage extends Packet implements Settable, EpsilonComparable +{ + public static final byte ROBOT_SIDE_LEFT = (byte) 0; + public static final byte ROBOT_SIDE_RIGHT = (byte) 1; + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long sequence_id_; + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte robot_side_; + /** + * The height at which the kick should be targeted. + */ + public double kick_height_; + /** + * The impulse with which the kick should be executed. + */ + public double kick_impulse_; + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double kick_target_distance_; + /** + * A boolean for tracking whether a kick has been requested. + */ + public boolean trigger_kick_request_; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double prekick_weight_distribution_; + + public KickDoorMessage() + { + } + + public KickDoorMessage(KickDoorMessage other) + { + this(); + set(other); + } + + public void set(KickDoorMessage other) + { + sequence_id_ = other.sequence_id_; + + robot_side_ = other.robot_side_; + + kick_height_ = other.kick_height_; + + kick_impulse_ = other.kick_impulse_; + + kick_target_distance_ = other.kick_target_distance_; + + trigger_kick_request_ = other.trigger_kick_request_; + + prekick_weight_distribution_ = other.prekick_weight_distribution_; + + } + + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public void setSequenceId(long sequence_id) + { + sequence_id_ = sequence_id; + } + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long getSequenceId() + { + return sequence_id_; + } + + /** + * Specifies the side of the robot that will execute the kick. + */ + public void setRobotSide(byte robot_side) + { + robot_side_ = robot_side; + } + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte getRobotSide() + { + return robot_side_; + } + + /** + * The height at which the kick should be targeted. + */ + public void setKickHeight(double kick_height) + { + kick_height_ = kick_height; + } + /** + * The height at which the kick should be targeted. + */ + public double getKickHeight() + { + return kick_height_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickImpulse(double kick_impulse) + { + kick_impulse_ = kick_impulse; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickImpulse() + { + return kick_impulse_; + } + + /** + * The target distance from the robot to where the kick should be aimed. + */ + public void setKickTargetDistance(double kick_target_distance) + { + kick_target_distance_ = kick_target_distance; + } + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double getKickTargetDistance() + { + return kick_target_distance_; + } + + /** + * A boolean for tracking whether a kick has been requested. + */ + public void setTriggerKickRequest(boolean trigger_kick_request) + { + trigger_kick_request_ = trigger_kick_request; + } + /** + * A boolean for tracking whether a kick has been requested. + */ + public boolean getTriggerKickRequest() + { + return trigger_kick_request_; + } + + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public void setPrekickWeightDistribution(double prekick_weight_distribution) + { + prekick_weight_distribution_ = prekick_weight_distribution; + } + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double getPrekickWeightDistribution() + { + return prekick_weight_distribution_; + } + + + public static Supplier getPubSubType() + { + return KickDoorMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorMessage)) return false; + + KickDoorMessage otherMyClass = (KickDoorMessage) other; + + if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + + if(this.robot_side_ != otherMyClass.robot_side_) return false; + + if(this.kick_height_ != otherMyClass.kick_height_) return false; + + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + + if(this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) return false; + + if(this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorMessage {"); + builder.append("sequence_id="); + builder.append(this.sequence_id_); builder.append(", "); + builder.append("robot_side="); + builder.append(this.robot_side_); builder.append(", "); + builder.append("kick_height="); + builder.append(this.kick_height_); builder.append(", "); + builder.append("kick_impulse="); + builder.append(this.kick_impulse_); builder.append(", "); + builder.append("kick_target_distance="); + builder.append(this.kick_target_distance_); builder.append(", "); + builder.append("trigger_kick_request="); + builder.append(this.trigger_kick_request_); builder.append(", "); + builder.append("prekick_weight_distribution="); + builder.append(this.prekick_weight_distribution_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessagePubSubType.java new file mode 100644 index 000000000000..6b3988f57101 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessagePubSubType.java @@ -0,0 +1,210 @@ +package controller_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorMessage" defined in "KickDoorMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorMessage_.idl instead. +* +*/ +public class KickDoorMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "controller_msgs::msg::dds_::KickDoorMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "b1357ce9e113f26ceb2843a82d4f242f1fc1bb1d286d3b3469528444c5669609"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.KickDoorMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.KickDoorMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.KickDoorMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + + return current_alignment - initial_alignment; + } + + public static void write(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_4(data.getSequenceId()); + + cdr.write_type_9(data.getRobotSide()); + + cdr.write_type_6(data.getKickHeight()); + + cdr.write_type_6(data.getKickImpulse()); + + cdr.write_type_6(data.getKickTargetDistance()); + + cdr.write_type_7(data.getTriggerKickRequest()); + + cdr.write_type_6(data.getPrekickWeightDistribution()); + + } + + public static void read(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) + { + data.setSequenceId(cdr.read_type_4()); + + data.setRobotSide(cdr.read_type_9()); + + data.setKickHeight(cdr.read_type_6()); + + data.setKickImpulse(cdr.read_type_6()); + + data.setKickTargetDistance(cdr.read_type_6()); + + data.setTriggerKickRequest(cdr.read_type_7()); + + data.setPrekickWeightDistribution(cdr.read_type_6()); + + + } + + @Override + public final void serialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_4("sequence_id", data.getSequenceId()); + ser.write_type_9("robot_side", data.getRobotSide()); + ser.write_type_6("kick_height", data.getKickHeight()); + ser.write_type_6("kick_impulse", data.getKickImpulse()); + ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); + ser.write_type_7("trigger_kick_request", data.getTriggerKickRequest()); + ser.write_type_6("prekick_weight_distribution", data.getPrekickWeightDistribution()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.KickDoorMessage data) + { + data.setSequenceId(ser.read_type_4("sequence_id")); + data.setRobotSide(ser.read_type_9("robot_side")); + data.setKickHeight(ser.read_type_6("kick_height")); + data.setKickImpulse(ser.read_type_6("kick_impulse")); + data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); + data.setTriggerKickRequest(ser.read_type_7("trigger_kick_request")); + data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); + } + + public static void staticCopy(controller_msgs.msg.dds.KickDoorMessage src, controller_msgs.msg.dds.KickDoorMessage dest) + { + dest.set(src); + } + + @Override + public controller_msgs.msg.dds.KickDoorMessage createData() + { + return new controller_msgs.msg.dds.KickDoorMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(controller_msgs.msg.dds.KickDoorMessage src, controller_msgs.msg.dds.KickDoorMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorMessagePubSubType newInstance() + { + return new KickDoorMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg index 58a51ce676c0..d0c2233e5540 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg @@ -19,6 +19,8 @@ byte SCREW_PRIMITIVE_ACTION = 15 byte PELVIS_HEIGHT_ORIENTATION_ACTION = 16 byte WAIT_DURATION_ACTION = 17 byte FOOT_POSE_ACTION = 18 +byte KICK_DOOR_ACTION = 19 +byte KICK_DOOR_APPROACH_ACTION = 20 # Monotonically increasing message ID that matches the CRDTInfo update number uint32 sequence_id @@ -66,3 +68,7 @@ behavior_msgs/PelvisHeightOrientationActionStateMessage[<=200] pelvis_height_act behavior_msgs/WaitDurationActionStateMessage[<=200] wait_duration_actions behavior_msgs/FootPoseActionStateMessage[<=200] foot_pose_actions + +behavior_msgs/KickDoorActionStateMessage[<=200] kick_door_actions + +behavior_msgs/KickDoorApproachPlanStateMessage[<=200] kick_door_approach_actions diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg new file mode 100644 index 000000000000..f173d0ae0e1a --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg @@ -0,0 +1,23 @@ +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Name of the parent frame the footsteps are expressed in +string parent_frame_name + +# Specifies the side of the robot that will execute the kick. +byte robot_side 255 + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# The stance foot width. +float64 stance_foot_width + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg new file mode 100644 index 000000000000..e216f1b810d3 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg @@ -0,0 +1,8 @@ +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorActionDefinitionMessage definition + +# Execution state +byte execution_state diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg new file mode 100644 index 000000000000..bdf67a6f8749 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg @@ -0,0 +1,35 @@ +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Name of the parent frame the footsteps are expressed in +string parent_frame_name + +# Swing duration +float64 swing_duration + +# Transfer duration +float64 transfer_duration + +# OVERRIDE (0) or QUEUE (1) +int32 execution_mode + +# Specifies the side of the robot that will execute the kick. +byte robot_side 255 + +# The impulse with which the kick should be executed. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# The distance towards the inside of the door from where the kick foot should be aligned. +float64 horizontal_distance_from_handle + +# The stance foot width. +float64 stance_foot_width + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg new file mode 100644 index 000000000000..5b0a28fa0039 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -0,0 +1,40 @@ +byte FOOTSTEP_PLANNING = 0 +byte PLANNING_FAILED = 1 +byte PLANNING_SUCCEEDED = 2 +byte PLAN_COMMANDED = 3 + +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorApproachPlanDefinitionMessage definition + +# Kick goal pose +geometry_msgs/Pose kick_goal_pose + +# Left foot goal pose +geometry_msgs/Pose left_foot_goal_pose + +# Right foot goal pose +geometry_msgs/Pose right_foot_goal_pose + +# Execution state +int8 execution_state + +# Total number of footsteps; used for walking actions +uint16 total_number_of_footsteps + +# Incomplete footsteps; used for walking actions +uint16 number_of_incomplete_footsteps + +# Desired left footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_left_footsteps + +# Desired right footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_right_footsteps + +# Current left pose +geometry_msgs/Pose current_left_foot_pose + +# Current right pose +geometry_msgs/Pose current_right_foot_pose diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/KickDoorMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/KickDoorMessage.msg new file mode 100644 index 000000000000..6fcb1e5434c3 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/KickDoorMessage.msg @@ -0,0 +1,26 @@ +# This message is part of the IHMC whole-body controller API. +# This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + +byte ROBOT_SIDE_LEFT = 0 +byte ROBOT_SIDE_RIGHT = 1 + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + +# Specifies the side of the robot that will execute the kick. +byte robot_side + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# A boolean for tracking whether a kick has been requested. +bool trigger_kick_request + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg index c3d2734b6d1c..ca3f4efb3779 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg @@ -33,6 +33,10 @@ int8 WAIT_DURATION_ACTION=17 int8 FOOT_POSE_ACTION=18 +int8 KICK_DOOR_ACTION=19 + +int8 KICK_DOOR_APPROACH_ACTION=20 + # Monotonically increasing message ID that matches the CRDTInfo update number uint32 sequence_id @@ -80,4 +84,8 @@ behavior_msgs/WaitDurationActionStateMessage[] wait_duration_actions behavior_msgs/FootPoseActionStateMessage[] foot_pose_actions +behavior_msgs/KickDoorActionStateMessage[] kick_door_actions + +behavior_msgs/KickDoorApproachPlanStateMessage[] kick_door_approach_actions + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg new file mode 100644 index 000000000000..3d4c132eddbe --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg @@ -0,0 +1,27 @@ + +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Name of the parent frame the footsteps are expressed in +string parent_frame_name + +# Specifies the side of the robot that will execute the kick. +# Field default value 255 +int8 robot_side + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# The stance foot width. +float64 stance_foot_width + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution + + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg new file mode 100644 index 000000000000..3daba46305d8 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg @@ -0,0 +1,11 @@ + +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorActionDefinitionMessage definition + +# Execution state +int8 execution_state + + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg new file mode 100644 index 000000000000..4313f76311a8 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg @@ -0,0 +1,39 @@ + +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Name of the parent frame the footsteps are expressed in +string parent_frame_name + +# Swing duration +float64 swing_duration + +# Transfer duration +float64 transfer_duration + +# OVERRIDE (0) or QUEUE (1) +int32 execution_mode + +# Specifies the side of the robot that will execute the kick. +# Field default value 255 +int8 robot_side + +# The impulse with which the kick should be executed. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# The distance towards the inside of the door from where the kick foot should be aligned. +float64 horizontal_distance_from_handle + +# The stance foot width. +float64 stance_foot_width + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution + + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg new file mode 100644 index 000000000000..98ad8be5a893 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -0,0 +1,46 @@ + +int8 FOOTSTEP_PLANNING=0 + +int8 PLANNING_FAILED=1 + +int8 PLANNING_SUCCEEDED=2 + +int8 PLAN_COMMANDED=3 + +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorApproachPlanDefinitionMessage definition + +# Kick goal pose +geometry_msgs/Pose kick_goal_pose + +# Left foot goal pose +geometry_msgs/Pose left_foot_goal_pose + +# Right foot goal pose +geometry_msgs/Pose right_foot_goal_pose + +# Execution state +int8 execution_state + +# Total number of footsteps; used for walking actions +uint16 total_number_of_footsteps + +# Incomplete footsteps; used for walking actions +uint16 number_of_incomplete_footsteps + +# Desired left footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_left_footsteps + +# Desired right footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_right_footsteps + +# Current left pose +geometry_msgs/Pose current_left_foot_pose + +# Current right pose +geometry_msgs/Pose current_right_foot_pose + + diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/KickDoorMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/KickDoorMessage.msg new file mode 100644 index 000000000000..39c71594e1c3 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/KickDoorMessage.msg @@ -0,0 +1,29 @@ +# This message is part of the IHMC whole-body controller API. +# This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + +int8 ROBOT_SIDE_LEFT=0 + +int8 ROBOT_SIDE_RIGHT=1 + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + +# Specifies the side of the robot that will execute the kick. +int8 robot_side + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# A boolean for tracking whether a kick has been requested. +bool trigger_kick_request + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution + +