From 0f4ea695cd2911df86a48db73c0ef10d1cf1aea8 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 26 Nov 2025 19:36:25 -0600 Subject: [PATCH 1/8] Extract Grip and ControlMode to classes. --- .../actions/RDXAbilityHandAction.java | 14 ++++---- .../psyonicAbilityHand/RDXAbilityHand.java | 18 +++++------ .../actions/AbilityHandActionDefinition.java | 32 +++++++++---------- .../actions/AbilityHandActionExecutor.java | 10 +++--- 4 files changed, 37 insertions(+), 37 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/actions/RDXAbilityHandAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/actions/RDXAbilityHandAction.java index aaa8423306e..ae970db80ff 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/actions/RDXAbilityHandAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/actions/RDXAbilityHandAction.java @@ -5,8 +5,8 @@ import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionDefinition; import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionDefinition.SuccessCriteria; import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionState; -import us.ihmc.handsros2.abilityHand.AbilityHandManager; -import us.ihmc.handsros2.abilityHand.AbilityHandManager.ControlMode; +import us.ihmc.handsros2.abilityHand.AbilityHandGrip; +import us.ihmc.handsros2.abilityHand.AbilityHandControlMode; import us.ihmc.rdx.behaviorTree.RDXBehaviorTreeRootNode; import us.ihmc.rdx.imgui.ImBooleanWrapper; import us.ihmc.rdx.imgui.ImFloatWrapper; @@ -23,7 +23,7 @@ public class RDXAbilityHandAction extends RDXActionNode side; - private final CRDTBidirectionalEnumField controlMode; - private final CRDTBidirectionalEnumField grip; + private final CRDTBidirectionalEnumField controlMode; + private final CRDTBidirectionalEnumField grip; private final CRDTBidirectionalFloatArray goalPositions; private final CRDTBidirectionalFloatArray goalVelocities; private final CRDTBidirectionalEnumField successCriteria; @@ -39,8 +39,8 @@ public enum SuccessCriteria // On disk fields private RobotSide onDiskSide; - private ControlMode onDiskControlMode; - private Grip onDiskGrip; + private AbilityHandControlMode onDiskControlMode; + private AbilityHandGrip onDiskGrip; private final float[] onDiskGoalPositions = new float[6]; private final float[] onDiskGoalVelocities = new float[6]; private SuccessCriteria onDiskSuccessCriteria; @@ -55,8 +55,8 @@ public AbilityHandActionDefinition(BehaviorTreeRootNodeDefinition rootNode) super(rootNode); side = new CRDTBidirectionalEnumField<>(this, RobotSide.LEFT); - controlMode = new CRDTBidirectionalEnumField<>(this, ControlMode.GRIP); - grip = new CRDTBidirectionalEnumField<>(this, Grip.RELAX); + controlMode = new CRDTBidirectionalEnumField<>(this, AbilityHandControlMode.GRIP); + grip = new CRDTBidirectionalEnumField<>(this, AbilityHandGrip.RELAX); goalPositions = new CRDTBidirectionalFloatArray(this, 6); goalVelocities = new CRDTBidirectionalFloatArray(this, 6); for (int i = 0; i < 6; i++) @@ -97,8 +97,8 @@ public void loadFromFile(JsonNode jsonNode) super.loadFromFile(jsonNode); side.setValue(RobotSide.getSideFromString(jsonNode.get("side").asText())); - controlMode.setValue(ControlMode.valueOf(jsonNode.get("controlMode").asText())); - grip.setValue(Grip.valueOf(jsonNode.get("grip").asText())); + controlMode.setValue(AbilityHandControlMode.valueOf(jsonNode.get("controlMode").asText())); + grip.setValue(AbilityHandGrip.valueOf(jsonNode.get("grip").asText())); for (int i = 0; i < 6; i++) goalPositions.setValue(i, (float) jsonNode.get("goalPositions").get(i).asDouble()); for (int i = 0; i < 6; i++) @@ -199,8 +199,8 @@ public void fromMessage(AbilityHandActionDefinitionMessage message) super.fromMessage(message.getDefinition()); side.fromMessage(RobotSide.fromByte(message.getRobotSide())); - controlMode.fromMessageOrdinal(message.getControlMode(), ControlMode.values); - grip.fromMessageOrdinal(message.getGrip(), Grip.values); + controlMode.fromMessageOrdinal(message.getControlMode(), AbilityHandControlMode.values); + grip.fromMessageOrdinal(message.getGrip(), AbilityHandGrip.values); goalPositions.fromMessage(message.getGoalPositions()); goalVelocities.fromMessage(message.getGoalVelocities()); successCriteria.fromMessageOrdinal(message.getSuccessCriteria(), SuccessCriteria.values); @@ -222,22 +222,22 @@ public void setSide(RobotSide side) this.side.setValue(side); } - public ControlMode getControlMode() + public AbilityHandControlMode getControlMode() { return controlMode.getValue(); } - public void setControlMode(ControlMode controlMode) + public void setControlMode(AbilityHandControlMode controlMode) { this.controlMode.setValue(controlMode); } - public Grip getGrip() + public AbilityHandGrip getGrip() { return grip.getValue(); } - public void setGrip(Grip grip) + public void setGrip(AbilityHandGrip grip) { this.grip.setValue(grip); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java index 30f82279368..35fc93e4a45 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java @@ -6,8 +6,8 @@ import us.ihmc.behaviors.behaviorTree.action.ActionNodeExecutor; import us.ihmc.handsros2.HandInterface; import us.ihmc.handsros2.HandType; -import us.ihmc.handsros2.abilityHand.AbilityHandManager.ControlMode; -import us.ihmc.handsros2.abilityHand.AbilityHandManager.Grip; +import us.ihmc.handsros2.abilityHand.AbilityHandControlMode; +import us.ihmc.handsros2.abilityHand.AbilityHandGrip; import us.ihmc.robotics.EuclidCoreMissingTools; import us.ihmc.tools.Timer; @@ -47,9 +47,9 @@ public void triggerExecution() state.getCommandedJointTrajectories().addTrajectoryPoint(i, handState.getActuatorPositions()[i], 0.0); command.setControlMode(definition.getControlMode().toByte()); - if (definition.getControlMode() == ControlMode.GRIP) + if (definition.getControlMode() == AbilityHandControlMode.GRIP) { - Grip grip = definition.getGrip(); + AbilityHandGrip grip = definition.getGrip(); command.setGrip(grip.toByte()); double stageLength = nominalExecutionDuration / grip.getNumberOfStages(); @@ -151,7 +151,7 @@ public void updateCurrentlyExecuting() AbilityHandCommand command = getCommand(); if (command != null) { - command.setControlMode(ControlMode.POSITION.toByte()); + command.setControlMode(AbilityHandControlMode.POSITION.toByte()); for (int i = 0; i < 6; i++) { float position = definition.getGoalPositions().getValueReadOnly(i); From 6fbf7d092d99abb8c4e77224c1616626a08addbb Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 26 Nov 2025 21:07:17 -0600 Subject: [PATCH 2/8] Removing interfaces where possible. --- .../us/ihmc/rdx/ui/hands/RDXHandManager.java | 39 +++++++++++-------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java index cd65272c539..9ce31088154 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java @@ -5,7 +5,6 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.RobotVersion; import us.ihmc.handsros2.HandInterface; -import us.ihmc.handsros2.HandROS2HardwareCommunication; import us.ihmc.handsros2.HandType; import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; import us.ihmc.handsros2.ezGripper.EZGripperROS2HardwareCommunication; @@ -15,9 +14,6 @@ import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import java.util.HashMap; -import java.util.Map; - /** * Manages the UI for a humanoid robot's hands. A hand configuration is like "open", "closed", etc. */ @@ -26,7 +22,8 @@ public class RDXHandManager private final SideDependentList rdxHands = new SideDependentList<>(); private final SideDependentList quickAccessButtons = new SideDependentList<>(); - private final Map> handCommunications = new HashMap<>(); + private EZGripperROS2HardwareCommunication ezGripperCommunication = null; + private AbilityHandROS2HardwareCommunication abilityHandCommunication = null; public RDXHandManager(DRCRobotModel robotModel) { @@ -39,31 +36,37 @@ public RDXHandManager(DRCRobotModel robotModel) if (!robotVersion.hasHandWithFingers(side) || handType == null) continue; - if (!handCommunications.containsKey(handType)) + switch (handType) { - HandROS2HardwareCommunication handCommunication = switch (handType) + case EZ_GRIPPER -> + { + if (ezGripperCommunication == null) + ezGripperCommunication = new EZGripperROS2HardwareCommunication(getClass().getSimpleName() + "EZGripperCommunication"); + } + case ABILITY_HAND -> { - case EZ_GRIPPER -> new EZGripperROS2HardwareCommunication(getClass().getSimpleName() + "EZGripperCommunication"); - case ABILITY_HAND -> new AbilityHandROS2HardwareCommunication(getClass().getSimpleName() + "AbilityHandCommunication"); - }; - handCommunications.put(handType, handCommunication); + if (abilityHandCommunication == null) + abilityHandCommunication = new AbilityHandROS2HardwareCommunication(getClass().getSimpleName() + "AbilityHandCommunication"); + } } String handIdentifier = HandInterface.getSimpleIdentifier(robotName, side, handType); switch (handType) { case EZ_GRIPPER -> - rdxHands.put(side, new RDXEZGripper(handIdentifier, side, (EZGripperROS2HardwareCommunication) handCommunications.get(handType))); + rdxHands.put(side, new RDXEZGripper(handIdentifier, side, ezGripperCommunication)); case ABILITY_HAND -> - rdxHands.put(side, new RDXAbilityHand(handIdentifier, side, (AbilityHandROS2HardwareCommunication) handCommunications.get(handType))); + rdxHands.put(side, new RDXAbilityHand(handIdentifier, side, abilityHandCommunication)); } } } public void create(RDXBaseUI baseUI) { - for (HandROS2HardwareCommunication handCommunication : handCommunications.values()) - handCommunication.start(); + if (ezGripperCommunication != null) + ezGripperCommunication.start(); + if (abilityHandCommunication != null) + abilityHandCommunication.start(); for (RobotSide side : rdxHands.sides()) quickAccessButtons.put(side, new RDXHandQuickAccessButtons(baseUI, rdxHands.get(side))); @@ -99,7 +102,9 @@ public RDXHandInterface getHand(RobotSide handSide) public void destroy() { - for (HandROS2HardwareCommunication handCommunication : handCommunications.values()) - handCommunication.shutdown(); + if (ezGripperCommunication != null) + ezGripperCommunication.shutdown(); + if (abilityHandCommunication != null) + abilityHandCommunication.shutdown(); } } From 989b9fca9fc39f59178891342de9486ddd52119f Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 26 Nov 2025 21:35:15 -0600 Subject: [PATCH 3/8] Get a little stuck with double float nonsense. --- .../behaviorTree/action/actions/AbilityHandActionExecutor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java index 35fc93e4a45..980641ea507 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java @@ -58,7 +58,7 @@ public void triggerExecution() for (int i = 0; i < grip.getFingersInStage(s); i++) { int finger = grip.getStageFingerIndex(s, i); - float position = grip.getStageFingerPosition(s, i); + double position = grip.getStageFingerPosition(s, i); state.getCommandedJointTrajectories().addTrajectoryPoint(finger, position, (s + 1) * stageLength); } } From 95e07db229c614af0879ad9b1176bc8a47553617 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Fri, 28 Nov 2025 15:47:18 -0600 Subject: [PATCH 4/8] Merge EZGripperManager into EZGripper. --- .../us/ihmc/rdx/ui/hands/sakeEZGripper/RDXEZGripper.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/sakeEZGripper/RDXEZGripper.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/sakeEZGripper/RDXEZGripper.java index b2c33c2ecb2..f715c36f7af 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/sakeEZGripper/RDXEZGripper.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/sakeEZGripper/RDXEZGripper.java @@ -20,8 +20,7 @@ import us.ihmc.robotics.EuclidCoreMissingTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.handsros2.ezGripper.EZGripperError; -import us.ihmc.handsros2.ezGripper.EZGripperManager; -import us.ihmc.handsros2.ezGripper.EZGripperManager.OperationMode; +import us.ihmc.handsros2.ezGripper.EZGripper.OperationMode; import us.ihmc.handsros2.ezGripper.EZGripperROS2HardwareCommunication; public class RDXEZGripper implements RDXHandInterface @@ -49,7 +48,7 @@ public class RDXEZGripper implements RDXHandInterface private final ImGuiFlashingText needResetStatusText = new ImGuiFlashingText(ImGuiTools.RED); private final ImGuiFlashingText sakeErrorStatusText = new ImGuiFlashingText(ImGuiTools.RED); - private EZGripperManager.OperationMode previousOperationMode = null; + private OperationMode previousOperationMode = null; public RDXEZGripper(String identifier, RobotSide handSide, EZGripperROS2HardwareCommunication communication) { From 34e7000d749d6bbcaf3f8b34ad50941a6a4f87d3 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Fri, 28 Nov 2025 19:55:33 -0600 Subject: [PATCH 5/8] Improve ability hand comms in RDX UI and behavior executor. --- .../rdx/ui/affordances/RDXArmManager.java | 2 +- .../us/ihmc/rdx/ui/hands/RDXHandManager.java | 16 +---- .../psyonicAbilityHand/RDXAbilityHand.java | 63 ++++++++++------- .../behaviorTree/BehaviorTreeExecutor.java | 13 ++-- .../BehaviorTreeExecutorNodeBuilder.java | 10 +-- .../BehaviorTreeNodeExecutor.java | 11 +-- .../BehaviorTreeRootNodeExecutor.java | 11 +-- .../actions/AbilityHandActionComms.java | 67 +++++++++++++++++++ .../actions/AbilityHandActionExecutor.java | 52 +++++--------- 9 files changed, 151 insertions(+), 94 deletions(-) create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionComms.java diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java index 6d54cb2db0b..de33e1a7886 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java @@ -121,7 +121,7 @@ public RDXArmManager(CommunicationHelper communicationHelper, armConfigurationNames[i] = PresetArmConfiguration.values[i].name(); } - handManager = new RDXHandManager(robotModel); + handManager = new RDXHandManager(robotModel, communicationHelper.getROS2Node()); } public void create(RDXBaseUI baseUI) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java index 9ce31088154..11d0cb5e84d 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java @@ -6,13 +6,13 @@ import us.ihmc.avatar.drcRobot.RobotVersion; import us.ihmc.handsros2.HandInterface; import us.ihmc.handsros2.HandType; -import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; import us.ihmc.handsros2.ezGripper.EZGripperROS2HardwareCommunication; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.hands.psyonicAbilityHand.RDXAbilityHand; import us.ihmc.rdx.ui.hands.sakeEZGripper.RDXEZGripper; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.ros2.ROS2Node; /** * Manages the UI for a humanoid robot's hands. A hand configuration is like "open", "closed", etc. @@ -23,9 +23,8 @@ public class RDXHandManager private final SideDependentList quickAccessButtons = new SideDependentList<>(); private EZGripperROS2HardwareCommunication ezGripperCommunication = null; - private AbilityHandROS2HardwareCommunication abilityHandCommunication = null; - public RDXHandManager(DRCRobotModel robotModel) + public RDXHandManager(DRCRobotModel robotModel, ROS2Node ros2Node) { RobotVersion robotVersion = robotModel.getRobotVersion(); String robotName = robotModel.getSimpleRobotName(); @@ -43,11 +42,6 @@ public RDXHandManager(DRCRobotModel robotModel) if (ezGripperCommunication == null) ezGripperCommunication = new EZGripperROS2HardwareCommunication(getClass().getSimpleName() + "EZGripperCommunication"); } - case ABILITY_HAND -> - { - if (abilityHandCommunication == null) - abilityHandCommunication = new AbilityHandROS2HardwareCommunication(getClass().getSimpleName() + "AbilityHandCommunication"); - } } String handIdentifier = HandInterface.getSimpleIdentifier(robotName, side, handType); @@ -56,7 +50,7 @@ public RDXHandManager(DRCRobotModel robotModel) case EZ_GRIPPER -> rdxHands.put(side, new RDXEZGripper(handIdentifier, side, ezGripperCommunication)); case ABILITY_HAND -> - rdxHands.put(side, new RDXAbilityHand(handIdentifier, side, abilityHandCommunication)); + rdxHands.put(side, new RDXAbilityHand(handIdentifier, side, ros2Node)); } } } @@ -65,8 +59,6 @@ public void create(RDXBaseUI baseUI) { if (ezGripperCommunication != null) ezGripperCommunication.start(); - if (abilityHandCommunication != null) - abilityHandCommunication.start(); for (RobotSide side : rdxHands.sides()) quickAccessButtons.put(side, new RDXHandQuickAccessButtons(baseUI, rdxHands.get(side))); @@ -104,7 +96,5 @@ public void destroy() { if (ezGripperCommunication != null) ezGripperCommunication.shutdown(); - if (abilityHandCommunication != null) - abilityHandCommunication.shutdown(); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java index 7ccdea82059..8ac2493e105 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java @@ -2,19 +2,24 @@ import com.badlogic.gdx.graphics.Color; import ihmc_hands_ros2.msg.dds.AbilityHandCommand; +import ihmc_hands_ros2.msg.dds.AbilityHandState; import imgui.ImGui; import imgui.flag.ImGuiCol; import imgui.type.ImFloat; import us.ihmc.commons.thread.Throttler; +import us.ihmc.commons.thread.TypedNotification; import us.ihmc.handsros2.abilityHand.AbilityHandControlMode; import us.ihmc.handsros2.abilityHand.AbilityHandGrip; -import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; +import us.ihmc.handsros2.abilityHand.AbilityHandROS2API; import us.ihmc.log.LogTools; import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.ui.hands.RDXHandInterface; import us.ihmc.robotics.EuclidCoreMissingTools; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; +import us.ihmc.tools.Timer; public class RDXAbilityHand implements RDXHandInterface { @@ -30,45 +35,59 @@ public class RDXAbilityHand implements RDXHandInterface private final String identifier; private final RobotSide handSide; - private final AbilityHandROS2HardwareCommunication communication; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); private final ImFloat[] desiredPositions = new ImFloat[6]; private final ImFloat[] desiredVelocities = new ImFloat[6]; - - private float[] currentPositions = new float[6]; private AbilityHandGrip executeGrip = null; private boolean executeVelToPos = false; - private final Throttler publishThrottler = new Throttler().setFrequency(30.0); - public RDXAbilityHand(String identifier, RobotSide handSide, AbilityHandROS2HardwareCommunication communication) + private final TypedNotification stateNotification = new TypedNotification<>(); + private AbilityHandState latestState = null; + private final AbilityHandCommand command = new AbilityHandCommand(); + private final ROS2Publisher commandPublisher; + private final Throttler commandThrottler = new Throttler().setFrequency(30.0); + private final Timer connectedTimer = new Timer(); + + public RDXAbilityHand(String identifier, RobotSide handSide, ROS2Node ros2Node) { this.identifier = identifier; this.handSide = handSide; - this.communication = communication; for (int i = 0; i < 6; i++) { desiredPositions[i] = new ImFloat(START_POSITION); desiredVelocities[i] = new ImFloat(DEFAULT_VELOCITY); } + + ros2Node.createSubscription2(AbilityHandROS2API.STATE_TOPIC, stateNotification::set); + command.setIdentifier(identifier); + commandPublisher = ros2Node.createPublisher(AbilityHandROS2API.COMMAND_TOPIC); } @Override public void update() { - if (!communication.getAvailableHands().contains(identifier)) + if (stateNotification.poll()) { - executeGrip = null; // Clear so they don't get executed later - executeVelToPos = false; - return; + AbilityHandState read = stateNotification.read(); + if (read.getIdentifierAsString().equals(identifier) && read.getHandSide() == handSide.toByte()) + { + latestState = read; + connectedTimer.reset(); + } } - currentPositions = communication.readState(identifier).getActuatorPositions(); + if (!connectedTimer.isRunning(0.5)) + latestState = null; - if ((executeGrip != null || executeVelToPos) && publishThrottler.run()) + if (latestState == null) + { + executeGrip = null; + executeVelToPos = false; + } + else if ((executeGrip != null || executeVelToPos) && commandThrottler.run()) { - AbilityHandCommand command = communication.getCommand(identifier); if (executeGrip != null) { command.setControlMode(AbilityHandControlMode.GRIP.toByte()); @@ -82,7 +101,7 @@ public void update() } for (int i = 0; i < 6; i++) command.getGoalVelocities()[i] = desiredVelocities[i].get(); - communication.publishCommand(identifier); + commandPublisher.publish(command); executeGrip = null; executeVelToPos = false; @@ -92,14 +111,12 @@ public void update() @Override public void renderImGuiWidgets() { - boolean connected = communication.getAvailableHands().contains(identifier); - ImGuiTools.separatorText(getSide().toString() + " Ability Hand", ImGuiTools.getSmallBoldFont()); - if (!connected) + if (latestState == null) ImGuiTools.textColored(Color.RED, "Not connected"); - ImGui.beginDisabled(!connected); + ImGui.beginDisabled(latestState == null); for (AbilityHandGrip grip : AbilityHandGrip.values) { @@ -114,15 +131,15 @@ public void renderImGuiWidgets() { float sliderMin = 0.0f; float sliderMax = i == 5 ? -120.0f : 120.0f; // thumb rotator moves negative - float currentNotch = (currentPositions[i] - sliderMin) / (sliderMax - sliderMin); + float currentNotch = (latestState.getActuatorPositions()[i] - sliderMin) / (sliderMax - sliderMin); float sliderWidth = ImGui.getColumnWidth() * 0.6f; ImGuiTools.renderSliderOrProgressNotch(currentNotch * sliderWidth, ImGui.getColorU32(ImGuiCol.Text)); ImGui.pushItemWidth(sliderWidth); scheduleExecuteVelToPos |= ImGui.sliderFloat(labels.getHidden(FINGER_NAMES[i]), desiredPositions[i].getData(), sliderMin, sliderMax, - "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], currentPositions[i], EuclidCoreMissingTools.DEGREE_SYMBOL)); + "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], latestState.getActuatorPositions()[i], EuclidCoreMissingTools.DEGREE_SYMBOL)); if (!ImGui.isItemActive() && !executeVelToPos) // Prevent overriding externally submitted positions too - desiredPositions[i].set(currentPositions[i]); + desiredPositions[i].set(latestState.getActuatorPositions()[i]); ImGui.popItemWidth(); ImGui.sameLine(); ImGui.pushItemWidth(ImGui.getColumnWidth()); @@ -180,6 +197,6 @@ public void sendFingerPosition(int index, float angleDegrees) @Override public float getFingerPosition(int index) { - return currentPositions[index]; + return latestState.getActuatorPositions()[index]; } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java index 186c38273d1..ee234982918 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java @@ -2,6 +2,7 @@ import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionComms; import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneExecutor; import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeTopologyOperationQueue; import us.ihmc.behaviors.behaviorTree.condition.LLMConditionExecutor; @@ -9,10 +10,11 @@ import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; import us.ihmc.communication.ros2.ROS2ActorDesignation; import us.ihmc.communication.ros2.sync.ROS2PeerClockOffsetEstimator; -import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; import us.ihmc.log.LogTools; import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseCommunicatorMap; import us.ihmc.perception.detections.yolo.YOLOv8DetectionExecutor; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.tools.io.WorkspaceResourceDirectory; import us.ihmc.tools.io.WorkspaceResourceFile; @@ -20,7 +22,7 @@ public class BehaviorTreeExecutor extends BehaviorTree abilityHandComms = new SideDependentList<>(); public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot, ROS2PeerClockOffsetEstimator peerClockEstimator, @@ -35,8 +37,8 @@ public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot, new BehaviorTreeExecutorNodeBuilder()); controllerStatusTracker = new ControllerStatusTracker(new LogToolsLogger(), ros2ControllerHelper.getROS2Node(), robotModel.getSimpleRobotName()); - abilityHandCommunication = new AbilityHandROS2HardwareCommunication("bt_abilty_hand"); - abilityHandCommunication.start(); + for (RobotSide robotSide : RobotSide.values) + abilityHandComms.put(robotSide, new AbilityHandActionComms(robotSide, ros2ControllerHelper.getROS2Node())); scene = new BehaviorTreeSceneExecutor(crdtInfo, this::getAndIncrementNextID, syncedRobot, yolo, foundationPose); setScene(scene); @@ -45,7 +47,7 @@ public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot, ros2ControllerHelper, syncedRobot, controllerStatusTracker, - abilityHandCommunication, + abilityHandComms, scene); } @@ -75,7 +77,6 @@ private void update(BehaviorTreeNodeExecutor node) public void destroy() { - abilityHandCommunication.shutdown(); modifyTreeTopology(BehaviorTreeTopologyOperationQueue::queueDestroyEntireTree); LLMConditionExecutor.destroy(); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java index b40a8fd0afc..6fb0cf06b96 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java @@ -11,7 +11,7 @@ import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneExecutor; import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; import us.ihmc.communication.crdt.CRDTInfo; -import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; +import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.tools.io.WorkspaceResourceDirectory; import java.util.HashMap; @@ -50,7 +50,7 @@ public class BehaviorTreeExecutorNodeBuilder implements BehaviorTreeNodeBuilder< private ROS2ControllerHelper ros2ControllerHelper; private ROS2SyncedRobotModel syncedRobot; private ControllerStatusTracker controllerStatusTracker; - private AbilityHandROS2HardwareCommunication abilityHandCommunication; + private SideDependentList abilityHandComms; private BehaviorTreeSceneExecutor scene; public void initialize(CRDTInfo crdtInfo, @@ -58,7 +58,7 @@ public void initialize(CRDTInfo crdtInfo, ROS2ControllerHelper ros2ControllerHelper, ROS2SyncedRobotModel syncedRobot, ControllerStatusTracker controllerStatusTracker, - AbilityHandROS2HardwareCommunication abilityHandCommunication, + SideDependentList abilityHandComms, BehaviorTreeSceneExecutor scene) { this.crdtInfo = crdtInfo; @@ -66,7 +66,7 @@ public void initialize(CRDTInfo crdtInfo, this.ros2ControllerHelper = ros2ControllerHelper; this.syncedRobot = syncedRobot; this.controllerStatusTracker = controllerStatusTracker; - this.abilityHandCommunication = abilityHandCommunication; + this.abilityHandComms = abilityHandComms; this.scene = scene; } @@ -79,7 +79,7 @@ public BehaviorTreeRootNodeExecutor createRootNode(long id) ros2ControllerHelper, syncedRobot, controllerStatusTracker, - abilityHandCommunication, + abilityHandComms, scene); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeExecutor.java index fef3f65f68c..cc8e2023126 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeExecutor.java @@ -3,9 +3,10 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionComms; import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneExecutor; import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; -import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; +import us.ihmc.robotics.robotSide.SideDependentList; import javax.annotation.Nullable; import java.util.ArrayList; @@ -34,7 +35,7 @@ public class BehaviorTreeNodeExecutor, protected final ROS2ControllerHelper ros2ControllerHelper; protected final ROS2SyncedRobotModel syncedRobot; protected final ControllerStatusTracker controllerStatusTracker; - protected final AbilityHandROS2HardwareCommunication abilityHandCommunication; + protected final SideDependentList abilityHandComms; protected final BehaviorTreeSceneExecutor scene; /** For creating a basic node. */ // TODO: Should not exist??? @@ -53,7 +54,7 @@ public BehaviorTreeNodeExecutor(S state, BehaviorTreeRootNodeExecutor rootNode) this.ros2ControllerHelper = rootNode.getRos2ControllerHelper(); this.syncedRobot = rootNode.getSyncedRobot(); this.controllerStatusTracker = rootNode.getControllerStatusTracker(); - this.abilityHandCommunication = rootNode.getAbilityHandCommunication(); + this.abilityHandComms = rootNode.getAbilityHandComms(); this.scene = rootNode.getScene(); } @@ -62,7 +63,7 @@ public BehaviorTreeNodeExecutor(S state, ROS2ControllerHelper ros2ControllerHelper, ROS2SyncedRobotModel syncedRobot, ControllerStatusTracker controllerStatusTracker, - AbilityHandROS2HardwareCommunication abilityHandCommunication, + SideDependentList abilityHandComms, BehaviorTreeSceneExecutor scene) { this.definition = state.getDefinition(); @@ -72,7 +73,7 @@ public BehaviorTreeNodeExecutor(S state, this.ros2ControllerHelper = ros2ControllerHelper; this.syncedRobot = syncedRobot; this.controllerStatusTracker = controllerStatusTracker; - this.abilityHandCommunication = abilityHandCommunication; + this.abilityHandComms = abilityHandComms; this.scene = scene; } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeRootNodeExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeRootNodeExecutor.java index 8bc92ca96ad..f9f39191f25 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeRootNodeExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeRootNodeExecutor.java @@ -5,11 +5,12 @@ import us.ihmc.avatar.ros2.ROS2ControllerHelper; import us.ihmc.behaviors.behaviorTree.action.ActionNodeExecutor; import us.ihmc.behaviors.behaviorTree.action.ActionNodeState; +import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionComms; import us.ihmc.behaviors.behaviorTree.control.FallbackNodeExecutor; import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneExecutor; import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; import us.ihmc.communication.crdt.CRDTInfo; -import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication; +import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.tools.io.WorkspaceResourceDirectory; import java.util.ArrayList; @@ -31,14 +32,14 @@ public BehaviorTreeRootNodeExecutor(long id, ROS2ControllerHelper ros2ControllerHelper, ROS2SyncedRobotModel syncedRobot, ControllerStatusTracker controllerStatusTracker, - AbilityHandROS2HardwareCommunication abilityHandCommunication, + SideDependentList abilityHandComms, BehaviorTreeSceneExecutor scene) { super(new BehaviorTreeRootNodeState(id, crdtInfo, saveFileDirectory, syncedRobot.getRobotModel(), scene), ros2ControllerHelper, syncedRobot, controllerStatusTracker, - abilityHandCommunication, + abilityHandComms, scene); } @@ -268,9 +269,9 @@ public ControllerStatusTracker getControllerStatusTracker() return controllerStatusTracker; } - public AbilityHandROS2HardwareCommunication getAbilityHandCommunication() + public SideDependentList getAbilityHandComms() { - return abilityHandCommunication; + return abilityHandComms; } public BehaviorTreeSceneExecutor getScene() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionComms.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionComms.java new file mode 100644 index 00000000000..04a87cb560e --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionComms.java @@ -0,0 +1,67 @@ +package us.ihmc.behaviors.behaviorTree.action.actions; + +import ihmc_hands_ros2.msg.dds.AbilityHandCommand; +import ihmc_hands_ros2.msg.dds.AbilityHandState; +import us.ihmc.commons.thread.Throttler; +import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.handsros2.abilityHand.AbilityHandROS2API; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; +import us.ihmc.tools.Timer; + +public class AbilityHandActionComms +{ + private final RobotSide handSide; + private final TypedNotification stateNotification = new TypedNotification<>(); + private AbilityHandState latestState = null; + private final AbilityHandCommand command = new AbilityHandCommand(); + private final ROS2Publisher commandPublisher; + private final Throttler commandThrottler = new Throttler().setFrequency(30.0); + private final Timer connectedTimer = new Timer(); + + public AbilityHandActionComms(RobotSide handSide, ROS2Node ros2Node) + { + this.handSide = handSide; + + ros2Node.createSubscription2(AbilityHandROS2API.STATE_TOPIC, stateNotification::set); + commandPublisher = ros2Node.createPublisher(AbilityHandROS2API.COMMAND_TOPIC); + } + + public void update() + { + if (stateNotification.poll()) + { + AbilityHandState read = stateNotification.read(); + if (read.getHandSide() == handSide.toByte()) + { + latestState = read; + command.setIdentifier(latestState.getIdentifierAsString()); + connectedTimer.reset(); + } + } + + if (!connectedTimer.isRunning(0.5)) + latestState = null; + } + + public AbilityHandCommand getCommand() + { + return command; + } + + public void publishCommand() + { + commandPublisher.publish(command); + } + + public boolean isConnected() + { + return latestState != null; + } + + public AbilityHandState getLatestState() + { + return latestState; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java index 980641ea507..9ee45cf1d1a 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/action/actions/AbilityHandActionExecutor.java @@ -4,8 +4,6 @@ import ihmc_hands_ros2.msg.dds.AbilityHandState; import us.ihmc.behaviors.behaviorTree.BehaviorTreeRootNodeExecutor; import us.ihmc.behaviors.behaviorTree.action.ActionNodeExecutor; -import us.ihmc.handsros2.HandInterface; -import us.ihmc.handsros2.HandType; import us.ihmc.handsros2.abilityHand.AbilityHandControlMode; import us.ihmc.handsros2.abilityHand.AbilityHandGrip; import us.ihmc.robotics.EuclidCoreMissingTools; @@ -34,9 +32,8 @@ public void triggerExecution() state.getLogger().info("Executing Ability Hand action for side: {} with grip {}", definition.getSide(), definition.getGrip()); - AbilityHandCommand command = getCommand(); - AbilityHandState handState = readState(); - if (command != null && handState != null) + AbilityHandActionComms comms = abilityHandComms.get(definition.getSide()); + if (comms.isConnected()) { double nominalExecutionDuration = 4.0; state.setNominalExecutionDuration(nominalExecutionDuration); @@ -44,8 +41,9 @@ public void triggerExecution() state.getCommandedJointTrajectories().clear(6); for (int i = 0; i < 6; i++) - state.getCommandedJointTrajectories().addTrajectoryPoint(i, handState.getActuatorPositions()[i], 0.0); + state.getCommandedJointTrajectories().addTrajectoryPoint(i, comms.getLatestState().getActuatorPositions()[i], 0.0); + AbilityHandCommand command = comms.getCommand(); command.setControlMode(definition.getControlMode().toByte()); if (definition.getControlMode() == AbilityHandControlMode.GRIP) { @@ -74,7 +72,7 @@ public void triggerExecution() } for (int i = 0; i < 6; i++) command.getGoalVelocities()[i] = definition.getGoalVelocities().getValueReadOnly(i); - abilityHandCommunication.publishCommand(identifier); + comms.publishCommand(); timer.reset(); } else @@ -96,7 +94,8 @@ public void updateCurrentlyExecuting() state.setIsExecuting(false); } - AbilityHandState handState = readState(); + AbilityHandActionComms comms = abilityHandComms.get(definition.getSide()); + AbilityHandState handState = comms.getLatestState(); if (handState != null) { // TODO: Look at SCS YoVariable data to inform better strategies @@ -148,20 +147,17 @@ public void updateCurrentlyExecuting() { if (!timer.isRunning(definition.getTimeToWiggle())) { - AbilityHandCommand command = getCommand(); - if (command != null) + AbilityHandCommand command = comms.getCommand(); + command.setControlMode(AbilityHandControlMode.POSITION.toByte()); + for (int i = 0; i < 6; i++) { - command.setControlMode(AbilityHandControlMode.POSITION.toByte()); - for (int i = 0; i < 6; i++) - { - 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()); - command.getGoalVelocities()[i] = definition.getGoalVelocities().getValueReadOnly(i); - } - abilityHandCommunication.publishCommand(identifier); + 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()); + command.getGoalVelocities()[i] = definition.getGoalVelocities().getValueReadOnly(i); } + comms.publishCommand(); } } } @@ -172,20 +168,4 @@ public void updateCurrentlyExecuting() state.setIsExecuting(false); } } - - private String updateIdentifier() - { - identifier = HandInterface.getSimpleIdentifier(robotModel.getSimpleRobotName(), definition.getSide(), HandType.ABILITY_HAND); - return identifier; - } - - private AbilityHandCommand getCommand() - { - return abilityHandCommunication.getAvailableHands().contains(updateIdentifier()) ? abilityHandCommunication.getCommand(identifier) : null; - } - - private AbilityHandState readState() - { - return abilityHandCommunication.getAvailableHands().contains(updateIdentifier()) ? abilityHandCommunication.readState(identifier) : null; - } } From d8c1325d7b9c06f5b657f8b66a5389ea126a37a4 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Sat, 29 Nov 2025 10:28:47 -0600 Subject: [PATCH 6/8] Fix NPE. --- .../rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java index 8ac2493e105..cd944f4b8e8 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java @@ -131,15 +131,16 @@ public void renderImGuiWidgets() { float sliderMin = 0.0f; float sliderMax = i == 5 ? -120.0f : 120.0f; // thumb rotator moves negative - float currentNotch = (latestState.getActuatorPositions()[i] - sliderMin) / (sliderMax - sliderMin); + float actuatorPosition = latestState == null ? Float.NaN : latestState.getActuatorPositions()[i]; + float currentNotch = (actuatorPosition - sliderMin) / (sliderMax - sliderMin); float sliderWidth = ImGui.getColumnWidth() * 0.6f; ImGuiTools.renderSliderOrProgressNotch(currentNotch * sliderWidth, ImGui.getColorU32(ImGuiCol.Text)); ImGui.pushItemWidth(sliderWidth); scheduleExecuteVelToPos |= ImGui.sliderFloat(labels.getHidden(FINGER_NAMES[i]), desiredPositions[i].getData(), sliderMin, sliderMax, - "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], latestState.getActuatorPositions()[i], EuclidCoreMissingTools.DEGREE_SYMBOL)); + "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], actuatorPosition, EuclidCoreMissingTools.DEGREE_SYMBOL)); if (!ImGui.isItemActive() && !executeVelToPos) // Prevent overriding externally submitted positions too - desiredPositions[i].set(latestState.getActuatorPositions()[i]); + desiredPositions[i].set(actuatorPosition); ImGui.popItemWidth(); ImGui.sameLine(); ImGui.pushItemWidth(ImGui.getColumnWidth()); From 36548f308b8985d3dc5bfe6b5cd51a89a2f1f0fb Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 3 Dec 2025 13:25:49 -0600 Subject: [PATCH 7/8] Back to working but slow position control. --- .../us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java index cd944f4b8e8..15ee49eb33f 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java @@ -140,7 +140,7 @@ public void renderImGuiWidgets() scheduleExecuteVelToPos |= ImGui.sliderFloat(labels.getHidden(FINGER_NAMES[i]), desiredPositions[i].getData(), sliderMin, sliderMax, "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], actuatorPosition, EuclidCoreMissingTools.DEGREE_SYMBOL)); if (!ImGui.isItemActive() && !executeVelToPos) // Prevent overriding externally submitted positions too - desiredPositions[i].set(actuatorPosition); + desiredPositions[i].set(latestState.getGoalPositions()[i]); ImGui.popItemWidth(); ImGui.sameLine(); ImGui.pushItemWidth(ImGui.getColumnWidth()); From d665206d812b2ea656342b021c4d3b1f54083ec5 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 3 Dec 2025 13:50:11 -0600 Subject: [PATCH 8/8] Simplify hand control code. Fix UI bug. --- .../us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java index 15ee49eb33f..1a86ee50c36 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java @@ -139,7 +139,7 @@ public void renderImGuiWidgets() ImGui.pushItemWidth(sliderWidth); scheduleExecuteVelToPos |= ImGui.sliderFloat(labels.getHidden(FINGER_NAMES[i]), desiredPositions[i].getData(), sliderMin, sliderMax, "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], actuatorPosition, EuclidCoreMissingTools.DEGREE_SYMBOL)); - if (!ImGui.isItemActive() && !executeVelToPos) // Prevent overriding externally submitted positions too + if (!ImGui.isItemActive() && !executeVelToPos && latestState != null) // Prevent overriding externally submitted positions too desiredPositions[i].set(latestState.getGoalPositions()[i]); ImGui.popItemWidth(); ImGui.sameLine();