@Override public void doControl() { if(!this.isInitialized.getValue()) { robotController.initialize(); this.isInitialized.set(true); } robotController.doControl(); }
@Override public String getDescription() { return floatingRobotController.getDescription(); }
@Override public String getName() { return robotController.getName(); }
@Override public void doControl() { if (rawSensorReader != null) rawSensorReader.read(); if (sensorProcessor != null) sensorProcessor.update(); for (int i = 0; i < robotControllers.size(); i++) { robotControllers.get(i).doControl(); } if (outputProcessor != null) outputProcessor.update(); if (rawOutputWriter != null) rawOutputWriter.write(); } }
public void initialize() { if (rawSensorReader != null) rawSensorReader.initialize(); if (sensorProcessor != null) sensorProcessor.initialize(); for (int i = 0; i < robotControllers.size(); i++) { robotControllers.get(i).initialize(); } if (outputProcessor != null) outputProcessor.initialize(); if (rawOutputWriter != null) rawOutputWriter.initialize(); }
@Override public YoVariableRegistry getYoVariableRegistry() { return robotController.getYoVariableRegistry(); }
private WholeBodyTrajectoryToolboxOutputStatus runToolboxController(int maxNumberOfIterations) throws UnreasonableAccelerationException { AtomicReference<WholeBodyTrajectoryToolboxOutputStatus> status = new AtomicReference<>(null); statusOutputManager.attachStatusMessageListener(WholeBodyTrajectoryToolboxOutputStatus.class, status::set); initializationSucceeded.set(false); this.numberOfIterations.set(0); if (visualize) { for (int i = 0; !toolboxController.isDone() && i < maxNumberOfIterations; i++) scs.simulateOneTimeStep(); } else { for (int i = 0; !toolboxController.isDone() && i < maxNumberOfIterations; i++) toolboxUpdater.doControl(); } return status.getAndSet(null); }
floatingRobotController.initialize();
public void addRobotController(RobotController robotController) { if(robotController == this) { throw new RuntimeException("Cannot add self to modular robot controller"); } this.robotControllers.add(robotController); YoVariableRegistry controllerRegistry = robotController.getYoVariableRegistry(); if (controllerRegistry != null) registry.addChild(controllerRegistry); }
private void runKinematicsToolboxController(int numberOfIterations) throws SimulationExceededMaximumTimeException { initializationSucceeded.set(false); this.numberOfIterations.set(0); if (visualize) { blockingSimulationRunner.simulateNTicksAndBlockAndCatchExceptions(numberOfIterations); } else { for (int i = 0; i < numberOfIterations; i++) toolboxUpdater.doControl(); } finalSolutionQuality.set(toolboxController.getSolution().getSolutionQuality()); }
@Override public String getDescription() { return robotController.getDescription(); }
public InitializingRobotController(RobotController robotController) { this.robotController = robotController; this.isInitialized = new YoBoolean("isInitialized", robotController.getYoVariableRegistry()); this.isInitialized.set(false); }
private void runKinematicsToolboxController(int numberOfIterations) throws SimulationExceededMaximumTimeException { initializationSucceeded.set(false); this.numberOfIterations.set(0); if (visualize) { blockingSimulationRunner.simulateNTicksAndBlockAndCatchExceptions(numberOfIterations); } else { for (int i = 0; i < numberOfIterations; i++) toolboxUpdater.doControl(); } finalSolutionQuality.set(toolboxController.getSolution().getSolutionQuality()); }
private void runKinematicsPlanningToolboxController(int numberOfIterations) throws SimulationExceededMaximumTimeException, UnreasonableAccelerationException { initializationSucceeded.set(false); this.numberOfIterations.set(0); if (visualize) { for (int i = 0; !toolboxController.isDone() && i < numberOfIterations; i++) if (visualize) scs.simulateOneTimeStep(); } else { for (int i = 0; !toolboxController.isDone() && i < numberOfIterations; i++) { toolboxUpdater.doControl(); } } trackSolution(); }