From d791883b1358a27646a9457d004b35707025149b Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 18 Jun 2025 17:42:21 -0500 Subject: [PATCH 1/3] Start on position passthrough state. --- .../PositionPassthroughControllerState.java | 79 +++++++++++++++++++ .../dataobjects/HighLevelControllerName.java | 4 +- 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/PositionPassthroughControllerState.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/PositionPassthroughControllerState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/PositionPassthroughControllerState.java new file mode 100644 index 000000000000..1bdeb5d3515b --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/PositionPassthroughControllerState.java @@ -0,0 +1,79 @@ +package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates; + +import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters; +import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.RootJointDesiredConfigurationData; +import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.RootJointDesiredConfigurationDataReadOnly; +import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.YoLowLevelOneDoFJointDesiredDataHolder; +import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; +import us.ihmc.communication.controllerAPI.CommandInputManager; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ArmTrajectoryCommand; +import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; +import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly; + +import java.util.List; + +/** + * This controller state accepts controller API commands, manages the trajectories, but + * passes the desired positions directly to the joint desired output list. + */ +public class PositionPassthroughControllerState extends HighLevelControllerState +{ + private final static HighLevelControllerName controllerState = HighLevelControllerName.POSITION_PASSTHROUGH; + private final CommandInputManager commandInputManager; + private final YoLowLevelOneDoFJointDesiredDataHolder jointDesiredDataHolder; + /** not used **/ + private final RootJointDesiredConfigurationData rootJointDesiredConfigurationData = new RootJointDesiredConfigurationData(); + + public PositionPassthroughControllerState(CommandInputManager commandInputManager, + HighLevelHumanoidControllerToolbox controllerToolbox, + HighLevelControllerParameters highLevelControllerParameters) + { + super(controllerState, highLevelControllerParameters, controllerToolbox.getControlledOneDoFJoints()); + + this.commandInputManager = commandInputManager; + + jointDesiredDataHolder = new YoLowLevelOneDoFJointDesiredDataHolder(controllerToolbox.getControlledOneDoFJoints(), + controllerToolbox.getYoVariableRegistry()); + } + + @Override + public void onEntry() + { + + } + + @Override + public void doAction(double timeInState) + { + List armTrajectoryCommands = commandInputManager.pollNewCommands(ArmTrajectoryCommand.class); + + for (ArmTrajectoryCommand armTrajectoryCommand : armTrajectoryCommands) + { + for (int i = 0; i < armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints(); i++) + { + +// jointDesiredDataHolder.setDesiredJointPosition(); + } + } + + + } + + @Override + public void onExit(double timeInState) + { + + } + + @Override + public JointDesiredOutputListReadOnly getOutputForLowLevelController() + { + return jointDesiredDataHolder; + } + + @Override + public RootJointDesiredConfigurationDataReadOnly getOutputForRootJoint() + { + return rootJointDesiredConfigurationData; + } +} diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/dataobjects/HighLevelControllerName.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/dataobjects/HighLevelControllerName.java index d084c1da5a13..8992b71af88b 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/dataobjects/HighLevelControllerName.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/dataobjects/HighLevelControllerName.java @@ -34,7 +34,9 @@ public enum HighLevelControllerName @RosEnumValueDocumentation(documentation = "whole body force control employing IHMC fast walking algorithms") QUICKSTER, EXTERNAL_TRANSITION_STATE, - EXTERNAL; + EXTERNAL, + @RosEnumValueDocumentation(documentation = "Accepts controller API commands, manages the trajectories, but passes the desired positions directly to the joint desired output list.") + POSITION_PASSTHROUGH; public static final HighLevelControllerName[] values = values(); From 4b63a775e230ea8fbbaeb04086673f7b230c4c0e Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 18 Jun 2025 17:52:34 -0500 Subject: [PATCH 2/3] Setup custom control state. --- ...tionPassthroughControllerStateFactory.java | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/PositionPassthroughControllerStateFactory.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/PositionPassthroughControllerStateFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/PositionPassthroughControllerStateFactory.java new file mode 100644 index 000000000000..dafff3738b62 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/PositionPassthroughControllerStateFactory.java @@ -0,0 +1,30 @@ +package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories; + +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper; +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState; +import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.PositionPassthroughControllerState; +import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; + +public class PositionPassthroughControllerStateFactory implements HighLevelControllerStateFactory +{ + private PositionPassthroughControllerState positionPassthroughControllerState; + + @Override + public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper) + { + if (positionPassthroughControllerState == null) + { + positionPassthroughControllerState = new PositionPassthroughControllerState(controllerFactoryHelper.getCommandInputManager(), + controllerFactoryHelper.getHighLevelHumanoidControllerToolbox(), + controllerFactoryHelper.getHighLevelControllerParameters()); + } + + return positionPassthroughControllerState; + } + + @Override + public HighLevelControllerName getStateEnum() + { + return HighLevelControllerName.POSITION_PASSTHROUGH; + } +} From 99c26783d84734631439ad22da5f36efca68d1fd Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Fri, 20 Jun 2025 12:15:32 -0500 Subject: [PATCH 3/3] Fix join for non-realtime. --- .../wholeBodyHardwareControl/AvatarMultiThreadingManager.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingManager.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingManager.java index 804f2502668f..9ac461ccab64 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingManager.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingManager.java @@ -10,6 +10,7 @@ import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData; import us.ihmc.commons.Conversions; import us.ihmc.commons.exception.DefaultExceptionHandler; +import us.ihmc.commons.exception.ExceptionTools; import us.ihmc.commons.thread.RepeatingTaskThread; import us.ihmc.commons.thread.ThreadTools; import us.ihmc.commons.time.FrequencyCalculator; @@ -262,6 +263,8 @@ public void join() { if (useRealtimeThreads) ((RealtimeThread) masterThread).join(); + else + ExceptionTools.handle(() -> ((RepeatingTaskThread) masterThread).join(), DefaultExceptionHandler.MESSAGE_AND_STACKTRACE); } public void stop()