Feature/side stepping ch

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<Empty> CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("clear_goal_footsteps");
    public static final ROS2Topic<PoseListMessage> PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps");
    public static final ROS2Topic<PoseListMessage> ROTATE_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("rotate_goal_footsteps");
+   public static final ROS2Topic<Float32> ROTATE_90_DEGREES = IHMC_ROOT.withModule(moduleName).withType(Float32.class).withSuffix("rotate_90_degrees");
    public static final ROS2Topic<Empty> SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("rotate_goal_footsteps");
 
    // Statuses supported from the Continuous Hiking Process

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<PoseListMessage> turningPublisher;
+   private final ROS2Publisher<Float32> 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<FramePose3D> 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<Pose3D> 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());

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");

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);

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);

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<FramePose3D> 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<Pose3D> 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());