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());
}
