diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCControllerTask.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCControllerTask.java index 0ba432c96c1..c413ced18f3 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCControllerTask.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCControllerTask.java @@ -1,7 +1,7 @@ package us.ihmc.avatar; -import us.ihmc.avatar.factory.HumanoidRobotControlTask; -import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData; +import us.ihmc.avatar.factory.MPCHumanoidRobotControlTask; +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; import us.ihmc.commonWalkingControlModules.controllerCore.command.CrossRobotCommandResolver; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.time.ThreadTimer; @@ -10,7 +10,7 @@ import java.util.ArrayList; import java.util.List; -public class MPCControllerTask extends HumanoidRobotControlTask +public class MPCControllerTask extends MPCHumanoidRobotControlTask { private final CrossRobotCommandResolver controllerResolver; private final CrossRobotCommandResolver masterResolver; @@ -24,7 +24,11 @@ public class MPCControllerTask extends HumanoidRobotControlTask protected final List postControllerCallbacks = new ArrayList<>(); protected final List schedulerThreadRunnables = new ArrayList<>(); - public MPCControllerTask(String prefix, AvatarControllerThreadInterface controllerThread, long divisor, double schedulerDt, FullHumanoidRobotModel masterFullRobotModel) + public MPCControllerTask(String prefix, + AvatarControllerThreadInterface controllerThread, + long divisor, + double schedulerDt, + FullHumanoidRobotModel masterFullRobotModel) { super(divisor); this.divisor = divisor; @@ -33,7 +37,7 @@ public MPCControllerTask(String prefix, AvatarControllerThreadInterface controll controllerResolver = new CrossRobotCommandResolver(controllerThread.getFullRobotModel()); masterResolver = new CrossRobotCommandResolver(masterFullRobotModel); -// String prefix = "Controller"; + // String prefix = "Controller"; timer = new ThreadTimer(prefix, schedulerDt * divisor, controllerThread.getYoVariableRegistry()); ticksBehindScheduled = new YoLong(prefix + "TicksBehindScheduled", controllerThread.getYoVariableRegistry()); } @@ -58,15 +62,13 @@ protected void execute() timer.stop(); } - @Override - protected void updateMasterContext(HumanoidRobotContextData masterContext) + protected void updateMasterContext(HumanoidRobotMPCContextData masterContext) { runAll(schedulerThreadRunnables); masterResolver.resolveHumanoidRobotContextDataController(controllerThread.getHumanoidRobotContextData(), masterContext); } - @Override - protected void updateLocalContext(HumanoidRobotContextData masterContext) + protected void updateLocalContext(HumanoidRobotMPCContextData masterContext) { controllerResolver.resolveHumanoidRobotContextDataScheduler(masterContext, controllerThread.getHumanoidRobotContextData()); controllerResolver.resolveHumanoidRobotContextDataEstimator(masterContext, controllerThread.getHumanoidRobotContextData()); @@ -83,5 +85,4 @@ public void addRunnableOnSchedulerThread(Runnable runnable) { schedulerThreadRunnables.add(runnable); } - } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCEstimatorTask.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCEstimatorTask.java index 4f8b7288a07..cc4e0028087 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCEstimatorTask.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCEstimatorTask.java @@ -1,7 +1,7 @@ package us.ihmc.avatar; -import us.ihmc.avatar.factory.HumanoidRobotControlTask; -import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData; +import us.ihmc.avatar.factory.MPCHumanoidRobotControlTask; +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; import us.ihmc.commonWalkingControlModules.controllerCore.command.CrossRobotCommandResolver; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.time.ThreadTimer; @@ -10,7 +10,7 @@ import java.util.ArrayList; import java.util.List; -public class MPCEstimatorTask extends HumanoidRobotControlTask +public class MPCEstimatorTask extends MPCHumanoidRobotControlTask { private final CrossRobotCommandResolver estimatorResolver; private final CrossRobotCommandResolver masterResolver; @@ -66,16 +66,14 @@ protected void execute() timer.stop(); } - @Override - protected void updateMasterContext(HumanoidRobotContextData masterContext) + protected void updateMasterContext(HumanoidRobotMPCContextData masterContext) { runAll(schedulerThreadRunnables); masterResolver.resolveHumanoidRobotContextDataEstimator(estimatorThread.getHumanoidRobotContextData(), masterContext); masterContextUpdated = true; } - @Override - protected void updateLocalContext(HumanoidRobotContextData masterContext) + protected void updateLocalContext(HumanoidRobotMPCContextData masterContext) { estimatorResolver.resolveHumanoidRobotContextDataScheduler(masterContext, estimatorThread.getHumanoidRobotContextData()); estimatorResolver.resolveHumanoidRobotContextDataController(masterContext, estimatorThread.getHumanoidRobotContextData()); @@ -98,5 +96,4 @@ public void addRunnableOnSchedulerThread(Runnable runnable) { schedulerThreadRunnables.add(runnable); } - } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCStepGeneratorTask.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCStepGeneratorTask.java index 759e9daaccd..d6b6a8d8fec 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCStepGeneratorTask.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCStepGeneratorTask.java @@ -1,17 +1,21 @@ package us.ihmc.avatar; -import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData; +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; import us.ihmc.robotModels.FullHumanoidRobotModel; public class MPCStepGeneratorTask extends MPCControllerTask { - public MPCStepGeneratorTask(String prefix, AvatarControllerThreadInterface controllerThread, long divisor, double schedulerDt, FullHumanoidRobotModel masterFullRobotModel) + public MPCStepGeneratorTask(String prefix, + AvatarControllerThreadInterface controllerThread, + long divisor, + double schedulerDt, + FullHumanoidRobotModel masterFullRobotModel) { super(prefix, controllerThread, divisor, schedulerDt, masterFullRobotModel); } @Override - protected void updateMasterContext(HumanoidRobotContextData masterContext) + protected void updateMasterContext(HumanoidRobotMPCContextData masterContext) { runAll(schedulerThreadRunnables); } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCWholeBodyControllerCoreTask.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCWholeBodyControllerCoreTask.java index dbf83b64999..091ebdf3e89 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCWholeBodyControllerCoreTask.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCWholeBodyControllerCoreTask.java @@ -1,7 +1,7 @@ package us.ihmc.avatar; -import us.ihmc.avatar.factory.HumanoidRobotControlTask; -import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData; +import us.ihmc.avatar.factory.MPCHumanoidRobotControlTask; +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; import us.ihmc.commonWalkingControlModules.controllerCore.command.CrossRobotCommandResolver; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.time.ThreadTimer; @@ -10,7 +10,7 @@ import java.util.ArrayList; import java.util.List; -public class MPCWholeBodyControllerCoreTask extends HumanoidRobotControlTask +public class MPCWholeBodyControllerCoreTask extends MPCHumanoidRobotControlTask { private final CrossRobotCommandResolver wholeBodyControllerCoreResolver; private final CrossRobotCommandResolver masterResolver; @@ -22,7 +22,6 @@ public class MPCWholeBodyControllerCoreTask extends HumanoidRobotControlTask protected final List postWholeBodyControllerCoreCallbacks = new ArrayList<>(); - public MPCWholeBodyControllerCoreTask(String prefix, AvatarControllerThreadInterface wholeBodyControllerCoreThread, long divisor, @@ -47,6 +46,7 @@ protected boolean initialize() ticksBehindScheduled.set(0); return super.initialize(); } + @Override protected void execute() { @@ -58,17 +58,16 @@ protected void execute() timer.stop(); } - @Override - protected void updateMasterContext(HumanoidRobotContextData context) + protected void updateMasterContext(HumanoidRobotMPCContextData context) { } - @Override - protected void updateLocalContext(HumanoidRobotContextData context) + protected void updateLocalContext(HumanoidRobotMPCContextData context) { } + @Override public void addCallbackPostTask(Runnable runnable) { diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCBarrierScheduledRobotController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCBarrierScheduledRobotController.java new file mode 100644 index 00000000000..8691a630ab6 --- /dev/null +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCBarrierScheduledRobotController.java @@ -0,0 +1,83 @@ +package us.ihmc.avatar.factory; + +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; +import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.BarrierScheduler; +import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.BarrierScheduler.TaskOverrunBehavior; +import us.ihmc.robotics.time.ThreadTimer; +import us.ihmc.yoVariables.registry.YoRegistry; + +import java.util.List; + +public class MPCBarrierScheduledRobotController implements DisposableRobotController +{ + private final YoRegistry registry; + private final BarrierScheduler barrierScheduler; + private final HumanoidRobotMPCContextData masterContext; + + private final ThreadTimer timer; + private final List tasks; + + public MPCBarrierScheduledRobotController(String name, + List tasks, + HumanoidRobotMPCContextData masterContext, + TaskOverrunBehavior overrunBehavior, + double schedulerDt) + { + this.tasks = tasks; + this.masterContext = masterContext; + + barrierScheduler = new BarrierScheduler<>(tasks, masterContext, overrunBehavior); + registry = new YoRegistry(name); + + timer = new ThreadTimer("Scheduler", schedulerDt, registry); + } + + @Override + public void initialize() + { + // Reset the internal counter to observe when the tasks get triggered. + timer.reset(); + // Reset the scheduler counter so the threads restart as if we started for the first time. + barrierScheduler.reset(); + for (int i = 0; i < tasks.size(); i++) + { + tasks.get(i).initialize(); + } + masterContext.setControllerRan(false); + masterContext.setEstimatorRan(false); + masterContext.setWholeBodyControllerCoreRan(false); + } + + @Override + public YoRegistry getYoRegistry() + { + return registry; + } + + @Override + public void doControl() + { + timer.start(); + masterContext.setSchedulerTick(timer.getTickCount()); + barrierScheduler.run(); + timer.stop(); + } + + public void waitUntilTasksDone() + { + try + { + barrierScheduler.waitUntilTasksDone(); + } + catch (InterruptedException e) + { + e.printStackTrace(); + } + } + + @Override + public void dispose() + { + barrierScheduler.shutdown(); + } +} diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCHumanoidRobotControlTask.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCHumanoidRobotControlTask.java new file mode 100644 index 00000000000..e0bb39e22b3 --- /dev/null +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCHumanoidRobotControlTask.java @@ -0,0 +1,59 @@ +package us.ihmc.avatar.factory; + +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; +import us.ihmc.concurrent.runtime.barrierScheduler.implicitContext.Task; + +import java.util.List; + +public abstract class MPCHumanoidRobotControlTask extends Task +{ + public MPCHumanoidRobotControlTask(long divisor) + { + super(divisor); + } + + @Override + protected boolean initialize() + { + return true; + } + + @Override + protected void cleanup() + { + } + + /** + * The given callback will be run on the task thread right before each control tick. + */ + public void addCallbackPreTask(Runnable callback) + { + throw new UnsupportedOperationException(getClass().getSimpleName() + " does not support this operation."); + } + + /** + * The given callback will be run on the task thread right after each control tick. + */ + public void addCallbackPostTask(Runnable callback) + { + throw new UnsupportedOperationException(getClass().getSimpleName() + " does not support this operation."); + } + + /** + * This will cause the provided runnable to be executed on the scheduler thread before the task is + * released. All runnables provided here will be executed periodically in the order they were + * provided. + * + * @param runnable + */ + public void addRunnableOnSchedulerThread(Runnable runnable) + { + throw new UnsupportedOperationException(getClass().getSimpleName() + " does not support this operation."); + } + + protected static void runAll(List runnables) + { + for (int i = 0; i < runnables.size(); i++) + runnables.get(i).run(); + } +} diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarMPCSimulationFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarMPCSimulationFactory.java index 64bd5e3fcad..e7929c19622 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarMPCSimulationFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarMPCSimulationFactory.java @@ -13,8 +13,8 @@ import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.IKStreamingRTPluginFactory; import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.IKStreamingRTPluginFactory.IKStreamingRTThread; import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.KinematicsStreamingToolboxParameters; -import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData; import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory; +import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData; import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextDataFactory; import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; @@ -151,7 +151,7 @@ public class SCS2AvatarMPCSimulationFactory protected IntraprocessYoVariableLogger intraprocessYoVariableLogger; protected SimulationConstructionSet2 simulationConstructionSet; protected JointDesiredOutputWriter simulationOutputWriter; - protected HumanoidRobotContextData masterContext; + protected HumanoidRobotMPCContextData masterContext; protected AvatarEstimatorThread estimatorThread; protected AvatarMPCControllerThread controllerThread; protected AvatarMPCStepGeneratorThread stepGeneratorThread; @@ -542,7 +542,7 @@ private void setupMultiThreadedRobotController() // Create intermediate data buffer for threading. FullHumanoidRobotModel masterFullRobotModel = robotModel.createFullRobotModel(); robotInitialSetup.get().initializeFullRobotModel(masterFullRobotModel); - masterContext = new HumanoidRobotContextData(masterFullRobotModel); + masterContext = new HumanoidRobotMPCContextData(masterFullRobotModel); // Create the tasks that will be run on their own threads. int estimatorDivisor = (int) Math.round(robotModel.getEstimatorDT() / simulationDT.get()); @@ -550,22 +550,22 @@ private void setupMultiThreadedRobotController() int stepGeneratorDivisor = (int) Math.round(robotModel.getStepGeneratorDT() / simulationDT.get()); int handControlDivisor = (int) Math.round(robotModel.getSimulatedHandControlDT() / simulationDT.get()); int wholeBodyControllerCoreDivisor = (int) Math.round(robotModel.getWBCCDT() / simulationDT.get()); - HumanoidRobotControlTask estimatorTask = new MPCEstimatorTask(estimatorThread, estimatorDivisor, simulationDT.get(), masterFullRobotModel); - HumanoidRobotControlTask controllerTask = new MPCControllerTask("Controller", - controllerThread, - controllerDivisor, - simulationDT.get(), - masterFullRobotModel); - HumanoidRobotControlTask stepGeneratorTask = new MPCStepGeneratorTask("StepGenerator", - stepGeneratorThread, - stepGeneratorDivisor, - simulationDT.get(), - masterFullRobotModel); - HumanoidRobotControlTask wholeBodyControllerCoreTask = new MPCWholeBodyControllerCoreTask("WholeBodyController", - wholeBodyControllerCoreThread, - wholeBodyControllerCoreDivisor, - simulationDT.get(), - masterFullRobotModel); + MPCHumanoidRobotControlTask estimatorTask = new MPCEstimatorTask(estimatorThread, estimatorDivisor, simulationDT.get(), masterFullRobotModel); + MPCHumanoidRobotControlTask controllerTask = new MPCControllerTask("Controller", + controllerThread, + controllerDivisor, + simulationDT.get(), + masterFullRobotModel); + MPCHumanoidRobotControlTask stepGeneratorTask = new MPCStepGeneratorTask("StepGenerator", + stepGeneratorThread, + stepGeneratorDivisor, + simulationDT.get(), + masterFullRobotModel); + MPCHumanoidRobotControlTask wholeBodyControllerCoreTask = new MPCWholeBodyControllerCoreTask("WholeBodyController", + wholeBodyControllerCoreThread, + wholeBodyControllerCoreDivisor, + simulationDT.get(), + masterFullRobotModel); HumanoidRobotControlTask ikStreamingRTTask; if (createIKStreamingRealTimeController.get()) ikStreamingRTTask = ikStreamingRealTimePluginFactory.createRTTask(simulationDT.get()); @@ -614,15 +614,17 @@ private void setupMultiThreadedRobotController() }); } - List tasks = new ArrayList<>(); + List tasks = new ArrayList<>(); tasks.add(estimatorTask); tasks.add(controllerTask); tasks.add(stepGeneratorTask); tasks.add(wholeBodyControllerCoreTask); - if (ikStreamingRTTask != null) - tasks.add(ikStreamingRTTask); - if (handControlTask != null) - tasks.add(handControlTask); + + // for now just remove this. If we want to include this, should make MPC specific type of ikStreamingRTTask and handControlTask. + // if (ikStreamingRTTask != null) + // tasks.add(ikStreamingRTTask); + // if (handControlTask != null) + // tasks.add(handControlTask); // Create the controller that will run the tasks. String controllerName = "DRCSimulation"; @@ -634,7 +636,7 @@ private void setupMultiThreadedRobotController() else { TaskOverrunBehavior overrunBehavior = TaskOverrunBehavior.BUSY_WAIT; - robotController = new BarrierScheduledRobotController(controllerName, tasks, masterContext, overrunBehavior, simulationDT.get()); + robotController = new MPCBarrierScheduledRobotController(controllerName, tasks, masterContext, overrunBehavior, simulationDT.get()); tasks.forEach(task -> new Thread(task, task.getClass().getSimpleName() + "Thread").start()); } @@ -685,7 +687,8 @@ private void setupMultiThreadedRobotController() yoVariableServer.addRegistry(wholeBodyControllerCoreThread.getYoVariableRegistry(), enableSCS1YoGraphics.get() ? wholeBodyControllerCoreThread.getSCS1YoGraphicsListRegistry() : null, enableSCS2YoGraphics.get() ? wholeBodyControllerCoreThread.getSCS2YoGraphics() : null); - wholeBodyControllerCoreTask.addCallbackPostTask(() -> yoVariableServer.update(wholeBodyControllerCoreThread.getHumanoidRobotContextData().getTimestamp(), + wholeBodyControllerCoreTask.addCallbackPostTask(() -> yoVariableServer.update(wholeBodyControllerCoreThread.getHumanoidRobotContextData() + .getTimestamp(), wholeBodyControllerCoreThread.getYoVariableRegistry())); if (ikStreamingRTThread != null) { @@ -709,12 +712,15 @@ private void setupMultiThreadedRobotController() mirroredRegistries.add(setupWithMirroredRegistry(estimatorThread.getYoRegistry(), estimatorTask, robotController.getYoRegistry())); mirroredRegistries.add(setupWithMirroredRegistry(controllerThread.getYoVariableRegistry(), controllerTask, robotController.getYoRegistry())); mirroredRegistries.add(setupWithMirroredRegistry(stepGeneratorThread.getYoVariableRegistry(), stepGeneratorTask, robotController.getYoRegistry())); - mirroredRegistries.add(setupWithMirroredRegistry(wholeBodyControllerCoreThread.getYoVariableRegistry(), wholeBodyControllerCoreTask, robotController.getYoRegistry())); - - if (ikStreamingRTTask != null) - mirroredRegistries.add(setupWithMirroredRegistry(ikStreamingRTThread.getYoVariableRegistry(), ikStreamingRTTask, robotController.getYoRegistry())); - if (handControlThread != null) - mirroredRegistries.add(setupWithMirroredRegistry(handControlThread.getYoVariableRegistry(), handControlTask, robotController.getYoRegistry())); + mirroredRegistries.add(setupWithMirroredRegistry(wholeBodyControllerCoreThread.getYoVariableRegistry(), + wholeBodyControllerCoreTask, + robotController.getYoRegistry())); + + // These two task should be handled MPC specific task. But for now, just ignore them. + // if (ikStreamingRTTask != null) + // mirroredRegistries.add(setupWithMirroredRegistry(ikStreamingRTThread.getYoVariableRegistry(), ikStreamingRTTask, robotController.getYoRegistry())); + // if (handControlThread != null) + // mirroredRegistries.add(setupWithMirroredRegistry(handControlThread.getYoVariableRegistry(), handControlTask, robotController.getYoRegistry())); robot.getRegistry().addChild(robotController.getYoRegistry()); robot.getControllerManager().addController(new Controller() { @@ -774,7 +780,7 @@ public static List createYoVariableServerJointList(RigidBodyBasics return joints; } - private static MirroredYoVariableRegistry setupWithMirroredRegistry(YoRegistry registry, HumanoidRobotControlTask owner, YoRegistry schedulerRegistry) + private static MirroredYoVariableRegistry setupWithMirroredRegistry(YoRegistry registry, MPCHumanoidRobotControlTask owner, YoRegistry schedulerRegistry) { MirroredYoVariableRegistry mirroredRegistry = new MirroredYoVariableRegistry(registry); owner.addRunnableOnSchedulerThread(() -> diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java index 4027eadf17c..e2cdef95ba4 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java @@ -17,6 +17,8 @@ @SuppressWarnings("serial") public class HumanoidRobotMPCContextData extends HumanoidRobotContextData { + private boolean wholeBodyControllerCoreRan = false; + public HumanoidRobotMPCContextData() { super(); @@ -49,6 +51,10 @@ public HumanoidRobotMPCContextData(List joints) super(joints); } + public void setWholeBodyControllerCoreRan(boolean wholeBodyControllerCoreRan) + { + this.wholeBodyControllerCoreRan = wholeBodyControllerCoreRan; + } public void set(HumanoidRobotMPCContextData other) { @@ -59,15 +65,15 @@ public void copyFrom(HumanoidRobotMPCContextData src) { super.copyFrom(src); } + public boolean equals(Object obj) { - if(obj == this) + if (obj == this) { return true; } - else if(obj instanceof HumanoidRobotMPCContextData) + else if (obj instanceof HumanoidRobotMPCContextData other) { - HumanoidRobotMPCContextData other = (HumanoidRobotMPCContextData) obj; return super.equals(other); } else