Stream IK streaming through to H1 position controller

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()

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;
+   }
+}

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<ArmTrajectoryCommand> 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;
+   }
+}

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();