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