Initial implementation for square up and walk-to-goal for CH process #694

PotatoPeeler3000 · 2025-02-24T19:37:35Z

This is a Bhav level PR lol, a bunch of random changes that are grouped together. Most of the file changes come from refactoring things to different places. Here is a list of the changes:
-------------------------------- Package Change ------------------------------

Moved RapidHeightMapManager to ihmc-perception module
Moved ControllerQueueFootstepMonitor to ihmc-humanoid-robots module
-------------------------------- Refactor Parameters ------------------------------
Movedd RapidHeightMapExtractor.getHeightMapParameters() to get called in the RapidHeightMapManager.getHeightMapParameters() which allows for only one set of parameters to be created.
Change ROS2Helper to ROS2Node for the height map pipeline.
-------------------------------- Height Map Kernels -----------------------------------
Added a FilteredRapidHeightMapExtractor class that has a kernel that performs a weighted moving average on the height map as a filter. In this implementation its post-processing the height map. What should really be done is pre-processing the height map. (Change for the future if its a problem)
Tests for FilteredRapidHeightMapExtractor
Example CUDAExample3DKernel to show how to pass a 3D array into the kernels
-------------------------------- RDX UI Changes -----------------------------
Added some better environments that reflect tested data. Cleaned up a couple of other environments
Removed a panel for the CUDA Height Map Parameters as its no longer needed
Removed bloat from CH panel, condensed dropdown parameters so they aren't visible by default
-------------------------------- Continuous Hiking Changes -----------------------------
Created a square up message, its an empty message but in the CH process it handles the square up step required
Set squaring up to be the default and added an 'emergency stop' button to the UI (Esc Key).
Added a plan-to-goal feature for CH. This skips the CH pipeline and plans directly to a goal all at once. Takes a little but but the intent is to use it to rotate the robot in place.
Added a couple buttons on the UI to rotate the robot 90 degrees left or right. This uses the plan-to-goal feature to do this.
Reset graphics when the robot isn't walking.
Basically made the ContinuousHikingLogger useless by removing the usage from ControllerQueueFootstepMonitor so should come back to that but its not pressing.

…istory of height maps

…verage and variance

…e feature on and off was added as well.

…hed everything upstream that was affected

… the parameters.

...-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java

PotatoPeeler3000 · 2025-02-24T21:39:39Z

...tstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java

@@ -18,6 +19,8 @@ public class ContinuousHikingAPI
   public static final ROS2Topic<ContinuousHikingCommandMessage> CONTINUOUS_HIKING_COMMAND = IHMC_ROOT.withModule(moduleName).withType(ContinuousHikingCommandMessage.class).withSuffix("command");
   public static final ROS2Topic<std_msgs.msg.dds.Empty> CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.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");


New messages to walk the robot around with little user input

PotatoPeeler3000 · 2025-02-24T21:40:11Z

...high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java

@@ -210,6 +213,11 @@ else if (dScroll < 0.0)
            latestPickPoint.getOrientation().setYawPitchRoll(latestFootstepYaw + deltaYaw, 0.0, 0.0);
         }
      }
+      if (input.isWindowHovered() && input.mouseReleasedWithoutDrag(ImGuiMouseButton.Middle) && calculateStancePose.get() && selectionActive)


Middle click allows for this feature to plan-to-goal rather then add it to the CH goal queue

...haviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java

PotatoPeeler3000 · 2025-02-24T21:40:51Z

...c/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java

@@ -56,6 +56,7 @@ public void doAction(double timeInState)
         message.setClearRemainingFootstepQueue(true);
         continuousPlanner.setLatestFootstepPlan(null);
         pauseWalkingPublisher.publish(message);
+         debugger.resetVisualizationForUIPublisher();


Clear visuals when standing

PotatoPeeler3000 · 2025-02-24T21:41:24Z

...rc/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java

The new messages affect this state, this way I can skip the entire CH process when desired.

PotatoPeeler3000 · 2025-02-24T21:41:56Z

...erception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java

Filtered height map class

PotatoPeeler3000 · 2025-02-24T21:42:23Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

@@ -34,10 +34,12 @@
 */
 public class RapidHeightMapManager
 {
+   static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU");


Now we have one parameter set for the height map, not one each for the extractor type

PotatoPeeler3000 · 2025-02-24T21:42:40Z

ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java

      setContactMap(other.contactMap);
      setTerrainCostMap(other.terrainCostMap);
+
+      setSteppableRegionAssignmentMat(other.steppableRegionAssignmentMat);
+      setSteppableRegionRingMat(other.steppableRegionRingMat);


This was missing some stuff in the copy constructor

PotatoPeeler3000 · 2025-02-24T21:42:58Z

ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu

Pretty simple but could prove useful in the future

TomaszTB

I really feel like this PR should be split into four or so PRs... Maybe like this?

ROS2Helper -> ROS2Node + ROS2Publisher changes (this one might be controversial)
3D CUDA kernel example + height map filter
Cleanup + parameter changes
Changes related to continuous hiking + UI

Otherwise I feel like getting this one merged will be difficult

TomaszTB

I'll review this PR a bit slowly, one piece at a time (sorry this might take a while).

I added comments about the height map filter. Logic looks good, just some cleanup and memory freeing stuff I noticed.

...eption/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu

TomaszTB · 2025-02-25T15:55:20Z

...eption/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu

+#define LAYERS 0
+
+__global__
+void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA,


Could you give a better name to matrixPointer? What sort of matrix is it? Seems like it holds some sort of history to me.

TomaszTB · 2025-02-25T15:56:49Z

ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java

+      // Create a 2x2 with 5 layers
+      int rows = 2;
+      int cols = 2;
+      int layers = 4;


Comment says 5 layers but layers = 4?

Still looks the same to me

...erception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java

TomaszTB · 2025-02-25T19:40:27Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java

@@ -286,6 +287,9 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform
      error = cudaStreamSynchronize(stream);
      CUDATools.checkCUDAError(error);

+      if (heightMapParameters.getEnableAlphaFilter())
+         globalHeightMapImage = filteredRapidHeightMapExtractor.update(globalHeightMapImage);


You should close the globalHeightMapImage between the call to update and the assignment

You need to free the CUDA memory of the globalHeightMapImage before you reassign it with a new GpuMat

Oh, it's actually gonna be a bit more complicated since you don't always return a new GpuMat. Sometimes you return the passed in one, then you don't want to free anything... Hmmm... Maybe just always return a new GpuMat? Or do it OpenCV style where you pass in the "destination" GpuMat as a parameter to the update method. That also leaves memory handling to whatever is calling update.

...eption/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu

TomaszTB

Looking pretty good with the height map filter. Just a few minor things I noticed.

...erception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java

...tstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java

...high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java

TomaszTB · 2025-02-26T22:07:34Z

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java

+   {
+      MovingReferenceFrame midFeetZUpFrame = syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame();
+      FramePose3D midFeetZUpPose = new FramePose3D(midFeetZUpFrame, midFeetZUpFrame.getTransformToWorldFrame());
+      SideDependentList<FramePose3D> goalPoses = new SideDependentList<>(new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()),


I feel like you're not getting anything out of this side dependent list. Why not just have two variables leftGoalPose and rightGoalPose?

TomaszTB · 2025-02-26T22:16:11Z

...rc/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java

+                                                                                        FramePose3DReadOnly soleFramePose;
+                                                                                        if (controllerQueueMonitor.getNumberOfIncompleteFootsteps() > 0)
+                                                                                        {
+                                                                                           LogTools.info("Yes this queue is not empty");


Do you want to keep this?

Formatter, rip

I don't mean the formatting, I mean the logging
(although the formatting is awful too)

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

TomaszTB · 2025-02-26T22:29:11Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

+         ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotName), message ->
+         {
+            if (message.takeNextData().getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING)
+            {
               resetHeightMapRequested.set();
+            }
         });


Suggested change

ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotName), message ->

{

if (message.takeNextData().getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING)

{

resetHeightMapRequested.set();

}

});

ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotName), message ->

{

if (message.getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING)

{

resetHeightMapRequested.set();

}

});

ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

...

PotatoPeeler3000 and others added 23 commits February 10, 2025 20:45

Added an example of how to create a 3d matrix

f4cb234

Remove unused code for testing

a8294a6

Fixed 3d cuda example and return the average to the cpu

7f03cda

Using cudaMalloc3D

c5b1f13

Started writing a test to have this example be used in a class

06c1006

Fixed FilteredRapidHeightMapExtractor to compute the average of the h…

94dac47

…istory of height maps

Updated the kernel to compute the height estimate based on a moving a…

29920f7

…verage and variance

Alpha filter (ish) is working on the height map, UI button to turn th…

ab8ee1a

…e feature on and off was added as well.

Tuned filter a little bit

8c0cd8e

Merge branch 'refs/heads/develop' into feature/3d-kernel-example

03f2c43

Update environments for the UI

582910a

Switched from ROS2Helper to ROS2Node for StoredPropertySets and switc…

da4c67b

…hed everything upstream that was affected

Using createSubscription2 where applicable (#679)

f2d2dd8

Merge branch 'develop' into feature/one-ui

7210f10

Switch height map to use ros2Node to remove ros2Helper

c51203a

Implemented square up and walk to goal as a message within CH

4f5fdd7

Fix translation for when robot isn't facing forward

b5b09e3

Cleanup UI look so there is less scrolling involved

6221b64

Moved the RapidHeightMapManager into the perception package to reduce…

bdd6eb4

… the parameters.

Add reset method for visuals

b98358d

Merge branch 'feature/3d-kernel-example' into feature/new-ch-messages

d1205fa

Cleanup files and remove testing lines

3109bc8

Moved publisher to constructor

a78af41

PotatoPeeler3000 changed the title ~~Feature/new ch messages~~ Initial implementation for square up and walk-to-goal for CH process Feb 24, 2025

Merge branch 'develop' into feature/new-ch-messages

539411a

PotatoPeeler3000 commented Feb 24, 2025

View reviewed changes

PotatoPeeler3000 requested review from TomaszTB and LaughingCommit February 24, 2025 21:45

Change planar region to use ros2helper still, merge conflict

5431014

TomaszTB reviewed Feb 24, 2025

View reviewed changes

Cleanup code mainly for height map filter

9bec35b

TomaszTB requested changes Feb 25, 2025

View reviewed changes

Cleanup filtered kernel

e22e2e6

TomaszTB requested changes Feb 26, 2025

View reviewed changes

PotatoPeeler3000 added 3 commits February 26, 2025 15:35

Check error after mem copy

7fb78e1

Change error checking

4025d10

Change error checking

7873a70

TomaszTB previously requested changes Feb 26, 2025

View reviewed changes

Cleanup fields

9adb15d

ihmc-rosie approved these changes Feb 27, 2025

View reviewed changes

Merge branch 'develop' into feature/new-ch-messages

8c04fc1

PotatoPeeler3000 merged commit 34baa22 into develop Feb 27, 2025
63 of 68 checks passed

PotatoPeeler3000 deleted the feature/new-ch-messages branch February 27, 2025 18:19

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Initial implementation for square up and walk-to-goal for CH process #694

Initial implementation for square up and walk-to-goal for CH process #694

PotatoPeeler3000 commented Feb 24, 2025 •

edited

Loading

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

PotatoPeeler3000 Feb 24, 2025

TomaszTB left a comment

TomaszTB left a comment

TomaszTB Feb 25, 2025

TomaszTB Feb 25, 2025

PotatoPeeler3000 Feb 26, 2025

TomaszTB Feb 26, 2025

TomaszTB Feb 25, 2025

PotatoPeeler3000 Feb 26, 2025

TomaszTB Feb 26, 2025

TomaszTB Feb 26, 2025

TomaszTB left a comment

TomaszTB Feb 26, 2025

TomaszTB Feb 26, 2025

PotatoPeeler3000 Feb 26, 2025

TomaszTB Feb 27, 2025 •

edited

Loading

TomaszTB Feb 26, 2025

Initial implementation for square up and walk-to-goal for CH process #694

Initial implementation for square up and walk-to-goal for CH process #694

Conversation

PotatoPeeler3000 commented Feb 24, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TomaszTB left a comment

Choose a reason for hiding this comment

TomaszTB left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TomaszTB left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TomaszTB Feb 27, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

PotatoPeeler3000 commented Feb 24, 2025 •

edited

Loading

TomaszTB Feb 27, 2025 •

edited

Loading