public void process(long timestamp) {
    for (WandererJoint joint : WandererJoint.values) {
      AcsellJointState jointState = state.getJointState(joint);
      OneDoFJoint oneDoFJoint = jointMap.get(joint);
      RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint);

      oneDoFJoint.setQ(jointState.getQ());
      oneDoFJoint.setQd(jointState.getQd());
      state.updateRawSensorData(joint, rawSensor);
    }

    AcsellXSensState xSensState = state.getXSensState();
    xSensState.getQuaternion(rotation);
    //      rootJoint.setRotation(rotation);
    rootJoint.updateFramesRecursively();
    if (!initialized) {
      controller.initialize(timestamp);
      initialized = true;
    }

    controller.doControl(timestamp);

    if (controller.turnOutputOn()) {
      command.enableActuators();
    }

    for (WandererJoint joint : WandererJoint.values) {
      OneDoFJoint oneDoFJoint = jointMap.get(joint);
      RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint);

      AcsellJointCommand jointCommand = command.getAcsellJointCommand(joint);
      jointCommand.setTauDesired(oneDoFJoint.getTau(), 0.0, rawSensor);
      jointCommand.setDamping(oneDoFJoint.getKd());
    }

    outputWriter.write();

    if (visualizer != null) {
      visualizer.update(timestamp);
    }
  }
  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());
  }