[WIP]Feature/wbcc separate thread

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarControllerThread.java

@@ -1,17 +1,14 @@
 package us.ihmc.avatar;
 
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-import java.util.function.Supplier;
-
 import controller_msgs.msg.dds.ControllerCrashNotificationPacket;
 import us.ihmc.avatar.drcRobot.DRCRobotModel;
 import us.ihmc.avatar.initialSetup.RobotInitialSetup;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextJointData;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextTools;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.ControllerCoreCommandDataHolder;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.ControllerCoreOutputDataHolder;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
 import us.ihmc.commonWalkingControlModules.corruptors.FullRobotModelCorruptor;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HumanoidHighLevelControllerManager;
@@ -20,7 +17,6 @@
 import us.ihmc.commonWalkingControlModules.visualizer.InverseDynamicsMechanismReferenceFrameVisualizer;
 import us.ihmc.commons.Conversions;
 import us.ihmc.communication.HumanoidControllerAPI;
-import us.ihmc.ros2.ROS2PublisherBasics;
 import us.ihmc.communication.packets.ControllerCrashLocation;
 import us.ihmc.communication.packets.MessageTools;
 import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
@@ -34,6 +30,7 @@
 import us.ihmc.robotics.sensors.ForceSensorDataHolder;
 import us.ihmc.robotics.sensors.ForceSensorDataHolderReadOnly;
 import us.ihmc.robotics.time.ExecutionTimer;
+import us.ihmc.ros2.ROS2PublisherBasics;
 import us.ihmc.ros2.RealtimeROS2Node;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
@@ -56,6 +53,11 @@
 import us.ihmc.yoVariables.variable.YoDouble;
 import us.ihmc.yoVariables.variable.YoLong;
 
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.Supplier;
+
 public class AvatarControllerThread implements AvatarControllerThreadInterface
 {
    private static final boolean CREATE_DYNAMICALLY_CONSISTENT_NULLSPACE_EVALUATOR = false;
@@ -87,6 +89,7 @@ public class AvatarControllerThread implements AvatarControllerThreadInterface
    private final HumanoidRobotContextData humanoidRobotContextData;
 
    private final ExecutionTimer controllerThreadTimer;
+   private final boolean wbccThreadRun;
 
    public AvatarControllerThread(String robotName,
                                  DRCRobotModel robotModel,
@@ -97,9 +100,11 @@ public AvatarControllerThread(String robotName,
                                  DRCOutputProcessor outputProcessor,
                                  RealtimeROS2Node realtimeROS2Node,
                                  double gravity,
-                                 boolean kinematicsSimulation)
+                                 boolean kinematicsSimulation,
+                                 boolean wbccThreadRun)
    {
       controllerFullRobotModel = robotModel.createFullRobotModel();
+      this.wbccThreadRun = wbccThreadRun;
       if (robotInitialSetup != null)
       {
          robotInitialSetup.initializeFullRobotModel(controllerFullRobotModel);
@@ -110,13 +115,19 @@ public AvatarControllerThread(String robotName,
       CenterOfMassDataHolder centerOfMassDataHolderForController = new CenterOfMassDataHolder();
       CenterOfPressureDataHolder centerOfPressureDataHolderForEstimator = new CenterOfPressureDataHolder(controllerFullRobotModel);
       LowLevelOneDoFJointDesiredDataHolder desiredJointDataHolder = new LowLevelOneDoFJointDesiredDataHolder(controllerFullRobotModel.getControllableOneDoFJoints());
+      LowLevelOneDoFJointDesiredDataHolder wholeBodyControllerCoreDesiredJointDataHolder = new LowLevelOneDoFJointDesiredDataHolder(controllerFullRobotModel.getControllableOneDoFJoints());
+      ControllerCoreOutputDataHolder controllerCoreOutPutDataHolder = new ControllerCoreOutputDataHolder(controllerFullRobotModel.getControllableOneDoFJoints());
+      ControllerCoreCommandDataHolder controllerCoreCommandDataHolder = new ControllerCoreCommandDataHolder();
       RobotMotionStatusHolder robotMotionStatusHolder = new RobotMotionStatusHolder();
       contextDataFactory.setForceSensorDataHolder(forceSensorDataHolderForController);
       contextDataFactory.setCenterOfMassDataHolder(centerOfMassDataHolderForController);
       contextDataFactory.setCenterOfPressureDataHolder(centerOfPressureDataHolderForEstimator);
       contextDataFactory.setRobotMotionStatusHolder(robotMotionStatusHolder);
       contextDataFactory.setJointDesiredOutputList(desiredJointDataHolder);
+      contextDataFactory.setWBCCJointDesiredOutputList(wholeBodyControllerCoreDesiredJointDataHolder);
       contextDataFactory.setProcessedJointData(processedJointData);
+      contextDataFactory.setControllerCoreOutputDataHolder(controllerCoreOutPutDataHolder);
+      contextDataFactory.setControllerCoreCommandDataHolder(controllerCoreCommandDataHolder);
       contextDataFactory.setSensorDataContext(new SensorDataContext(controllerFullRobotModel));
       humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData();
 
@@ -149,7 +160,9 @@ public AvatarControllerThread(String robotName,
                                                   centerOfMassDataHolderForController,
                                                   centerOfPressureDataHolderForEstimator,
                                                   sensorInformation,
-                                                  desiredJointDataHolder,
+                                                  wholeBodyControllerCoreDesiredJointDataHolder,
+                                                  controllerCoreOutPutDataHolder,
+                                                  controllerCoreCommandDataHolder,
                                                   yoGraphicsListRegistry,
                                                   registry,
                                                   kinematicsSimulation,
@@ -161,12 +174,13 @@ public AvatarControllerThread(String robotName,
 
       firstTick.set(true);
       registry.addChild(robotController.getYoRegistry());
-      if (outputProcessor != null)
-      {
-         outputProcessor.setLowLevelControllerCoreOutput(processedJointData, desiredJointDataHolder);
-         outputProcessor.setForceSensorDataHolderForController(forceSensorDataHolderForController);
-         registry.addChild(outputProcessor.getControllerYoVariableRegistry());
-      }
+      //TODO looks like should be moved to "new" thread
+//      if (outputProcessor != null)
+//      {
+//         outputProcessor.setLowLevelControllerCoreOutput(processedJointData, desiredJointDataHolder);
+//         outputProcessor.setForceSensorDataHolderForController(forceSensorDataHolderForController);
+//         registry.addChild(outputProcessor.getControllerYoVariableRegistry());
+//      }
 
       ParameterLoaderHelper.loadParameters(this, robotModel, registry);
    }
@@ -224,7 +238,9 @@ private ModularRobotController createHighLevelController(FullHumanoidRobotModel
                                                             CenterOfMassDataHolderReadOnly centerOfMassDataHolderForController,
                                                             CenterOfPressureDataHolder centerOfPressureDataHolderForEstimator,
                                                             HumanoidRobotSensorInformation sensorInformation,
-                                                            JointDesiredOutputListBasics lowLevelControllerOutput,
+                                                            JointDesiredOutputListBasics wholeBodyControllerCoreOutput,
+                                                            ControllerCoreOutputDataHolder controllerCoreOutPutDataHolder,
+                                                            ControllerCoreCommandDataHolder controllerCoreCommandDataHolder,
                                                             YoGraphicsListRegistry yoGraphicsListRegistry,
                                                             YoRegistry registry,
                                                             boolean kinematicsSimulation,
@@ -256,7 +272,9 @@ private ModularRobotController createHighLevelController(FullHumanoidRobotModel
                                                                                            forceSensorDataHolderForController,
                                                                                            centerOfMassDataHolderForController,
                                                                                            centerOfPressureDataHolderForEstimator,
-                                                                                           lowLevelControllerOutput,
+                                                                                           wholeBodyControllerCoreOutput,
+                                                                                           controllerCoreOutPutDataHolder,
+                                                                                           controllerCoreCommandDataHolder,
                                                                                            jointsToIgnore);
       scs2YoGraphicHolders.add(() -> robotController.getSCS2YoGraphics());
 
@@ -301,13 +319,20 @@ public void initialize()
       firstTick.set(true);
       humanoidRobotContextData.setControllerRan(false);
       humanoidRobotContextData.setEstimatorRan(false);
+      humanoidRobotContextData.setWholeBodyControllerCoreRan(false);
 
-      LowLevelOneDoFJointDesiredDataHolder jointDesiredOutputList = humanoidRobotContextData.getJointDesiredOutputList();
+      //TODO this should be removed.
+      // New Thread (WBCCThread) will replace this action
+      // in the EsitmatorThread, the initialize method clear the jointDesiredOutputList. So, let it be here.
 
-      for (int i = 0; i < jointDesiredOutputList.getNumberOfJointsWithDesiredOutput(); i++)
+//      LowLevelOneDoFJointDesiredDataHolder jointDesiredOutputList = humanoidRobotContextData.getJointDesiredOutputList();
+      LowLevelOneDoFJointDesiredDataHolder wholeBodyControllerCoreOutPutList = humanoidRobotContextData.getWholeBodyControllerCoreDesiredOutPutList();
+
+      for (int i = 0; i < wholeBodyControllerCoreOutPutList.getNumberOfJointsWithDesiredOutput(); i++)
       {
-         jointDesiredOutputList.getJointDesiredOutput(i).clear();
+         wholeBodyControllerCoreOutPutList.getJointDesiredOutput(i).clear();
       }
+
    }
 
    @Override

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java

@@ -1,12 +1,5 @@
 package us.ihmc.avatar;
 
-import java.io.InputStream;
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collection;
-import java.util.List;
-import java.util.function.BooleanSupplier;
-
 import controller_msgs.msg.dds.ControllerCrashNotificationPacket;
 import controller_msgs.msg.dds.RequestWristForceSensorCalibrationPacket;
 import controller_msgs.msg.dds.RobotConfigurationData;
@@ -15,12 +8,13 @@
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextJointData;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.ControllerCoreCommandDataHolder;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.ControllerCoreOutputDataHolder;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.ContactableBodiesFactory;
 import us.ihmc.commons.Conversions;
 import us.ihmc.communication.HumanoidControllerAPI;
 import us.ihmc.communication.controllerAPI.ControllerAPI;
-import us.ihmc.ros2.ROS2PublisherBasics;
 import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.BarrierScheduler;
 import us.ihmc.euclid.geometry.LineSegment2D;
 import us.ihmc.euclid.transform.RigidBodyTransform;
@@ -34,11 +28,8 @@
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SegmentDependentList;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.sensors.CenterOfMassDataHolder;
-import us.ihmc.robotics.sensors.ForceSensorDataHolder;
-import us.ihmc.robotics.sensors.ForceSensorDataHolderReadOnly;
-import us.ihmc.robotics.sensors.ForceSensorDefinition;
-import us.ihmc.robotics.sensors.IMUDefinition;
+import us.ihmc.robotics.sensors.*;
+import us.ihmc.ros2.ROS2PublisherBasics;
 import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.ros2.RealtimeROS2Node;
 import us.ihmc.sensorProcessing.communication.producers.RobotConfigurationDataPublisher;
@@ -71,6 +62,13 @@
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
 
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+
 public class AvatarEstimatorThreadFactory
 {
    private final YoRegistry estimatorRegistry = new YoRegistry("DRCEstimatorThread");
@@ -103,7 +101,7 @@ public class AvatarEstimatorThreadFactory
    private final OptionalFactoryField<HumanoidRobotContextData> humanoidRobotContextDataField = new OptionalFactoryField<>("humanoidRobotContextData");
    private final OptionalFactoryField<HumanoidRobotContextJointData> humanoidRobotContextJointDataField = new OptionalFactoryField<>("humanoidRobotContextJointData");
    private final OptionalFactoryField<LowLevelOneDoFJointDesiredDataHolder> desiredJointDataHolderField = new OptionalFactoryField<>("desiredJointDataHolder");
-
+   private final OptionalFactoryField<LowLevelOneDoFJointDesiredDataHolder> wbccDesiredJointDataHolderField = new OptionalFactoryField<>("desiredJointDataHolder");
    private final OptionalFactoryField<FloatingJointBasics> rootJointField = new OptionalFactoryField<>("rootJoint");
    private final OptionalFactoryField<OneDoFJointBasics[]> oneDoFJointsField = new OptionalFactoryField<>("oneDoFJoints");
    private final OptionalFactoryField<OneDoFJointBasics[]> controllableOneDoFJointsField = new OptionalFactoryField<>("controllableOneDoFJoints");
@@ -113,6 +111,8 @@ public class AvatarEstimatorThreadFactory
    private final OptionalFactoryField<ForceSensorDataHolder> forceSensorDataHolderField = new OptionalFactoryField<>("forceSensorDataHolder");
    private final OptionalFactoryField<CenterOfMassDataHolder> centerOfMassDataHolderField = new OptionalFactoryField<>("centerOfMassDataHolder");
    private final OptionalFactoryField<ForceSensorDefinition[]> forceSensorDefinitionsField = new OptionalFactoryField<>("forceSensorDefinitionsField");
+   private final OptionalFactoryField<ControllerCoreOutputDataHolder> controllerCoreOutputDataHolderField = new OptionalFactoryField<>("wholeBodyControllerCoreDataHolder");
+   private final OptionalFactoryField<ControllerCoreCommandDataHolder> controllerCoreCommandDataHolderField = new OptionalFactoryField<>("controllerCoreCommandDataHolder");
    private final OptionalFactoryField<IMUDefinition[]> imuDefinitionsField = new OptionalFactoryField<>("imuDefinitions");
 
    private final OptionalFactoryField<ContactableBodiesFactory<RobotSide>> contactableBodiesFactoryField = new OptionalFactoryField<>("contactableBodiesFactory");
@@ -169,7 +169,6 @@ public void setGravity(double gravity)
     * </ul>
     *
     * @param robotModel        the robot model used to configure this factory.
-    * @param robotInitialSetup
     */
    public void configureWithDRCRobotModel(DRCRobotModel robotModel)
    {
@@ -589,6 +588,9 @@ public HumanoidRobotContextData getHumanoidRobotContextData()
          contextDataFactory.setJointDesiredOutputList(getDesiredJointDataHolder());
          contextDataFactory.setProcessedJointData(getHumanoidRobotContextJointData());
          contextDataFactory.setSensorDataContext(getSensorDataContext());
+         contextDataFactory.setWBCCJointDesiredOutputList(getWBCCDesiredJointDataHolder());
+         contextDataFactory.setControllerCoreOutputDataHolder(getControllerCoreOutPutDataHolder());
+         contextDataFactory.setControllerCoreCommandDataHolder(getControllerCoreCommandDataHolder());
          humanoidRobotContextDataField.set(contextDataFactory.createHumanoidRobotContextData());
       }
       return humanoidRobotContextDataField.get();
@@ -600,6 +602,24 @@ public LowLevelOneDoFJointDesiredDataHolder getDesiredJointDataHolder()
          desiredJointDataHolderField.set(new LowLevelOneDoFJointDesiredDataHolder(getControllableOneDoFJoints()));
       return desiredJointDataHolderField.get();
    }
+   public LowLevelOneDoFJointDesiredDataHolder getWBCCDesiredJointDataHolder()
+   {
+      if (!wbccDesiredJointDataHolderField.hasValue())
+         wbccDesiredJointDataHolderField.set(new LowLevelOneDoFJointDesiredDataHolder(getControllableOneDoFJoints()));
+      return wbccDesiredJointDataHolderField.get();
+   }
+   public ControllerCoreOutputDataHolder getControllerCoreOutPutDataHolder()
+   {
+      if(!controllerCoreOutputDataHolderField.hasValue())
+         controllerCoreOutputDataHolderField.set(new ControllerCoreOutputDataHolder(getControllableOneDoFJoints()));
+      return controllerCoreOutputDataHolderField.get();
+   }
+   public ControllerCoreCommandDataHolder getControllerCoreCommandDataHolder()
+   {
+      if(!controllerCoreCommandDataHolderField.hasValue())
+         controllerCoreCommandDataHolderField.set(new ControllerCoreCommandDataHolder());
+      return controllerCoreCommandDataHolderField.get();
+   }
 
    public RobotMotionStatusHolder getRobotMotionStatusFromController()
    {