diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java index ec32f05f2bc..3b8828c522d 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java @@ -4,6 +4,7 @@ import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; +import std_msgs.msg.dds.Float32; import std_msgs.msg.dds.Empty; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; import us.ihmc.ros2.ROS2Topic; @@ -20,6 +21,7 @@ public class ContinuousHikingAPI public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); public static final ROS2Topic ROTATE_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("rotate_goal_footsteps"); + public static final ROS2Topic ROTATE_90_DEGREES = IHMC_ROOT.withModule(moduleName).withType(Float32.class).withSuffix("rotate_90_degrees"); public static final ROS2Topic SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("rotate_goal_footsteps"); // Statuses supported from the Continuous Hiking Process diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index b338a4b2fc5..fe0d6246585 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -7,6 +7,7 @@ import com.badlogic.gdx.graphics.g3d.RenderableProvider; import com.badlogic.gdx.utils.Array; import com.badlogic.gdx.utils.Pool; +import com.studiohartman.jamepad.ControllerButton; import controller_msgs.msg.dds.FootstepDataListMessage; import controller_msgs.msg.dds.FootstepStatusMessage; import controller_msgs.msg.dds.PlanOffsetStatus; @@ -15,6 +16,7 @@ import imgui.ImGui; import imgui.type.ImBoolean; import std_msgs.msg.dds.Empty; +import std_msgs.msg.dds.Float32; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; @@ -42,7 +44,6 @@ import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; import us.ihmc.footstepPlanning.tools.SwingPlannerTools; import us.ihmc.log.LogTools; -import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.comms.PerceptionComms; import us.ihmc.perception.heightMap.TerrainMapData; @@ -107,7 +108,10 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private boolean publishAndSubscribe; private double simulatedDriftInMeters = -0.1; - private final ROS2Publisher turningPublisher; + private final ROS2Publisher turn90DegreesPublisher; + private boolean previousRightBumper; + private boolean previousLeftBumper; + private boolean previousYButton; public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) { @@ -122,6 +126,14 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod planOffsetStatusPublisher = ros2Node.createPublisher(getTopic(PlanOffsetStatus.class, robotModel.getSimpleRobotName())); clearGoalFootstepsPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS); + MonteCarloFootstepPlannerParameters monteCarloPlannerParameters = new MonteCarloFootstepPlannerParameters(); + terrainPlanningDebugger = new RDXTerrainPlanningDebugger(ros2Node, + monteCarloPlannerParameters, + robotModel.getContactPointParameters().getControllerFootGroundContactPoints()); + + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), + (s) -> terrainPlanningDebugger.reset()); + ros2Node.createSubscription2(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); ros2Node.createSubscription2(ContinuousHikingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); @@ -139,24 +151,16 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod defaultContactPoints.put(robotSide, defaultFoothold); } - turningPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS); + turn90DegreesPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_90_DEGREES); StancePoseCalculator stancePoseCalculator = new StancePoseCalculator(defaultContactPoints); stancePoseSelectionPanel = new RDXStancePoseSelectionPanel(baseUI, ros2Node, stancePoseCalculator); addChild(stancePoseSelectionPanel); - MonteCarloFootstepPlannerParameters monteCarloPlannerParameters = new MonteCarloFootstepPlannerParameters(); DefaultFootstepPlannerParametersBasics footstepPlannerParameters = robotModel.getFootstepPlannerParameters("ForContinuousWalking"); SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters(); this.swingTrajectoryParameters = robotModel.getWalkingControllerParameters().getSwingTrajectoryParameters(); - terrainPlanningDebugger = new RDXTerrainPlanningDebugger(ros2Node, - monteCarloPlannerParameters, - robotModel.getContactPointParameters().getControllerFootGroundContactPoints()); - - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), - (s) -> terrainPlanningDebugger.reset()); - hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Node); continuousHikingParameters = new ContinuousHikingParameters(); createParametersPanel(continuousHikingParameters, @@ -292,13 +296,13 @@ public void renderImGuiWidgets() if (ImGui.button("Turn Left 90°")) { - turnRobot(Math.PI / 2.0); + turnRobot((float) (Math.PI / 2.0)); } ImGui.sameLine(); if (ImGui.button("Turn Right 90°")) { - turnRobot(-Math.PI / 2.0); + turnRobot((float) (-Math.PI / 2.0)); } if (ImGui.collapsingHeader("Continuous Hiking Parameters")) @@ -363,17 +367,13 @@ public void renderImGuiWidgets() { publishContinuousHikingCommandWithEnabled(); } + else if ((ImGui.isKeyDown(ImGuiTools.getLeftArrowKey()) || ImGui.isKeyDown(ImGuiTools.getRightArrowKey())) && ImGui.getIO().getKeyShift()) + { + publishContinuousHikingCommandSideStepEnabled(ImGui.isKeyDown(ImGuiTools.getLeftArrowKey())); + } else if (controllerConnected) { - if (joystickController.getButton(joystickController.getMapping().buttonA)) - { - publishJoystickStatus(joystickController); - } - - if (joystickController.getButton(joystickController.getMapping().buttonX)) - { - publishStopContinuousHiking(); - } + performJoystickControllerAction(joystickController); } // Pressing this key will stop Continuous Hiking @@ -388,6 +388,46 @@ else if (ImGui.isKeyPressed(ImGuiTools.getEscapeKey())) } } + private void performJoystickControllerAction(Controller joystickController) + { + boolean currentYButtonPressed = joystickController.getButton(ControllerButton.Y.ordinal()); + boolean currentLeftBumper = joystickController.getButton(ControllerButton.LEFTBUMPER.ordinal()); + boolean currentRightBumper = joystickController.getButton(ControllerButton.RIGHTBUMPER.ordinal()); + + if (previousYButton && !currentYButtonPressed) + { + squareUpPublisher.publish(new Empty()); + } + + if (previousLeftBumper && !currentLeftBumper) + { + turnRobot((float) (Math.PI / 2.0)); + } + + if (previousRightBumper && !currentRightBumper) + { + turnRobot((float) (-Math.PI / 2.0)); + } + + if (joystickController.getButton(joystickController.getMapping().buttonA)) + { + publishJoystickStatus(joystickController); + } + + if (joystickController.getButton(joystickController.getMapping().buttonX) && joystickController.getButton(ControllerButton.DPAD_DOWN.ordinal())) + { + publishStopContinuousHikingGracefully(); + } + else if (joystickController.getButton(joystickController.getMapping().buttonX)) + { + publishStopContinuousHiking(); + } + + previousYButton = joystickController.getButton(ControllerButton.Y.ordinal()); + previousLeftBumper = joystickController.getButton(ControllerButton.LEFTBUMPER.ordinal()); + previousRightBumper = joystickController.getButton(ControllerButton.RIGHTBUMPER.ordinal()); + } + public void processImGui3DViewInput(ImGui3DViewInput input) { stancePoseSelectionPanel.processImGui3DViewInput(input); @@ -405,30 +445,11 @@ public void clearPlannedFootsteps() clearGoalFootstepsPublisher.publish(new Empty()); } - public void turnRobot(double rotationRadians) + public void turnRobot(float rotationRadians) { - MovingReferenceFrame midFeetZUpFrame = syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame(); - FramePose3D midFeetZUpPose = new FramePose3D(midFeetZUpFrame, midFeetZUpFrame.getTransformToWorldFrame()); - SideDependentList goalPoses = new SideDependentList<>(new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()), - new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame())); - - midFeetZUpPose.appendYawRotation(rotationRadians); - goalPoses.get(RobotSide.RIGHT).set(midFeetZUpPose); - goalPoses.get(RobotSide.RIGHT).changeFrame(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()); - goalPoses.get(RobotSide.RIGHT).appendTranslation(0, -0.15, 0); - - goalPoses.get(RobotSide.LEFT).set(midFeetZUpPose); - goalPoses.get(RobotSide.LEFT).changeFrame(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()); - goalPoses.get(RobotSide.LEFT).appendTranslation(0, 0.15, 0); - - List poses = new ArrayList<>(); - poses.add(new Pose3D(goalPoses.get(RobotSide.LEFT))); - poses.add(new Pose3D(goalPoses.get(RobotSide.RIGHT))); - - PoseListMessage poseListMessage = new PoseListMessage(); - MessageTools.packPoseListMessage(poses, poseListMessage); - - turningPublisher.publish(poseListMessage); + std_msgs.msg.dds.Float32 rotationInRadians = new Float32(); + rotationInRadians.setData(rotationRadians); + turn90DegreesPublisher.publish(rotationInRadians); } /** @@ -529,6 +550,30 @@ private void publishContinuousHikingCommandWithEnabled() commandMessage.setEnableContinuousHiking(true); commandMessage.setStepsBeforeSafetyStop((int) stepsBeforeSafetyStop.getDoubleValue()); commandMessage.setWalkForwards(true); + commandMessage.setSideStep(false); + commandMessage.setLeftDirection(false); + commandMessage.setSquareUpToGoal(squareUpToGoal.get()); + commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); + commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); + commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get()); + commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get()); + + commandMessage.setUseJoystickController(false); + commandMessage.setForwardValue(0.0); + commandMessage.setWalkBackwards(false); + commandMessage.setLateralValue(0.0); + commandMessage.setTurningValue(0.0); + + commandPublisher.publish(commandMessage); + } + + private void publishContinuousHikingCommandSideStepEnabled(boolean leftDirection) + { + commandMessage.setEnableContinuousHiking(true); + commandMessage.setStepsBeforeSafetyStop((int) stepsBeforeSafetyStop.getDoubleValue()); + commandMessage.setWalkForwards(true); + commandMessage.setSideStep(true); + commandMessage.setLeftDirection(leftDirection); commandMessage.setSquareUpToGoal(squareUpToGoal.get()); commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java index c057eee1fe6..3e23d9a188e 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java @@ -24,6 +24,7 @@ public class ContinuousHikingParameters extends StoredPropertySet implements Con public static final DoubleStoredPropertyKey goalPoseForwardDistance = keys.addDoubleKey("Goal pose forward distance"); public static final DoubleStoredPropertyKey goalPoseUpDistance = keys.addDoubleKey("Goal pose up distance"); public static final DoubleStoredPropertyKey goalPoseBackwardDistance = keys.addDoubleKey("Goal pose backward distance"); + public static final DoubleStoredPropertyKey goalPoseSidewaysDistance = keys.addDoubleKey("Goal pose sideways distance"); public static final DoubleStoredPropertyKey swingTime = keys.addDoubleKey("Swing time"); public static final DoubleStoredPropertyKey transferTime = keys.addDoubleKey("Transfer time"); public static final DoubleStoredPropertyKey planningTimeoutAsAFractionOfTheStepDuration = keys.addDoubleKey("Planning timeout as a fraction of the step duration"); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java index 4860c31305d..d4293a8bac2 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java @@ -38,6 +38,11 @@ default void setGoalPoseBackwardDistance(double goalPoseBackwardDistance) set(ContinuousHikingParameters.goalPoseBackwardDistance, goalPoseBackwardDistance); } + default void setGoalPoseSidewaysDistance(double goalPoseSidewaysDistance) + { + set(ContinuousHikingParameters.goalPoseSidewaysDistance, goalPoseSidewaysDistance); + } + default void setSwingTime(double swingTime) { set(ContinuousHikingParameters.swingTime, swingTime); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java index ecf1c13533c..49a35abf389 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java @@ -40,6 +40,11 @@ default double getGoalPoseBackwardDistance() return get(goalPoseBackwardDistance); } + default double getGoalPoseSidewaysDistance() + { + return get(goalPoseSidewaysDistance); + } + default double getSwingTime() { return get(swingTime); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java index fed6d7dfa77..15def6fd571 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java @@ -5,7 +5,9 @@ import controller_msgs.msg.dds.QueuedFootstepStatusMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; import ihmc_common_msgs.msg.dds.QueueableMessage; +import org.apache.regexp.RE; import org.jetbrains.annotations.NotNull; +import std_msgs.msg.dds.Float32; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.networkProcessor.footstepPlanningModule.FootstepPlanningModuleLauncher; @@ -36,10 +38,12 @@ import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.robotics.stateMachine.core.State; import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.heightMap.HeightMapData; +import java.util.ArrayList; import java.util.List; import java.util.concurrent.atomic.AtomicReference; import java.util.function.Supplier; @@ -95,6 +99,7 @@ public JustWaitState(DRCRobotModel robotModel, controllerFootstepDataTopic = HumanoidControllerAPI.getTopic(FootstepDataListMessage.class, "Nadia"); ros2Helper.createPublisher(controllerFootstepDataTopic); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.ROTATE_90_DEGREES, this::rotate90Degrees); ros2Helper.subscribeViaCallback(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS, this::planToGoal); ros2Helper.subscribeViaCallback(ContinuousHikingAPI.SQUARE_UP_STEP, this::squareUpStep); } @@ -133,6 +138,56 @@ public boolean isDone(double timeInState) return isDone; } + public void rotate90Degrees(Float32 float32Message) + { + float rotationRadians = float32Message.getData(); + + MovingReferenceFrame midFeetZUpFrame = syncedRobot.getReferenceFrames().getMidFeetZUpFrame(); + FramePose3D midFeetZUpPose = new FramePose3D(midFeetZUpFrame, midFeetZUpFrame.getTransformToWorldFrame()); + SideDependentList goalPoses = new SideDependentList<>(new FramePose3D(syncedRobot.getReferenceFrames().getMidFeetZUpFrame()), + new FramePose3D(syncedRobot.getReferenceFrames().getMidFeetZUpFrame())); + + if (!controllerQueueMonitor.getControllerFootstepQueue().isEmpty()) + { + FramePose3DReadOnly lastFootstepInQueue = controllerQueueMonitor.getLastFootstepInQueue(); + RobotSide lastFootstepSide = RobotSide.fromByte(controllerQueueMonitor.getControllerFootstepQueue() + .get(controllerQueueMonitor.getControllerFootstepQueue().size() - 1) + .getRobotSide()); + + FramePose3D tempMidFeetPose = new FramePose3D(); + tempMidFeetPose.set(lastFootstepInQueue); + + if (lastFootstepSide == RobotSide.LEFT) + { + tempMidFeetPose.appendTranslation(0, -0.12, 0); + } + else + { + tempMidFeetPose.appendTranslation(0, 0.12, 0); + } + + midFeetZUpPose.set(midFeetZUpFrame, tempMidFeetPose); + } + + midFeetZUpPose.appendYawRotation(rotationRadians); + goalPoses.get(RobotSide.RIGHT).set(midFeetZUpPose); + goalPoses.get(RobotSide.RIGHT).changeFrame(syncedRobot.getReferenceFrames().getMidFeetZUpFrame()); + goalPoses.get(RobotSide.RIGHT).appendTranslation(0, -0.15, 0); + + goalPoses.get(RobotSide.LEFT).set(midFeetZUpPose); + goalPoses.get(RobotSide.LEFT).changeFrame(syncedRobot.getReferenceFrames().getMidFeetZUpFrame()); + goalPoses.get(RobotSide.LEFT).appendTranslation(0, 0.15, 0); + + List poses = new ArrayList<>(); + poses.add(new Pose3D(goalPoses.get(RobotSide.LEFT))); + poses.add(new Pose3D(goalPoses.get(RobotSide.RIGHT))); + + PoseListMessage poseListMessage = new PoseListMessage(); + MessageTools.packPoseListMessage(poses, poseListMessage); + + planToGoal(poseListMessage); + } + public void planToGoal(PoseListMessage poseListMessage) { ThreadTools.startAThread(() -> @@ -191,12 +246,10 @@ public void planToGoal(PoseListMessage poseListMessage) FootstepPlannerOutput plannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); - FootstepPlan newestFootstepPlan = null; + if (plannerOutput == null) + return; - if (plannerOutput != null) - { - newestFootstepPlan = plannerOutput.getFootstepPlan(); - } + FootstepPlan newestFootstepPlan = plannerOutput.getFootstepPlan(); FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.setDefaultSwingDuration(0.8); @@ -205,7 +258,6 @@ public void planToGoal(PoseListMessage poseListMessage) for (int i = 0; i < footstepPlanner.getOutput().getFootstepPlan().getNumberOfSteps(); i++) { - assert newestFootstepPlan != null; PlannedFootstep footstep = newestFootstepPlan.getFootstep(i); footstep.limitFootholdVertices(); footstepDataListMessage.getFootstepDataList().add().set(footstep.getAsMessage()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java index 088fa375d02..8a529a692ef 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java @@ -225,12 +225,28 @@ else if (Math.abs(commandMessage.get().getLateralValue()) < 0.1) } else { - goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); + if (commandMessage.get().getSideStep()) + { + double sidewaysDistance = commandMessage.get().getLeftDirection() ? + continuousHikingParameters.getGoalPoseSidewaysDistance() : + -continuousHikingParameters.getGoalPoseSidewaysDistance(); + + goalPoses = ContinuousPlannerTools.setSidStepGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) sidewaysDistance, + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); + } + else + { + goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); + } } return goalPoses; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java index 7983ab5553f..cd5d4057400 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java @@ -168,20 +168,24 @@ public FootstepPlan generateAStarFootstepPlan(HeightMapData heightMapData, request.setSnapGoalSteps(true); request.setAbortIfGoalStepSnappingFails(true); - // When walking backwards, we want to keep the body facing in the same direction, otherwise the robot will turn as it tries to step backwards - if (commandMessage.get().getWalkBackwards()) + // When walking backwards or sideways, we want to keep the body facing in the same direction, otherwise the robot will turn as it tries to step + if (commandMessage.get().getWalkBackwards() || commandMessage.get().getSideStep()) { FramePose3D bodyMidGoalPose = new FramePose3D(); bodyMidGoalPose.interpolate(goalPoses.get(RobotSide.LEFT), goalPoses.get(RobotSide.RIGHT), 0.5); request.getBodyPathWaypoints().add(walkingStartMidPose); request.getBodyPathWaypoints().add(bodyMidGoalPose); - // To allow walking backwards, we need to change the minimum step length, but this affects other walking, so we only do it when going backwards + // To allow walking backwards or sideways, we need to change the max/min step length, but this affects other walking, so we only do it when going backwards footstepPlannerParameters.setMinStepLength(-0.3); + footstepPlannerParameters.setMaxStepReach(0.2); + footstepPlannerParameters.setMaxStepWidth(0.6); } else { footstepPlannerParameters.setMinStepLength(0.1); + footstepPlannerParameters.setMaxStepReach(0.5); + footstepPlannerParameters.setMaxStepWidth(0.45); } if (useMonteCarloPlanAsReference && monteCarloFootstepPlan.get() != null && monteCarloFootstepPlan.get().getNumberOfSteps() > 0) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerTools.java index adb216926fb..2b171f75d6d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerTools.java @@ -160,6 +160,50 @@ public static SideDependentList setStraightBackwardGoalPoses(FrameP return goalPose; } + public static SideDependentList setSidStepGoalPoses(FramePose3D walkingStartPose, + SideDependentList stancePose, + float yDistance, + float zDistance, + float xRandomMargin, + float nominalStanceWidth) + { + float offsetX = (float) (Math.random() * xRandomMargin - xRandomMargin / 2.0f); + + FramePose3D stanceMidPose = new FramePose3D(); + stanceMidPose.interpolate(stancePose.get(RobotSide.LEFT), stancePose.get(RobotSide.RIGHT), 0.5); + + SideDependentList goalPose = new SideDependentList<>(); + for (RobotSide side : RobotSide.values) + { + goalPose.put(side, new FramePose3D()); + RigidBodyTransform stanceToWalkingFrameTransform = new RigidBodyTransform(); + RigidBodyTransform worldToWalkingFrameTransform = new RigidBodyTransform(); + + stanceToWalkingFrameTransform.set(stanceMidPose); + worldToWalkingFrameTransform.set(walkingStartPose); + worldToWalkingFrameTransform.invert(); + stanceToWalkingFrameTransform.multiply(worldToWalkingFrameTransform); + + double yWalkDistance = stanceToWalkingFrameTransform.getTranslation().norm(); + goalPose.get(side).getPosition().set(walkingStartPose.getPosition()); + goalPose.get(side).getOrientation().set(walkingStartPose.getOrientation()); + if (yDistance > 0) + { + goalPose.get(side).appendTranslation(0, offsetX + yWalkDistance + yDistance, stanceMidPose.getZ() + zDistance - walkingStartPose.getZ()); + } + else + { + goalPose.get(side).appendTranslation(0, -offsetX - yWalkDistance + yDistance, stanceMidPose.getZ() + zDistance - walkingStartPose.getZ()); + } + } + + // These are done after the for loop because of the ( - ) or ( + ) for the nominal stance + goalPose.get(RobotSide.LEFT).appendTranslation(0.0, nominalStanceWidth / 2.0f, 0.0); + goalPose.get(RobotSide.RIGHT).appendTranslation(0.0, -nominalStanceWidth / 2.0f, 0.0); + + return goalPose; + } + public static void generateSensorZUpToStraightGoalFootPoses(HeightMapData latestHeightMapData, RigidBodyTransform sensorZUpToWorldTransform, SideDependentList startPoseToPack, diff --git a/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json b/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json index 6d14c437f99..088ea743ba8 100644 --- a/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json +++ b/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json @@ -6,6 +6,7 @@ "Goal pose forward distance" : 1.4, "Goal pose up distance" : 0.5, "Goal pose backward distance" : 0.8, + "Goal pose sideways distance" : 1.2, "Swing time" : 0.95, "Transfer time" : 0.6, "Planning timeout as a fraction of the step duration" : 0.3, diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl index 6b203e70c63..372d6bcbe33 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl @@ -27,6 +27,14 @@ module behavior_msgs * Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick */ boolean walk_forwards; + /** + * Flag to sidestep with Continuous Hiking. + */ + boolean side_step; + /** + * Flag for which direction to side step with Continuous Hiking. + */ + boolean left_direction; /** * Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. * Setting this to true tells the robot to step on the final goal positions. diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java index 365d48c6a11..62dd6df639c 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java @@ -24,6 +24,14 @@ public class ContinuousHikingCommandMessage extends Packet= width || y >= height) return; + + // Access current height value using pitched memory + unsigned short *row = (unsigned short *)((char *)matrixPointer + y * pitchMatrix); + unsigned short currentHeight = row[x]; + + // Array to store valid neighbor values + unsigned short neighbors[8]; + int neighborCount = 0; + + // Define neighbor offsets (8-connected) + int dx[8] = { 0, 0, -1, 1, -1, 1, -1, 1 }; + int dy[8] = {-1, 1, 0, 0, -1, -1, 1, 1 }; + + // Find min and max among valid neighbors + unsigned short minNeighbor = currentHeight; + unsigned short maxNeighbor = currentHeight; + + for (int i = 0; i < 8; i++) { + int nx = x + dx[i]; + int ny = y + dy[i]; + + // Check bounds + if (nx >= 0 && nx < width && ny >= 0 && ny < height) { + unsigned short *neighborRow = (unsigned short *)((char *)matrixPointer + ny * pitchMatrix); + unsigned short neighborHeight = neighborRow[nx]; + + neighbors[neighborCount++] = neighborHeight; + if (neighborHeight < minNeighbor) minNeighbor = neighborHeight; + if (neighborHeight > maxNeighbor) maxNeighbor = neighborHeight; + } + } + + // Only adjust if the height jump is significant (above threshold) + if ((currentHeight > minNeighbor + threshold) && (currentHeight < maxNeighbor - threshold)) { + // Adjust to the closest level + if (abs(currentHeight - minNeighbor) < abs(currentHeight - maxNeighbor)) { + currentHeight = minNeighbor; + } else { + currentHeight = maxNeighbor; + } + } + + // Store result in pitched output matrix + unsigned short *resultRow = (unsigned short *)((char *)resultPointer + y * pitchResult); + resultRow[x] = currentHeight; +} \ No newline at end of file diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java index 218b162923e..2ba9827f439 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java @@ -21,6 +21,7 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP public static final BooleanStoredPropertyKey flyingPointsFilter = keys.addBooleanKey("Flying points filter"); public static final BooleanStoredPropertyKey resetHeightMap = keys.addBooleanKey("Reset Height Map"); public static final BooleanStoredPropertyKey enableAlphaFilter = keys.addBooleanKey("Enable alpha filter"); + public static final BooleanStoredPropertyKey enableVerticalFilter = keys.addBooleanKey("Enable vertical filter"); public static final IntegerStoredPropertyKey searchWindowHeight = keys.addIntegerKey("Search window height"); public static final IntegerStoredPropertyKey searchWindowWidth = keys.addIntegerKey("Search window width"); public static final DoubleStoredPropertyKey minHeightRegistration = keys.addDoubleKey("Min height registration"); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java index 134f6e02ae0..ec0dfc83181 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java @@ -23,6 +23,11 @@ default void setEnableAlphaFilter(boolean enableAlphaFilter) set(HeightMapParameters.enableAlphaFilter, enableAlphaFilter); } + default void setEnableVerticalFilter(boolean enableVerticalFilter) + { + set(HeightMapParameters.enableVerticalFilter, enableVerticalFilter); + } + default void setSearchWindowHeight(int searchWindowHeight) { set(HeightMapParameters.searchWindowHeight, searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java index ec1ac8d67cb..e69fd6b07ae 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java @@ -25,6 +25,11 @@ default boolean getEnableAlphaFilter() return get(enableAlphaFilter); } + default boolean getEnableVerticalFilter() + { + return get(enableVerticalFilter); + } + default int getSearchWindowHeight() { return get(searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json index 03c0ee8e323..5d7af2a8444 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json @@ -3,6 +3,7 @@ "Flying points filter" : true, "Reset Height Map" : false, "Enable alpha filter" : true, + "Enable vertical filter" : false, "Search window height": { "value": 20, "lowerBound": 10, diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index 6b91c14cb6b..31ada4a53ce 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -3,13 +3,14 @@ "Flying points filter" : true, "Reset Height Map" : false, "Enable alpha filter" : true, + "Enable vertical filter" : true, "Search window height" : { "value" : 350, "lowerBound" : 10, "upperBound" : 400 }, "Search window width" : { - "value" : 200, + "value" : 300, "lowerBound" : 10, "upperBound" : 400 }, @@ -69,9 +70,9 @@ "upperBound" : 0.06 }, "Global width in meters" : { - "value" : 4.0, - "lowerBound" : 1.0, - "upperBound" : 5.5 + "value" : 8.0, + "lowerBound" : 2.0, + "upperBound" : 10.0 }, "Global cell size in meters" : { "value" : 0.02, @@ -99,7 +100,7 @@ "upperBound" : 20000.0 }, "Crop window size" : { - "value" : 201, + "value" : 401, "lowerBound" : 40, "upperBound" : 300 },