-
Notifications
You must be signed in to change notification settings - Fork 91
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Adding MPCBarrierScheduledRobotController and MPCHumanoidRobotContr…
…olTask. - Changing the parents class for MPCControllerTaks, MPCEstimatorTaks, MPCStepGeneratorTask and MPCWholeBodyControllerCoreTask to MPCHumanoidRobotControlTask. - boolean of wholebodycontrollercoreRan is added to HumanoidRobotMPCContextData.
- Loading branch information
Showing
8 changed files
with
220 additions
and
65 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
10 changes: 7 additions & 3 deletions
10
ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCStepGeneratorTask.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
83 changes: 83 additions & 0 deletions
83
...r-interfaces/src/main/java/us/ihmc/avatar/factory/MPCBarrierScheduledRobotController.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<HumanoidRobotMPCContextData> barrierScheduler; | ||
| private final HumanoidRobotMPCContextData masterContext; | ||
|
|
||
| private final ThreadTimer timer; | ||
| private final List<MPCHumanoidRobotControlTask> tasks; | ||
|
|
||
| public MPCBarrierScheduledRobotController(String name, | ||
| List<MPCHumanoidRobotControlTask> 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(); | ||
| } | ||
| } |
59 changes: 59 additions & 0 deletions
59
ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/factory/MPCHumanoidRobotControlTask.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<HumanoidRobotMPCContextData> | ||
| { | ||
| 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 <b>before</b> 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 <b>after</b> 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<Runnable> runnables) | ||
| { | ||
| for (int i = 0; i < runnables.size(); i++) | ||
| runnables.get(i).run(); | ||
| } | ||
| } |
Oops, something went wrong.