Integrate FoundationPose into Behaviors #1096

calvertdw · 2025-11-14T17:53:41Z

No description provided.

…etections.

…ing to execute them.

… sequence execution.

…ogic.

calvertdw · 2025-11-26T20:39:46Z

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/ui/RDX3DPanelNotificationManager.java

+
+   public void pushNotification(int additionalStackTraceHeight, String text, boolean log)
+   {
+      if (log)


In the behavior intraprocess test app I didn't want things printing twice.

calvertdw · 2025-11-26T21:38:14Z

...high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/actions/RDXHandPoseAction.java

-      for (int i = 0; i < armMultiBodyGraphic.getJoints().length; i++)
+      double solutionQuality = state.getSolutionQuality();
+      if (solutionQuality < ArmIKSolver.GOOD_QUALITY_MAX || lowQualityRenderThrottler.run(0.5))
      {


Makes the UI not look buggy for low quality solutions.

calvertdw · 2025-11-26T21:39:11Z

...viors/src/main/java/us/ihmc/behaviors/behaviorTree/condition/ProximityConditionExecutor.java

I rewrote the proximity node so it works better and is more general.

calvertdw · 2025-11-26T21:40:11Z

...igh-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/scene/RDXBehaviorTreeScene.java

+      if (message.getDetectionTypeAsString().equals(YOLOv8InstantDetection.class.getSimpleName()))
+         type = "(YOLOv8)";
+      String text = "%s %s %.2f Hz Size: %d ID.%s".formatted(type,
+                                                             message.getObjectClassAsString(),


calvertdw · 2025-11-26T21:40:35Z

...-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java

+      {
+         if (ImGui.button(labels.get(grip.name())))
+            executeGrip = grip;
+         if (grip != Grip.values[5] && grip != Grip.values[7] && grip != Grip.values[Grip.values.length - 1])


calvertdw · 2025-11-26T21:41:31Z

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXLeafNode.java

+            ImGui.getWindowDrawList().addLine(lineStartX, offsetY, offsetX - ImGui.getFontSize(), offsetY, color, 1);
+            ImGui.dummy(0.0f, frameHeight);
+         }
+      }


calvertdw · 2025-11-26T21:42:24Z

...evel-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/condition/RDXProximityCondition.java

+         case XY -> Math.hypot(vectorBToA.getX(), vectorBToA.getY());
+         case Z -> Math.abs(vectorBToA.getZ());
+      };
+      ImGui.text("Distance for Type: %.3f".formatted(distance));


calvertdw · 2025-11-26T21:46:46Z

...h-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/actions/RDXAbilityHandAction.java

+
+      enableWiggleOnFailureWidget.renderImGuiWidget();
+      if (definition.getEnableWiggleOnFailure())
+         timeToWiggleWidget.renderImGuiWidget();


calvertdw · 2025-11-26T22:37:30Z

...vel-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeRootNodeExecutor.java

This is a major rewrite of the concurrent action sequence execution with support for try-catch fallbacks. It is a lot more condensed and solid now.

calvertdw · 2025-11-26T22:38:27Z

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/LeafNodeState.java

 public class LeafNodeState<D extends LeafNodeDefinition> extends BehaviorTreeNodeState<D>
 {
   private final CRDTStatusBoolean isNextForExecution;
-   private final CRDTStatusInteger concurrencyRank;


Concurrency rank has been completely removed. It was a bad idea

calvertdw · 2025-11-26T22:39:19Z

...src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionDefinition.java

+   private final CRDTBidirectionalFloat sufficientCumulativeJointMovement;
+   private final CRDTBidirectionalBoolean enableWiggleOnFailure;
+   private final CRDTBidirectionalFloat timeToWiggle;
+   private final CRDTBidirectionalFloat ultimateTimeout;


I added as much as I could to play with the Ability hands, to try and get them to be more robust. This still needs to be tested on hardware.

calvertdw · 2025-11-26T22:40:27Z

...s/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java

+                     float position = definition.getGoalPositions().getValueReadOnly(i);
+                     float wiggleAmplitude = 7.0f;
+                     float wiggleFrequency = 2.0f;
+                     command.getGoalPositions()[i] = position + wiggleAmplitude * (float) Math.sin(wiggleFrequency * 2 * Math.PI * timer.getElapsedTime());


Hopefully we don't need the wiggle thing, but when the fingers go unresponsive, I don't know another option or if wiggling works.

calvertdw · 2025-11-26T22:41:23Z

...iors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/HandPoseActionExecutor.java

-            }
+            if (rootNode.getState().getOrderedLeaves().get(i) instanceof ChestOrientationActionState chestOrientationAction)
+               concurrentChestOrientationAction = chestOrientationAction;
+            if (rootNode.getState().getOrderedLeaves().get(i) instanceof PelvisHeightOrientationActionState pelvisHeightPitchAction)


Simpler algorithm for concurrent whole body preview. The prior impl was all that concurrency rank was used for.

Also this handles more cases.

calvertdw · 2025-11-26T22:42:27Z

...s/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/SceneActionNodeDefinition.java

+   private final CRDTBidirectionalBoolean useFoundationPose;
+   private final CRDTBidirectionalStringList enabledYoloModels;
+   private final CRDTBidirectionalIntegerList ignoredYoloClassIndices;
+   private final CRDTBidirectionalEnumList<IsaacROSFoundationPoseObject> enabledFoundationPoseModels;


I added a ton of potentially useful fields to the scene action node. We still need to implement the RDX and executor for some of this.

calvertdw · 2025-11-26T22:43:00Z

...ors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/SceneActionNodeExecutor.java

+
+               continue;
+            }
+         }


Above I fixed major bugs. I tested this on real robot and it works perfectly now.

calvertdw · 2025-11-26T22:43:40Z

...ehaviors/src/main/java/us/ihmc/behaviors/behaviorTree/condition/ConditionNodeDefinition.java

-      PROXIMITY;
+      PROXIMITY,
+      ALWAYS_FAIL,
+      ALWAYS_SUCCEED;


Added always succeed and fail conditions, which are useful for testing and workarounds.

calvertdw · 2025-11-26T22:44:24Z

...vel-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/condition/ConditionNodeState.java

   private final LLMConditionState llm;
   private final ProximityConditionState proximityCheck;
-   private final CRDTStatusBoolean conditionMet;
-   private final CRDTStatusBoolean evaluatingCondition;


Remove Luigi's hacks from when I broke my knee and couldn't review it 😆

calvertdw · 2025-11-26T22:45:13Z

...vel-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/control/FallbackNodeExecutor.java

I put as much of the fallback specific execution logic in here as I could.

calvertdw · 2025-11-26T22:45:42Z

...-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/scene/BehaviorTreeSceneExecutor.java

+         IsaacROSFoundationPoseCommunicator mustardCommunicator = foundationPose.get(IsaacROSFoundationPoseObject.MUSTARD);
+         mustardCommunicator.enable(true);
+         mustardCommunicator.addResultCallback(detection -> instantDetectionQueue.add(List.of(detection)));
+      }


calvertdw · 2025-11-26T22:46:49Z

ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java

Untested spatial mapping support.

calvertdw force-pushed the feature/ability-hand-action branch from 1f762db to d6afb77 Compare November 15, 2025 02:20

calvertdw force-pushed the feature/foundation-pose-integration branch from 9dc284c to e9ada27 Compare November 15, 2025 02:32

calvertdw force-pushed the feature/ability-hand-action branch from d6afb77 to 64e46af Compare November 15, 2025 16:12

calvertdw force-pushed the feature/foundation-pose-integration branch from e9ada27 to 103107e Compare November 15, 2025 16:13

Base automatically changed from feature/ability-hand-action to develop November 15, 2025 17:59

calvertdw force-pushed the feature/foundation-pose-integration branch 4 times, most recently from c8498f6 to 00d91cc Compare November 20, 2025 22:21

calvertdw and others added 21 commits November 21, 2025 14:45

Begin FP integration.

62d84d3

Include FoundationPose detections in behavior scene.

d1d7ffe

Get foundationpose on jetson testbed talking to the behavior UI locally.

e50fec0

Let Claude Agent go crazy implementing spatial mapping.

ccccd5b

Fix edge case in isNextForExecution calculation.

f6a7b6e

Fix out of bound index exception.

7522784

Add fields to scene action node.

83bbf30

WIP setting up fields in scene action node.

253170c

Completed draft of new scene action node fields.

6c98255

Make new scene action fields settable and filter for FoundationPose d…

81a4d75

…etections.

Prevent can't execute errors from printing unless we are actually try…

3a1db1d

…ing to execute them.

Fix up scene action node and add debug info.

a543317

Fix drag and drop bug.

35c12ca

Throttle low quality arm solve rendering.

aeb39bb

Avoid duplicate log messages in test UI.

abc6d68

Add always fail and succeed condition node types and iron out bugs in…

a234e44

… sequence execution.

Reimplement proximity check to be more general.

a25de4a

Completely rewrite sequence and fallback execution with concurrency l…

1f31708

…ogic.

Draw fallback boundary.

c1c92f7

Iron out fallback logic. Center circle.

20862cb

Improve logic.

ae47adb

calvertdw requested review from LuigiPenco93, TomaszTB and ds58 November 26, 2025 19:29

calvertdw commented Nov 26, 2025

View reviewed changes

renderRowEnd

f3328d0

calvertdw commented Nov 26, 2025

View reviewed changes

⬆️ ihmc_hands_ros2 0.1.3

13f1329

calvertdw commented Nov 26, 2025

View reviewed changes

ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java

Copy link

Member Author

calvertdw Nov 26, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Untested spatial mapping support.

calvertdw and others added 5 commits November 29, 2025 11:44

Add initial ROS2BehaviorTreeYoRegistry.

bc964bb

Remove concurrency rank from msgs.

4041c9e

Fill out some behavior tree YoVariables.

f90716c

Setup testbed for occupancy map.

af1aa95

zed occupancy map fix

da7fba9

Integrate FoundationPose into Behaviors #1096

Are you sure you want to change the base?

Integrate FoundationPose into Behaviors #1096

Uh oh!

Conversation

calvertdw commented Nov 14, 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!

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!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants