public static void startController(WandererController wandererController) {
AcsellSetup.startStreamingData();
YoVariableRegistry registry = new YoVariableRegistry("wanderer");
WandererRobotModel robotModel = new WandererRobotModel(true, true);
PeriodicThreadScheduler scheduler = new PeriodicRealtimeThreadScheduler(45);
YoVariableServer variableServer =
new YoVariableServer(
WandererSingleThreadedController.class,
scheduler,
robotModel.getLogModelProvider(),
robotModel.getLogSettings(),
0.01);
AcsellSetup wandererSetup = new AcsellSetup(variableServer);
PriorityParameters priority = new PriorityParameters(PriorityParameters.getMaximumPriority());
WandererSingleThreadedController communicator =
new WandererSingleThreadedController(
robotModel, priority, variableServer, wandererController, registry);
variableServer.start();
wandererSetup.start();
communicator.start();
ThreadTools.sleepForever();
}
private WandererSingleThreadedController(
WandererRobotModel robotModel,
PriorityParameters priorityParameters,
RobotVisualizer visualizer,
WandererController wandererController,
YoVariableRegistry registry) {
super(priorityParameters);
this.state = new WandererState(0.001, registry);
this.reader = new UDPAcsellStateReader(state);
this.visualizer = visualizer;
this.command = new WandererCommand(registry);
this.controller = wandererController;
this.outputWriter = new UDPAcsellOutputWriter(command);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
rootJoint = fullRobotModel.getRootJoint();
for (WandererJoint joint : WandererJoint.values) {
OneDoFJoint oneDoFJoint = fullRobotModel.getOneDoFJointByName(joint.getSdfName());
if (oneDoFJoint == null) {
throw new RuntimeException("Unknown joint: " + joint.getSdfName());
}
jointMap.put(joint, oneDoFJoint);
}
this.rawSensors = new RawJointSensorDataHolderMap(fullRobotModel);
this.controller.setFullRobotModel(fullRobotModel);
if (controller.getYoVariableRegistry() != null) {
registry.addChild(controller.getYoVariableRegistry());
}
if (visualizer != null) {
visualizer.setMainRegistry(registry, fullRobotModel, null);
}
outputWriter.connect(new WandererNetworkParameters());
}