Cleanup to get CH working on stairs, added safety measures, cleaned code, refactors stuff... #717

PotatoPeeler3000 · 2025-03-12T14:35:01Z

No description provided.

…filter on global height map, remove hard-coded swing and transfer time

…certain amount of swing time has passed

…or the HumanoidPerceptionModule

…a filter values after they change from the initial "empty data" value

… maps for normals and collision point calculation

… passes

…anner.

…ymore

…ay visualization was handled

…airs

…meters

…ak for the FilteredVerticalSurfacesExtractor

# Conflicts: # ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/swing/CollisionFreeSwingCalculator.java # ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java

PotatoPeeler3000 · 2025-03-19T17:50:18Z

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

@@ -17,6 +17,7 @@ public class ContinuousHikingAPI
   private static final String ACTIVE_MODULE_NAME = "active_perception";

   // Commands supported for the Continuous Hiking Process
+   public static final ROS2Topic<Empty> RESET_STATE_MACHINE = IHMC_ROOT.withModule(moduleName).withTypeName(Empty.class).withSuffix("reset_state_machine");


Message to reset the state machine

PotatoPeeler3000 · 2025-03-19T17:51:52Z

...footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/graph/LatticePoint.java

@@ -2,7 +2,7 @@

 public class LatticePoint
 {
-   public static final double gridSizeXY = 0.05;
+   public static final double gridSizeXY = 0.02;


Not sure about this

PotatoPeeler3000 · 2025-03-19T17:52:05Z

...rc/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java

Totally broken

PotatoPeeler3000 · 2025-03-19T17:53:50Z

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

@@ -159,11 +164,12 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod
      addChild(stancePoseSelectionPanel);

      DefaultFootstepPlannerParametersBasics footstepPlannerParameters = robotModel.getFootstepPlannerParameters("ForContinuousWalking");
-      SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters();
+      SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters("ForContinuousWalking");


Made swing planning for CH

PotatoPeeler3000 · 2025-03-19T17:54:46Z

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

@@ -208,39 +211,21 @@ private void createParametersPanel(StoredPropertySetBasics storedPropertySetPara
    * This allows the {@link ContinuousPlannerSchedulingTask} to be started for when things are running in simulation, during the operation on the robot this
    * method should not be called as it will interfere with the remote process
    */
-   public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe)
+   public void startContinuousPlannerSchedulingTask(ActiveMappingParameterToolBox activeMappingParameterToolBox,


Pass in a ActiveMappingParameterToolBox to get all the parameters

PotatoPeeler3000 · 2025-03-19T18:03:54Z

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

@@ -82,6 +84,13 @@ public RapidHeightMapManager(ROS2Node ros2Node,

   public void update(Mat latestDepthImage, Instant imageAcquisitionTime, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) throws Exception
   {
+      RigidBodyTransform sensorToWorld = cameraFrame.getTransformToWorldFrame();


Getting this RigidBodyTransform closer to when the frame was grabbed helps reduce the latency

PotatoPeeler3000 · 2025-03-19T18:06:21Z

ihmc-perception/src/main/java/us/ihmc/sensors/realsense/RealSenseImageSensor.java

@@ -17,7 +17,7 @@ public class RealSenseImageSensor extends ImageSensor
   public static final int DEPTH_IMAGE_KEY = 1;
   public static final int OUTPUT_IMAGE_COUNT = 2;

-   private static final double OUTPUT_FREQUENCY = 20.0;
+   private static final double OUTPUT_FREQUENCY = 30.0;


Up the publish rate to 30 hz

PotatoPeeler3000 · 2025-03-19T18:06:36Z

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

+    unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX;
+
+    // We have an initial guess at all the height values and if the previous height value is that guess, don't apply the alpha, just accept the new value
+    if (dontUseAlphaYet)


Don't use the filter if the data is based off of a guess

PotatoPeeler3000 · 2025-03-19T18:06:49Z

...ion/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredVerticalSurfacesExtractorTest.java

Test for GPU memory leak

PotatoPeeler3000 · 2025-03-19T18:07:02Z

...processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json

  "Enable alpha filter" : true,
  "Enable vertical filter" : true,
  "Search window height" : {
-    "value" : 350,
+    "value" : 400,


Little bit bigger

TomaszTB

Few comments you don't have to do anything about, and few things that should be fixed. Looking great overall!

...high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/footstepPlanner/RDXFootstepPlanning.java

...h-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java

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

...main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java

...ception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredVerticalSurfacesExtractor.java

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

...ption/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java

...ion/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredVerticalSurfacesExtractorTest.java

TomaszTB

The stuff that I'm familiar with looks good. No idea about some of the math and parameters haha. Just gotta trust it's good

PotatoPeeler3000 and others added 26 commits March 11, 2025 16:38

Ability to reset CH state machine when robot falls, perform vertical …

ac0b32a

…filter on global height map, remove hard-coded swing and transfer time

Add button to the UI to reset the state machine for CH

10c0dd4

Adding another height map parameter to disalbe the drift offset

484e5b4

Added the option to restrict the user to only start planning after a …

bd80a17

…certain amount of swing time has passed

Publish realsense at 30 hz

675fa73

Changed HeightMapParameters to not be static, removed some old code f…

2ad56e6

…or the HumanoidPerceptionModule

Fixed from hardware testing

a81be4b

Remove a duplicate alpha filter and update the other one to only alph…

a3d5996

…a filter values after they change from the initial "empty data" value

set up an improved collision avoidance detection routine using height…

68f8a96

… maps for normals and collision point calculation

found an error in the swing over tests that was giving a lot of false…

ec5a020

… passes

Remove an if statement from HeightMapData to speed up the footstep pl…

859af84

…anner.

Remove a percent through swing parameter from CH that we don't use an…

2676bb4

…ymore

fixed a bunch of bugs on the collision calculator, and improved the w…

862ffbd

…ay visualization was handled

further improved visualization to get all the tests to pass

f322543

Fixed getting the height map data on the UI side of things

55a353e

Merge branch 'feature/improve_collision_avoidance' into feature/ch-st…

308c4f6

…airs

Merge branch 'feature/improve_collision_avoidance' into feature/ch-st…

72ce88f

…airs

Reduce the lattice point for the xy grid size, and tune a couple para…

6559f1f

…meters

Fix parameter bug

a744de2

Unused import

44dd57f

Fixed the height map thread in the simulation UI

19cd2e2

Cleaning and make threading more clear

e11095e

Try to close more things but still getting GPU problem

051ec23

Fix memory leak for the FilteredRapidHeightMapExtractor

1a805d7

Add tests to check against a memory leak on the GPU and fix memory le…

0def595

…ak for the FilteredVerticalSurfacesExtractor

Merge branch 'develop' into feature/ch-stairs

6833c44

# Conflicts: # ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/swing/CollisionFreeSwingCalculator.java # ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java

PotatoPeeler3000 commented Mar 19, 2025

View reviewed changes

Unused import

4892e79

TomaszTB requested changes Mar 19, 2025

View reviewed changes

Simplified convertToHeightMapImage method

49c3b58

PotatoPeeler3000 added 2 commits March 20, 2025 12:11

Remove unneeded stream

4cb6494

Cleanup

df6c008

TomaszTB approved these changes Mar 20, 2025

View reviewed changes

Merge branch 'develop' into feature/ch-stairs

c0f1622

PotatoPeeler3000 merged commit 259cd3e into develop Mar 21, 2025
65 of 68 checks passed

PotatoPeeler3000 deleted the feature/ch-stairs branch March 21, 2025 13:28

Cleanup to get CH working on stairs, added safety measures, cleaned code, refactors stuff... #717

Cleanup to get CH working on stairs, added safety measures, cleaned code, refactors stuff... #717

Uh oh!

Conversation

PotatoPeeler3000 commented Mar 12, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

TomaszTB left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

TomaszTB left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!