3d shooting solver

lib/src/main/java/org/team100/lib/experiments/Experiment.java

@@ -75,5 +75,9 @@ public enum Experiment {
      * Compensate for moving shooter and moving target when computing turret
      * solution.
      */
-    TurretIntercept
+    TurretIntercept,
+    /**
+     * Use "shooting method" for turret solution
+     */
+    TurretShootingMethod
 }

lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.geometry.Twist3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N6;
 import edu.wpi.first.math.spline.PoseWithCurvature;
@@ -363,4 +364,8 @@ public static Vector<N3> toVec(Twist2d twist) {
     public static Vector<N6> toVec(Twist3d twist) {
         return VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz);
     }
+
+    public static Vector<N2> toVec(Translation2d t) {
+        return VecBuilder.fill(t.getX(), t.getY());
+    }
 }

lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java

@@ -0,0 +1,34 @@
+package org.team100.lib.geometry;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * Velocity in three dimensions, companion to Translation3d.
+ * 
+ * This is different from GlobalVelocityR3, which is the companion to Pose2d,
+ * i.e. velocity of planar rigid transforms.
+ */
+public record GlobalVelocity3d(double x, double y, double z) {
+
+    public static GlobalVelocity3d fromPolar(
+            Rotation2d azimuth, Rotation2d elevation, double speed) {
+        double vx = speed * azimuth.getCos() * elevation.getCos();
+        double vy = speed * azimuth.getSin() * elevation.getCos();
+        double vz = speed * elevation.getSin();
+        return new GlobalVelocity3d(vx, vy, vz);
+    }
+
+    /** Pick up the translation component of v, in the XY plane. */
+    public static GlobalVelocity3d fromSe2(GlobalVelocityR3 v) {
+        return new GlobalVelocity3d(v.x(), v.y(), 0);
+    }
+
+    public GlobalVelocity3d plus(GlobalVelocity3d other) {
+        return new GlobalVelocity3d(x + other.x, y + other.y, z + other.z);
+    }
+
+    public double normXY() {
+        return Math.sqrt(x * x + y * y);
+    }
+
+}

lib/src/main/java/org/team100/lib/kinematics/urdf/URDFRobot.java

@@ -132,7 +132,7 @@ public Map<String, Double> inverse(
                 minQ(m_qDim), maxQ(m_qDim),
                 tolerance, iterations, dqLimit);
         long startTime = System.nanoTime();
-        Vector<Q> qVec = solver.solve2(q0, restarts);
+        Vector<Q> qVec = solver.solve2(q0, restarts, true);
 
         if (DEBUG) {
             long finishTime = System.nanoTime();

lib/src/main/java/org/team100/lib/optimization/Bisection1d.java

@@ -2,14 +2,13 @@
 
 import java.util.function.DoubleUnaryOperator;
 
-import org.team100.lib.util.Math100;
-
 /**
  * Very simple scalar bisection solver.
  * 
  * @see https://en.wikipedia.org/wiki/Bisection_method
  */
 public class Bisection1d {
+    private static final boolean DEBUG = false;
 
     /**
      * @param func            to be solved
@@ -29,7 +28,7 @@ public static double findRoot(
             double f_1,
             double tolerance,
             int iterations_left) {
-        if (Math100.DEBUG) {
+        if (DEBUG) {
             System.out.printf("*************** i %d x_0 %.8f f_0 %.8f x_1 %.8f f_1 %.8f\n",
                     iterations_left, x_0, f_0, x_1, f_1);
         }
@@ -42,12 +41,12 @@ public static double findRoot(
             return 1.0;
         }
         if (Math.abs(f_0) < tolerance) {
-            if (Math100.DEBUG)
+            if (DEBUG)
                 System.out.println("left edge is the solution");
             return x_0;
         }
         if (Math.abs(f_1) < tolerance) {
-            if (Math100.DEBUG)
+            if (DEBUG)
                 System.out.println("right edge is the solution");
             return x_1;
         }
@@ -56,12 +55,12 @@ public static double findRoot(
 
         double x_guess = (x_1 - x_0) * s_guess + x_0;
         double f_guess = func.applyAsDouble(x_guess);
-        if (Math100.DEBUG) {
+        if (DEBUG) {
             System.out.printf("************* guess f(%.8f) = %.8f\n", x_guess, f_guess);
         }
 
         if (Math.abs(f_guess) < tolerance) {
-            if (Math100.DEBUG) {
+            if (DEBUG) {
                 System.out.printf("guess %.8f less than tolerance %.8f\n", f_guess, tolerance);
             }
             return s_guess;

lib/src/main/java/org/team100/lib/optimization/NewtonsMethod.java

@@ -47,7 +47,7 @@ public class NewtonsMethod<X extends Num, Y extends Num> {
      *                   input and return an error estimate in the tangent manifold
      *                   of whatever space it's actually acting in (e.g. SE(3))
      * @param xMin       minimum x
-     * @param xMax       minimum y
+     * @param xMax       maximum x
      * @param tolerance  return when solution x yields f(x) this close to zero
      * @param iterations when Newton gets stuck, it only takes a few iterations, so
      *                   this doesn't need to be large. Specify a generous
@@ -89,16 +89,19 @@ public Vector<X> solve(Vector<X> initial) {
             // Keep the x estimate within bounds.
             limit(x);
         }
+        System.out.println("exceeded max iterations");
         return x;
     }
 
     /**
      * Single-sided Jacobian, faster.
      * 
-     * @param initialX start here
-     * @param restarts number of random restarts in case of non-convergence
+     * @param initialX       start here
+     * @param restarts       number of random restarts in case of non-convergence
+     * @param throwOnFailure throw an exception if we fail to find a solution. Some
+     *                       clients can't tolerate a "kinda close" solution.
      */
-    public Vector<X> solve2(Vector<X> initialX, int restarts) {
+    public Vector<X> solve2(Vector<X> initialX, int restarts, boolean throwOnFailure) {
         // System.out.printf("initialX: %s\n", StrUtil.vecStr(initialX));
         long startTime = System.nanoTime();
         int iter = 0;
@@ -162,13 +165,14 @@ public Vector<X> solve2(Vector<X> initialX, int restarts) {
                     x.set(i, 0, x.get(i) + 0.1 * (random.nextDouble() - 0.5));
                 }
                 limit(x);
-                return solve2(x, restarts - 1);
+                return solve2(x, restarts - 1, throwOnFailure);
             }
             // if (DEBUG)
             System.out.printf("random restart failed, error %f\n", error.maxAbs());
-            // throw new IllegalArgumentException(
-            // String.format("failed to converge for inputs %s",
-            // StrUtil.vecStr(initialX)));
+            if (throwOnFailure)
+                throw new IllegalArgumentException(
+                        String.format("failed to converge for inputs %s",
+                                StrUtil.vecStr(initialX)));
             return x;
         } finally {
             long finishTime = System.nanoTime();

lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java

@@ -21,7 +21,10 @@
 import org.team100.lib.servo.AngularPositionServo;
 import org.team100.lib.servo.OnboardAngularPositionServo;
 import org.team100.lib.state.ModelR3;
+import org.team100.lib.targeting.Drag;
 import org.team100.lib.targeting.Intercept;
+import org.team100.lib.targeting.Range;
+import org.team100.lib.targeting.ShootingMethod;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -35,6 +38,13 @@
  * It provides Field2d visualization of the turret using the name "turret".
  */
 public class Turret extends SubsystemBase {
+    /**
+     * Azimuth and elevation are solved simultaneously and both actuated by the
+     * Turret.
+     */
+    public record Solution(Rotation2d azimuth, Rotation2d elevation) {
+    }
+
     private static final double GEAR_RATIO = 100;
     private static final double MIN_POSITION = -3;
     private static final double MAX_POSITION = 3;
@@ -48,9 +58,11 @@ public class Turret extends SubsystemBase {
     private final Supplier<ModelR3> m_state;
     private final Supplier<Translation2d> m_target;
     private final AngularPositionServo m_pivot;
+    private final AngularPositionServo m_elevation;
     /** Projectile speed m/s */
     private final double m_speed;
     private final Intercept m_intercept;
+    private final ShootingMethod m_shootingMethod;
     private boolean m_aiming;
 
     /**
@@ -69,6 +81,17 @@ public Turret(
         m_log_field_turret = field.doubleArrayLogger(Level.COMP, "turret");
         m_state = state;
         m_target = target;
+        m_pivot = pivot(log);
+        m_elevation = elevation(log);
+        m_speed = speed;
+        m_intercept = new Intercept(log);
+        Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1);
+        Range range = new Range(d, speed, 0);
+        m_shootingMethod = new ShootingMethod(range, 0.01);
+        m_aiming = false;
+    }
+
+    private static AngularPositionServo pivot(LoggerFactory log) {
         IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 5, 10, 0.05);
         ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05);
         PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1);
@@ -78,12 +101,26 @@ public Turret(
                 log, encoder, GEAR_RATIO);
         RotaryMechanism mech = new RotaryMechanism(
                 log, motor, sensor, GEAR_RATIO, MIN_POSITION, MAX_POSITION);
-        m_pivot = new OnboardAngularPositionServo(
+        AngularPositionServo pivot = new OnboardAngularPositionServo(
                 log, mech, ref, feedback);
-        m_pivot.reset();
-        m_speed = speed;
-        m_intercept = new Intercept(log);
-        m_aiming = false;
+        pivot.reset();
+        return pivot;
+    }
+
+    private static AngularPositionServo elevation(LoggerFactory log) {
+        IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 5, 10, 0.05);
+        ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05);
+        PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1);
+        SimulatedBareMotor motor = new SimulatedBareMotor(log, 600);
+        IncrementalBareEncoder encoder = motor.encoder();
+        SimulatedRotaryPositionSensor sensor = new SimulatedRotaryPositionSensor(
+                log, encoder, GEAR_RATIO);
+        RotaryMechanism mech = new RotaryMechanism(
+                log, motor, sensor, GEAR_RATIO, 0, Math.PI / 2);
+        AngularPositionServo pivot = new OnboardAngularPositionServo(
+                log, mech, ref, feedback);
+        pivot.reset();
+        return pivot;
     }
 
     public boolean onTarget() {
@@ -96,41 +133,67 @@ public Rotation2d getAzimuth() {
         return m_state.get().rotation().plus(relative);
     }
 
+    public Rotation2d getElevation() {
+        return new Rotation2d(m_elevation.getWrappedPositionRad());
+    }
+
     private void moveToAim() {
         m_aiming = true;
-        Optional<Rotation2d> soln = getSolution();
+        Optional<Solution> soln = getSolution();
         if (soln.isEmpty()) {
             // no solution is possible, don't do anything
             m_pivot.stop();
+            m_elevation.stop();
             return;
         }
-        Rotation2d absoluteBearing = soln.get();
+        Rotation2d absoluteBearing = soln.get().azimuth;
         Rotation2d relativeBearing = absoluteBearing.minus(m_state.get().rotation());
         m_pivot.setPositionProfiled(relativeBearing.getRadians(), 0);
+        m_elevation.setPositionProfiled(soln.get().elevation.getRadians(), 0);
     }
 
-    private Optional<Rotation2d> getSolution() {
+    private Optional<Solution> getSolution() {
+        if (Experiments.instance.enabled(Experiment.TurretShootingMethod)) {
+            // Use 3d intercept logic.
+            return getShootingMethod();
+        }
         if (Experiments.instance.enabled(Experiment.TurretIntercept)) {
+            // Use 2d intercept logic.
             return getAbsoluteBearingForIntercept();
         }
+        // For shining a flashlight at the target.
         return getAbsoluteBearingInstantaneous();
     }
 
     /**
      * Compute absolute bearing from robot to target without compensating for the
      * motion of either one.
      */
-    private Optional<Rotation2d> getAbsoluteBearingInstantaneous() {
+    private Optional<Solution> getAbsoluteBearingInstantaneous() {
         ModelR3 state = m_state.get();
         Translation2d target = m_target.get();
-        return Optional.of(target.minus(state.translation()).getAngle());
+        return Optional.of(
+                new Solution(
+                        target.minus(state.translation()).getAngle(),
+                        Rotation2d.kZero));
+    }
+
+    private Optional<Solution> getShootingMethod() {
+        ModelR3 state = m_state.get();
+        Translation2d robotPosition = state.translation();
+        GlobalVelocityR2 robotVelocity = state.velocityR2();
+        Translation2d targetPosition = m_target.get();
+        GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO;
+        Optional<ShootingMethod.Solution> s = m_shootingMethod.solve(
+                robotPosition, robotVelocity, targetPosition, targetVelocity);
+        return s.map(x -> new Solution(x.azimuth(), x.elevation()));
     }
 
     /**
      * Compute absolute bearing to the intercept point, given moving target and
      * moving robot.
      */
-    private Optional<Rotation2d> getAbsoluteBearingForIntercept() {
+    private Optional<Solution> getAbsoluteBearingForIntercept() {
         ModelR3 state = m_state.get();
 
         Translation2d robotPosition = state.translation();
@@ -144,12 +207,18 @@ private Optional<Rotation2d> getAbsoluteBearingForIntercept() {
         // for now, the target is assumed to be motionless
         // TODO: target tracking to derive velocity.
         GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO;
-        return m_intercept.intercept(
+        Optional<Rotation2d> azimuth = m_intercept.intercept(
                 robotPosition,
                 robotVelocity,
                 targetPosition,
                 targetVelocity,
                 m_speed);
+        if (azimuth.isEmpty())
+            return Optional.empty();
+        return Optional.of(
+                new Solution(
+                        azimuth.get(),
+                        Rotation2d.kZero));
     }
 
     private void stopAiming() {