Aiming a turret on a robot

optimizer_demo.pde

import processing.core.*;
import peasy.*;

// ==========================================
// GLOBALS
// ==========================================
GameField field;
Target target;
Robot robot;
TrajectorySimulator simulator;
ErrorPlot plot;
GradientDescentOptimizer optimizer;
PeasyCam cam;

PGraphics view3D;
PGraphics view2D;

float lastTime;
boolean autoAim = true; // Toggle with SPACE key

int h3D;
int h2D;

// ==========================================
// CONFIGURATION & CONSTANTS (Magic Numbers)
// ==========================================
final float TARGET_X = 5.0f, TARGET_Y = 5.0f, TARGET_Z = 2.6f;

final float GRAVITY = 9.81f;
final float SIM_TIME_MAX = 3.0f;
final float SIM_TIME_STEP = 0.05f;

// Turret Mount Offsets
final float TURRET_OFFSET_X = -0.3f; // Back
final float TURRET_OFFSET_Y = 0.0f;  // Center
final float TURRET_OFFSET_Z = 0.25f; // Sits 0.2m above the top of the 0.3m thick base
final float TURRET_JOINT_Z = 0.1f;   // Nozzle joint is 0.1m above the turret block center

final float TURRET_BARREL_L = 0.6f;
final float TURRET_YAW_MIN = 20 * PI / 180.0f;
final float TURRET_YAW_MAX = 310 * PI / 180.0f;
final float TURRET_YAW_MAX_SPD = 500 * PI / 180.0f;
final float TURRET_YAW_MAX_ACCEL = 2000 * PI / 180.0f;
final float TURRET_PITCH_MIN = 0.0f;
final float TURRET_PITCH_MAX = HALF_PI;
final float TURRET_V0_MIN = 0.0f;
final float TURRET_V0_MAX = 10.0f;
final float TURRET_V0_MAX_ACCEL = 10.0f; // m/s^2
final float TURRET_DRAG = 0.005f;

final float ROBOT_MOVE_ACCEL = 15.0f;
final float ROBOT_TURN_ACCEL = 10.0f;
final float ROBOT_LINEAR_DRAG = 0.01f;
final float ROBOT_ANGULAR_DRAG = 0.005f;

final float OPT_LR_YAW = 10.f;
final float OPT_LR_V0 = 5.0f;
final float OPT_DELTA = 0.001f;
final float OPT_HYPO_MARGIN = 0.5f;

final float FIRE_THRESHOLD = 0.2f;

void setup() {
  size(1000, 800, P3D);
  
  // Calculate buffer heights (75% 3D, 25% 2D)
  h3D = height * 3 / 4;
  h2D = height / 4;
  
  view3D = createGraphics(width, h3D, P3D);
  view2D = createGraphics(width, h2D, P2D);
  
  // Bind PeasyCam specifically to the 3D buffer
  view3D.perspective(PI/3.0, float(width)/float(h3D), 0.01, 100);
  cam = new PeasyCam(this, view3D, 2);
  cam.setSuppressRollRotationMode();
  
  // Initialize scene
  field = new GameField();
  target = new Target(TARGET_X, TARGET_Y, TARGET_Z);
  
  robot = new Robot(0, 0, 0);
  // Start turret at 90 degrees (PI/2) so it is within the 20 to 310 bounds, and start at 8m/s
  robot.turret = new Turret(PI/2, PI/4, 8.0f); 
  
  simulator = new TrajectorySimulator(GRAVITY, SIM_TIME_MAX, SIM_TIME_STEP);
  plot = new ErrorPlot();
  optimizer = new GradientDescentOptimizer();
  
  lastTime = millis() / 1000.0f;
}

void draw() {
  background(0);
  view3D.perspective(PI/3.0, float(width)/float(h3D), 0.01, 100);
  
  // Calculate delta time for smooth movement
  float currentTime = millis() / 1000.0f;
  float dt = currentTime - lastTime;
  lastTime = currentTime;
  
  // Run the optimizer FIRST so it can set the desired accelerations
  if (autoAim) {
    optimizer.optimize(robot, target, simulator, dt);
  }
  
  // Update physics SECOND so it integrates the newly calculated accelerations
  robot.update(dt);
  
  // ==========================================
  // 1. Render 3D Scene
  // ==========================================
  view3D.beginDraw();
  view3D.background(30);
  view3D.directionalLight(255, 255, 255, 0.5f, 0.5f, -1);
  view3D.ambientLight(100, 100, 100);
  
  field.draw(view3D);
  target.draw(view3D);
  robot.draw(view3D);
  simulator.drawTrajectory(view3D, robot, target);
  view3D.endDraw();
  
  // ==========================================
  // 2. Render 2D HUD
  // ==========================================
  plot.draw(view2D, robot, target, simulator);
  
  // ==========================================
  // 3. Draw to Main Canvas
  // ==========================================
  image(view3D, 0, 0);
  image(view2D, 0, height * 0.75f);
  
  // ==========================================
  // 4. Draw HUD Overlays
  // ==========================================
  
  // Auto-aim mode indicator
  if (autoAim) {
    fill(0, 255, 0);
    textSize(16);
    textAlign(LEFT, TOP);
    text("AUTO-AIM ACTIVE (Press SPACE to toggle)", 20, 30);
  } else {
    fill(255, 165, 0);
    textSize(16);
    textAlign(LEFT, TOP);
    text("MANUAL AIM (Press SPACE to toggle)", 20, 30);
  }
  
  // Ready to Fire Indicator
  float currentMinError = simulator.getMinError(robot, target);
  if (currentMinError < FIRE_THRESHOLD) {
    fill(0, 255, 0);
    textSize(24);
    textAlign(CENTER, TOP);
    text("READY TO FIRE", width / 2, 30);
  }
  
  // Reset text alignment just in case
  textAlign(LEFT, BASELINE);
}

// ==========================================
// INPUT HANDLING
// ==========================================
void keyPressed() {
  if (key == ' ') autoAim = !autoAim;
  robot.setKey(key, keyCode, true);
}

void keyReleased() {
  robot.setKey(key, keyCode, false);
}

// ==========================================
// CLASSES
// ==========================================

class GameField {
  PShape model;
  GameField() {
    model = createShape(BOX, 16, 8, 0.1f);
    model.setFill(color(100, 150, 100));
  }
  void draw(PGraphics pg) { pg.shape(model); }
}

class Target {
  float x, y, z;
  Target(float x, float y, float z) { this.x = x; this.y = y; this.z = z; }
  void draw(PGraphics pg) {
    pg.pushMatrix();
    pg.translate(x, y, z);
    pg.fill(0, 255, 0);
    pg.noStroke();
    pg.sphere(0.2f);
    pg.popMatrix();
  }
}

class DiscreteTimeDynamicDHLink {
  float theta, d, a, alpha;
  float v_theta, v_d, v_a, v_alpha;
  float a_theta, a_d, a_a, a_alpha;

  DiscreteTimeDynamicDHLink(float theta, float d, float a, float alpha) {
    this.theta = theta; 
    this.d = d; 
    this.a = a; 
    this.alpha = alpha;
  }

  void update(float dt) {
    // Integrate acceleration into velocity
    v_theta += a_theta * dt;
    v_d += a_d * dt;
    v_a += a_a * dt;
    v_alpha += a_alpha * dt;

    // Apply natural friction/damping to velocities ONLY if not actively accelerating.
    float drag = pow(TURRET_DRAG, dt); 
    if (a_theta == 0) v_theta *= drag;
    if (a_d == 0) v_d *= drag;
    if (a_a == 0) v_a *= drag;
    if (a_alpha == 0) v_alpha *= drag;

    // Integrate velocity into position
    theta += v_theta * dt;
    d += v_d * dt;
    a += v_a * dt;
    alpha += v_alpha * dt;
  }
  
  PMatrix3D getMatrix() {
    PMatrix3D mat = new PMatrix3D();
    mat.rotateZ(theta);
    mat.translate(0, 0, d);
    mat.translate(a, 0, 0);
    mat.rotateX(alpha);
    return mat;
  }
  
  PMatrix3D getInverseMatrix() {
    PMatrix3D mat = getMatrix();
    mat.invert();
    return mat;
  }
}

class Turret {
  float v0;
  DiscreteTimeDynamicDHLink linkYaw;
  DiscreteTimeDynamicDHLink linkPitch;
  
  PShape baseModel;
  PShape barrelModel;
  
  boolean keyUp, keyDown, keyLeft, keyRight;
  
  Turret(float t1, float t2, float v0) {
    this.v0 = v0;
    linkYaw = new DiscreteTimeDynamicDHLink(t1, TURRET_JOINT_Z, 0f, HALF_PI); 
    linkPitch = new DiscreteTimeDynamicDHLink(t2, 0f, TURRET_BARREL_L, 0f); 
    
    // New turret base block
    baseModel = createShape(BOX, 0.2f, 0.2f, 0.2f);
    baseModel.setFill(color(100, 100, 100));
    
    // Blue nozzle barrel
    barrelModel = createShape(BOX, TURRET_BARREL_L, 0.2f, 0.2f);
    barrelModel.setFill(color(50, 50, 150));
  }

  void update(float dt) {
    if (!autoAim) {
      // ONLY reset acceleration manually if auto-aim is off.
      // Otherwise, we overwrite the optimizer's calculations!
      linkYaw.a_theta = 0;
      linkPitch.a_theta = 0;
      float actual_v0Rate = 0;
      
      if (keyLeft) linkYaw.a_theta -= TURRET_YAW_MAX_ACCEL;
      if (keyRight) linkYaw.a_theta += TURRET_YAW_MAX_ACCEL;
      
      // UP and DOWN now adjust launch speed, as pitch is fixed/controlled elsewhere
      if (keyUp) actual_v0Rate += TURRET_V0_MAX_ACCEL;
      if (keyDown) actual_v0Rate -= TURRET_V0_MAX_ACCEL;
      
      v0 += actual_v0Rate * dt;
    }

    linkYaw.update(dt);
    linkPitch.update(dt);
    
    // Enforce Speed Caps
    linkYaw.v_theta = constrain(linkYaw.v_theta, -TURRET_YAW_MAX_SPD, TURRET_YAW_MAX_SPD);
    
    // Enforce Position Bounds
    if (linkYaw.theta < TURRET_YAW_MIN) {
        linkYaw.theta = TURRET_YAW_MIN;
        if (linkYaw.v_theta < 0) linkYaw.v_theta = 0;
    } else if (linkYaw.theta > TURRET_YAW_MAX) {
        linkYaw.theta = TURRET_YAW_MAX;
        if (linkYaw.v_theta > 0) linkYaw.v_theta = 0;
    }
    
    linkPitch.theta = constrain(linkPitch.theta, TURRET_PITCH_MIN, TURRET_PITCH_MAX);
    v0 = constrain(v0, TURRET_V0_MIN, TURRET_V0_MAX);
  }
  
  void draw(PGraphics pg) {
    // Draw the turret base block at the local mount origin
    pg.shape(baseModel);
    
    // Process kinematics for the barrel
    pg.pushMatrix();
    pg.applyMatrix(linkYaw.getMatrix());
    pg.applyMatrix(linkPitch.getMatrix());
    
    // Half of barrel length offset so the model rotates at its end
    pg.translate(-TURRET_BARREL_L / 2.0f, 0, 0); 
    pg.shape(barrelModel);
    pg.popMatrix();
  }
  
  void setKey(int code, boolean isPressed) {
    if (code == LEFT) keyLeft = isPressed;
    if (code == RIGHT) keyRight = isPressed;
    if (code == UP) keyUp = isPressed;
    if (code == DOWN) keyDown = isPressed;
  }
}

class Robot {
  float x, y, yaw;
  float vx, vy, vYaw; 
  
  boolean keyW, keyS, keyA, keyD, keyQ, keyE;
  
  Turret turret;
  PShape model;
  
  Robot(float x, float y, float yaw) {
    this.x = x; this.y = y; this.yaw = yaw;
    model = createShape(BOX, 0.8f, 0.8f, 0.3f);
    model.setFill(color(150, 50, 50));
  }
  
  PMatrix3D getBaseMatrix() {
    PMatrix3D mat = new PMatrix3D();
    mat.translate(x, y, 0);
    mat.rotateZ(yaw);
    return mat;
  }
  
  void draw(PGraphics pg) {
    pg.pushMatrix();
    pg.applyMatrix(getBaseMatrix());
    pg.shape(model);
    
    if (turret != null) {
      pg.pushMatrix();
      
      // Move to the back-left turret mount point
      pg.translate(TURRET_OFFSET_X, TURRET_OFFSET_Y, TURRET_OFFSET_Z);
      
      // Render the limits of the turret as an arc around the mount point
      pg.pushMatrix();
      pg.translate(0, 0, -0.05f); // Slightly below the turret block
      pg.noFill();
      pg.strokeWeight(3);
      
      // Active zone (Green)
      pg.stroke(0, 255, 0, 100); 
      pg.arc(0, 0, 1.2f, 1.2f, TURRET_YAW_MIN, TURRET_YAW_MAX);
      // Dead zone (Red)
      pg.stroke(255, 0, 0, 100); 
      pg.arc(0, 0, 1.2f, 1.2f, TURRET_YAW_MAX, TURRET_YAW_MIN + TWO_PI);
      pg.popMatrix();

      // Draw the turret blocks
      turret.draw(pg);
      
      pg.popMatrix();
    }
    pg.popMatrix();
  }
  
  void update(float dt) {
    float localAx = 0;
    float localAy = 0;
    
    if (keyW) localAx += ROBOT_MOVE_ACCEL;
    if (keyS) localAx -= ROBOT_MOVE_ACCEL;
    if (keyA) localAy += ROBOT_MOVE_ACCEL; // Strafe left
    if (keyD) localAy -= ROBOT_MOVE_ACCEL; // Strafe right
    
    // Rotate local acceleration to global frame
    vx += (cos(yaw) * localAx - sin(yaw) * localAy) * dt;
    vy += (sin(yaw) * localAx + cos(yaw) * localAy) * dt;
    
    float aYaw = 0;
    if (keyQ) aYaw -= ROBOT_TURN_ACCEL;
    if (keyE) aYaw += ROBOT_TURN_ACCEL;
    vYaw += aYaw * dt;
    
    // Natural friction damping (frame-rate independent)
    float linearDrag = pow(ROBOT_LINEAR_DRAG, dt); 
    float angularDrag = pow(ROBOT_ANGULAR_DRAG, dt);
    
    vx *= linearDrag;
    vy *= linearDrag;
    vYaw *= angularDrag;
    
    x += vx * dt;
    y += vy * dt;
    yaw += vYaw * dt;
    
    if (turret != null) turret.update(dt);
  }
  
  void setKey(char k, int code, boolean isPressed) {
    char lowerK = Character.toLowerCase(k);
    if (lowerK == 'w') keyW = isPressed;
    if (lowerK == 's') keyS = isPressed;
    if (lowerK == 'a') keyA = isPressed;
    if (lowerK == 'd') keyD = isPressed;
    if (lowerK == 'q') keyQ = isPressed;
    if (lowerK == 'e') keyE = isPressed;
    
    if (turret != null) turret.setKey(code, isPressed);
  }
}

class TrajectorySimulator {
  float g, timeMax, timeStep;
  
  TrajectorySimulator(float g, float timeMax, float timeStep) {
    this.g = g; this.timeMax = timeMax; this.timeStep = timeStep;
  }
  
  class BallState { float px, py, pz, vx, vy, vz; }
  
  BallState calculateInitialState(Robot r) {
    PMatrix3D T_robot = r.getBaseMatrix();
    PMatrix3D T_yaw = r.turret.linkYaw.getMatrix();
    PMatrix3D T_pitch = r.turret.linkPitch.getMatrix();
    
    PMatrix3D T = new PMatrix3D(T_robot);
    
    // Apply physical offset of the turret's mounting point
    T.translate(TURRET_OFFSET_X, TURRET_OFFSET_Y, TURRET_OFFSET_Z);
    T.apply(T_yaw);
    
    // Capture the turret base global location to factor in the turret's rotation sling effect
    float tx = T.m03; 
    float ty = T.m13;
    
    T.apply(T_pitch);
    
    float px = T.m03, py = T.m13, pz = T.m23;
    float dirX = T.m00, dirY = T.m10, dirZ = T.m20;
    
    // Robot base rotation sling effect
    float rx = px - r.x;
    float ry = py - r.y;
    float v_tangent_x = -r.vYaw * ry;
    float v_tangent_y = r.vYaw * rx;
    
    // Turret rotation sling effect
    float tx_nozzle = px - tx;
    float ty_nozzle = py - ty;
    v_tangent_x += -r.turret.linkYaw.v_theta * ty_nozzle;
    v_tangent_y += r.turret.linkYaw.v_theta * tx_nozzle;
    
    BallState state = new BallState();
    state.px = px; state.py = py; state.pz = pz;
    state.vx = r.vx + v_tangent_x + (r.turret.v0 * dirX);
    state.vy = r.vy + v_tangent_y + (r.turret.v0 * dirY);
    state.vz = 0  + 0             + (r.turret.v0 * dirZ);
    
    return state;
  }

  void drawTrajectory(PGraphics pg, Robot r, Target tgt) {
    BallState state = calculateInitialState(r);
    
    pg.noFill();
    pg.stroke(255, 165, 0);
    pg.strokeWeight(3);
    
    float t_min = 0;
    float min_e = Float.MAX_VALUE;
    
    pg.beginShape();
    for (float t = 0; t <= timeMax; t += timeStep) {
      float bx = state.px + state.vx * t;
      float by = state.py + state.vy * t;
      float bz = state.pz + state.vz * t - (0.5f * g * t * t);
      
      if (bz < 0 && t > 0.1f) break; 
      pg.vertex(bx, by, bz);
      
      // Calculate error strictly to locate the minimum for the visual marker
      float dx = bx - tgt.x;
      float dy = by - tgt.y;
      float dz = bz - tgt.z;
      float e = sqrt(dx*dx + dy*dy + dz*dz);
      if (e < min_e) {
          min_e = e;
          t_min = t;
      }
    }
    pg.endShape();
    
    // Render the closest point marker
    float bx_min = state.px + state.vx * t_min;
    float by_min = state.py + state.vy * t_min;
    float bz_min = state.pz + state.vz * t_min - (0.5f * g * t_min * t_min);
    
    pg.pushMatrix();
    pg.translate(bx_min, by_min, bz_min);
    pg.fill(255, 50, 50); // Red marker to match UI plot
    pg.noStroke();
    pg.sphere(0.15f);
    pg.popMatrix();
  }
  
  float calculateError(Robot r, Target tgt, float t) {
    BallState state = calculateInitialState(r);
    
    float bx = state.px + state.vx * t;
    float by = state.py + state.vy * t;
    float bz = state.pz + state.vz * t - (0.5f * g * t * t);
    
    float dx = bx - tgt.x;
    float dy = by - tgt.y;
    float dz = bz - tgt.z;
    
    return sqrt(dx*dx + dy*dy + dz*dz); 
  }
  
  float getMinError(Robot r, Target tgt) {
    float min_e = Float.MAX_VALUE;
    for (float t = 0; t <= timeMax; t += timeStep) {
      float e = calculateError(r, tgt, t);
      if (e < min_e) {
        min_e = e;
      }
    }
    return min_e;
  }
}

class ErrorPlot {
  ErrorPlot() {} 
  
  void draw(PGraphics pg, Robot r, Target tgt, TrajectorySimulator sim) {
    pg.beginDraw();
    pg.background(20);
    
    float t_min = 0;
    float min_e = Float.MAX_VALUE;
    
    for (float t = 0; t <= 5.0f; t += sim.timeStep) {
      float e = sim.calculateError(r, tgt, t);
      if (e < min_e) {
        min_e = e;
        t_min = t;
      }
    }
    
    float plotTimeMax = (t_min > 0.1f) ? (t_min / 0.75f) : 3.0f;
    
    float max_e = 0;
    for (float t = 0; t <= plotTimeMax; t += sim.timeStep) {
       float e = sim.calculateError(r, tgt, t);
       if (e > max_e) max_e = e;
    }
    float plotErrorMax = max(5.0f, max_e * 1.1f); 
    
    float leftMargin = 60;
    float rightMargin = 50;
    float topMargin = 30;
    float bottomMargin = 45;
    float plotWidth = pg.width - leftMargin - rightMargin;
    
    pg.stroke(255);
    pg.strokeWeight(1);
    pg.line(leftMargin, pg.height - bottomMargin, pg.width - rightMargin, pg.height - bottomMargin);
    pg.line(leftMargin, pg.height - bottomMargin, leftMargin, topMargin);
    
    pg.fill(255);
    pg.textSize(12);
    
    pg.textAlign(CENTER, TOP);
    pg.text("Time (s)", leftMargin + plotWidth / 2, pg.height - bottomMargin + 20);
    pg.textAlign(RIGHT, CENTER);
    pg.text("Distance (m)", leftMargin - 10, topMargin - 10);
    
    pg.textAlign(CENTER, TOP);
    pg.text("0.0s", leftMargin, pg.height - bottomMargin + 5);
    pg.text(nf(plotTimeMax, 1, 2) + "s", pg.width - rightMargin, pg.height - bottomMargin + 5);
    
    pg.textAlign(RIGHT, CENTER);
    pg.text("0.0m", leftMargin - 5, pg.height - bottomMargin);
    pg.text(nf(plotErrorMax, 1, 1) + "m", leftMargin - 5, topMargin);
    
    pg.noFill();
    pg.stroke(0, 255, 255);
    pg.strokeWeight(2);
    pg.beginShape();
    
    for (float t = 0; t <= plotTimeMax; t += sim.timeStep) {
      float e = sim.calculateError(r, tgt, t);
      float plotX = map(t, 0, plotTimeMax, leftMargin, pg.width - rightMargin);
      float plotY = map(e, 0, plotErrorMax, pg.height - bottomMargin, topMargin);
      pg.vertex(plotX, constrain(plotY, topMargin, pg.height - bottomMargin));
    }
    pg.endShape();
    
    float minPlotX = leftMargin + plotWidth * 0.75f;
    float minPlotY = map(min_e, 0, plotErrorMax, pg.height - bottomMargin, topMargin);
    
    pg.stroke(100, 100, 255);
    pg.strokeWeight(1);
    pg.line(minPlotX, pg.height - bottomMargin, minPlotX, minPlotY);
    
    pg.fill(255, 50, 50);
    pg.noStroke();
    pg.circle(minPlotX, minPlotY, 8);
    
    pg.textAlign(LEFT, BOTTOM);
    pg.text(" Min: " + nf(min_e, 1, 2) + "m @ " + nf(t_min, 1, 2) + "s", minPlotX + 5, minPlotY - 5);
    
    pg.endDraw();
  }
}

class GradientDescentOptimizer {
  GradientDescentOptimizer() {}
  
  void optimize(Robot r, Target tgt, TrajectorySimulator sim, float dt) {
    if (dt <= 0) return;
    
    float originalYaw = r.turret.linkYaw.theta;
    float originalV0 = r.turret.v0;
    
    // 1. Find the current time of minimum error
    float t_min = 0;
    float currentError = Float.MAX_VALUE;
    for (float t = 0; t <= sim.timeMax; t += sim.timeStep) {
      float e = sim.calculateError(r, tgt, t);
      if (e < currentError) {
        currentError = e;
        t_min = t;
      }
    }
    
    // 2. Multi-Hypothesis Global Search for Yaw
    // Evaluate 3 evenly distributed angles across the permitted bounds
    float[] hypotheses = {TURRET_YAW_MIN, (TURRET_YAW_MIN + TURRET_YAW_MAX)/2.0f, TURRET_YAW_MAX};
    float bestHypothesis = originalYaw;
    float bestHypothesisError = currentError;
    
    for (float hYaw : hypotheses) {
      r.turret.linkYaw.theta = hYaw;
      
      // Find the minimum error over time for this specific hypothesis
      float hMinErr = Float.MAX_VALUE;
      for (float t = 0; t <= sim.timeMax; t += sim.timeStep) {
        float e = sim.calculateError(r, tgt, t);
        if (e < hMinErr) hMinErr = e;
      }
      
      // Require a meaningful improvement to abandon the current local minimum
      if (hMinErr < bestHypothesisError - OPT_HYPO_MARGIN) {
        bestHypothesisError = hMinErr;
        bestHypothesis = hYaw;
      }
    }
    r.turret.linkYaw.theta = originalYaw; // Restore original yaw
    
    // 3. Partial Derivative w.r.t Yaw
    r.turret.linkYaw.theta += OPT_DELTA;
    float errorYawUp = sim.calculateError(r, tgt, t_min);
    r.turret.linkYaw.theta = originalYaw;
    float dError_dYaw = (errorYawUp - currentError) / OPT_DELTA;
    
    // 4. Partial Derivative w.r.t Launch Speed (v0)
    r.turret.v0 += OPT_DELTA;
    float errorV0Up = sim.calculateError(r, tgt, t_min);
    r.turret.v0 = originalV0;
    float dError_dV0 = (errorV0Up - currentError) / OPT_DELTA;
    
    // 5. Control Logic for Yaw 
    float desired_vYaw;
    if (bestHypothesis != originalYaw) {
        // A distant hypothesis is better: steer aggressively towards it to escape the local minimum
        desired_vYaw = (bestHypothesis > originalYaw) ? TURRET_YAW_MAX_SPD : -TURRET_YAW_MAX_SPD;
    } else {
        // We are in the correct global basin: use local gradient descent to fine-tune
        desired_vYaw = -OPT_LR_YAW * dError_dYaw;
    }
    desired_vYaw = constrain(desired_vYaw, -TURRET_YAW_MAX_SPD, TURRET_YAW_MAX_SPD);
    
    float requiredAccelYaw = (desired_vYaw - r.turret.linkYaw.v_theta) / dt;
    r.turret.linkYaw.a_theta = constrain(requiredAccelYaw, -TURRET_YAW_MAX_ACCEL, TURRET_YAW_MAX_ACCEL); 
    
    // 6. Control Logic for Launch Speed
    float desired_v0Rate = -OPT_LR_V0 * dError_dV0;
    
    float actual_v0Rate = constrain(desired_v0Rate, -TURRET_V0_MAX_ACCEL, TURRET_V0_MAX_ACCEL);
    
    r.turret.v0 += actual_v0Rate * dt;
    r.turret.v0 = constrain(r.turret.v0, TURRET_V0_MIN, TURRET_V0_MAX); 
  }
}

A Processing sketch visualizing a robot automatically aiming a turret to launch a ball at a target

I just run gradient descent manually a static number of steps, so we don't run into inconsistent timing issues.
Then, once I have a target direction, I use a P controller to find the turret motor power (angular acceleration) that eventually orients the turret in the right direction (hence the overshoot)

The turret overshoots/undershoots when the robot accelerates rapidly for the same reason. The simulator caps the rate of change in the nozzle speed, and a P controller drives the motor power (approximated as the rate of change*) to control the target speed.

A quick fix would be to use PID controllers instead, but other options exist too. Also, I'm too lazy to try on a simulated robot with bogus properties right now.

Not entirely accurate but it isn't too hard to update the plant dynamics

Video: https://youtu.be/Dvqz6t2KiLc