diff --git a/lib/src/main/java/org/team100/lib/experiments/Experiment.java b/lib/src/main/java/org/team100/lib/experiments/Experiment.java index 53e0843e..a544d12d 100644 --- a/lib/src/main/java/org/team100/lib/experiments/Experiment.java +++ b/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 } diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java index ed10a645..00adcbe7 100644 --- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java +++ b/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 toVec(Twist2d twist) { public static Vector toVec(Twist3d twist) { return VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz); } + + public static Vector toVec(Translation2d t) { + return VecBuilder.fill(t.getX(), t.getY()); + } } diff --git a/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java b/lib/src/main/java/org/team100/lib/geometry/GlobalVelocity3d.java new file mode 100644 index 00000000..8aeced8d --- /dev/null +++ b/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); + } + +} diff --git a/lib/src/main/java/org/team100/lib/kinematics/urdf/URDFRobot.java b/lib/src/main/java/org/team100/lib/kinematics/urdf/URDFRobot.java index 51922951..0395595d 100644 --- a/lib/src/main/java/org/team100/lib/kinematics/urdf/URDFRobot.java +++ b/lib/src/main/java/org/team100/lib/kinematics/urdf/URDFRobot.java @@ -132,7 +132,7 @@ public Map inverse( minQ(m_qDim), maxQ(m_qDim), tolerance, iterations, dqLimit); long startTime = System.nanoTime(); - Vector qVec = solver.solve2(q0, restarts); + Vector qVec = solver.solve2(q0, restarts, true); if (DEBUG) { long finishTime = System.nanoTime(); diff --git a/lib/src/main/java/org/team100/lib/optimization/Bisection1d.java b/lib/src/main/java/org/team100/lib/optimization/Bisection1d.java index 6e81f7d3..231bab32 100644 --- a/lib/src/main/java/org/team100/lib/optimization/Bisection1d.java +++ b/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; diff --git a/lib/src/main/java/org/team100/lib/optimization/NewtonsMethod.java b/lib/src/main/java/org/team100/lib/optimization/NewtonsMethod.java index f9274a63..a1541904 100644 --- a/lib/src/main/java/org/team100/lib/optimization/NewtonsMethod.java +++ b/lib/src/main/java/org/team100/lib/optimization/NewtonsMethod.java @@ -47,7 +47,7 @@ public class NewtonsMethod { * 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 solve(Vector 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 solve2(Vector initialX, int restarts) { + public Vector solve2(Vector 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 solve2(Vector 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(); diff --git a/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java b/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java index 17c8ad57..bd16f7bc 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java +++ b/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 m_state; private final Supplier 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,23 +133,35 @@ 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 soln = getSolution(); + Optional 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 getSolution() { + private Optional 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(); } @@ -120,17 +169,31 @@ private Optional getSolution() { * Compute absolute bearing from robot to target without compensating for the * motion of either one. */ - private Optional getAbsoluteBearingInstantaneous() { + private Optional 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 getShootingMethod() { + ModelR3 state = m_state.get(); + Translation2d robotPosition = state.translation(); + GlobalVelocityR2 robotVelocity = state.velocityR2(); + Translation2d targetPosition = m_target.get(); + GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO; + Optional 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 getAbsoluteBearingForIntercept() { + private Optional getAbsoluteBearingForIntercept() { ModelR3 state = m_state.get(); Translation2d robotPosition = state.translation(); @@ -144,12 +207,18 @@ private Optional 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 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() { diff --git a/lib/src/main/java/org/team100/lib/targeting/ShootingMethod.java b/lib/src/main/java/org/team100/lib/targeting/ShootingMethod.java new file mode 100644 index 00000000..954ac2cd --- /dev/null +++ b/lib/src/main/java/org/team100/lib/targeting/ShootingMethod.java @@ -0,0 +1,88 @@ +package org.team100.lib.targeting; + +import java.util.Optional; +import java.util.function.Function; + +import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.geometry.GlobalVelocityR2; +import org.team100.lib.optimization.NewtonsMethod; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N2; + +/** + * Solve the intercept problem via the "shooting method" for fixed muzzle + * velocity and spin. + */ +public class ShootingMethod { + public record Solution(Rotation2d azimuth, Rotation2d elevation) { + } + + private final Range m_range; + private final double m_tolerance; + + public ShootingMethod(Range range, double tolerance) { + m_range = range; + m_tolerance = tolerance; + } + + public Optional solve( + Translation2d robotPosition, + GlobalVelocityR2 robotVelocity, + Translation2d targetPosition, + GlobalVelocityR2 targetVelocity) { + // target relative to robot + Translation2d T0 = targetPosition.minus(robotPosition); + // target velocity relative to robot + GlobalVelocityR2 vT = targetVelocity.minus(robotVelocity); + + // domain is (az,el), range is error + Vector xMin = VecBuilder.fill(-Math.PI, 0); + Vector xMax = VecBuilder.fill(Math.PI, Math.PI / 2); + int iterations = 10; + double dxLimit = 0.1; + NewtonsMethod solver = new NewtonsMethod<>( + Nat.N2(), + Nat.N2(), + fn(T0, vT), + xMin, + xMax, + m_tolerance, + iterations, + dxLimit); + try { + Vector x = solver.solve2(VecBuilder.fill(0, 0), 3, true); + return Optional.of( + new Solution( + new Rotation2d(x.get(0)), + new Rotation2d(x.get(1)))); + } catch (IllegalArgumentException ex) { + return Optional.empty(); + } + } + + /** Curry */ + Function, Vector> fn(Translation2d T0, GlobalVelocityR2 vT) { + return x -> this.f(x, T0, vT); + } + + Vector f(Vector x, Translation2d T0, GlobalVelocityR2 vT) { + // Extract contents of the state variable + Rotation2d azimuth = new Rotation2d(x.get(0)); + double elevation = x.get(1); + // Lookup for this state. + Range.Solution rangeSolution = m_range.get(elevation); + // Ball location at impact, relative to initial robot position + Translation2d b = new Translation2d(rangeSolution.range(), azimuth); + // target location at impact, relative to initial robot position + Translation2d T = vT.integrate(T0, rangeSolution.tof()); + Translation2d err = b.minus(T); + // result error is (x, y) + return GeometryUtil.toVec(err); + } + +} diff --git a/lib/src/main/java/org/team100/lib/util/Math100.java b/lib/src/main/java/org/team100/lib/util/Math100.java index 13ff1d5a..fecc12e7 100644 --- a/lib/src/main/java/org/team100/lib/util/Math100.java +++ b/lib/src/main/java/org/team100/lib/util/Math100.java @@ -10,7 +10,6 @@ * Various math utilities. */ public class Math100 { - public static final boolean DEBUG = false; private static final double EPSILON = 1e-6; /** diff --git a/lib/src/main/java/org/team100/lib/visualization/Ball.java b/lib/src/main/java/org/team100/lib/visualization/Ball.java index 3741a1b8..92f21a2f 100644 --- a/lib/src/main/java/org/team100/lib/visualization/Ball.java +++ b/lib/src/main/java/org/team100/lib/visualization/Ball.java @@ -1,79 +1,23 @@ package org.team100.lib.visualization; -import java.util.function.Supplier; - -import org.team100.lib.framework.TimedRobot100; -import org.team100.lib.geometry.GlobalVelocityR2; -import org.team100.lib.logging.Level; -import org.team100.lib.logging.LoggerFactory; -import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; -import org.team100.lib.state.ModelR3; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -/** - * Simulated projectile. - * - * Provides Field2d visualization using the name "ball". - */ -public class Ball { - private static final double DT = TimedRobot100.LOOP_PERIOD_S; - private final DoubleArrayLogger m_log_field_ball; - private final Supplier m_robot; - private final Supplier m_azimuth; - /** Projectile speed m/s */ - private final double m_speed; - - // null when contained in robot. - private Translation2d m_location; - private GlobalVelocityR2 m_velocity; +public interface Ball { + /** Sets initial position and velocity. */ + void launch(); - public Ball( - LoggerFactory field, - Supplier robot, - Supplier azimuth, - double speed) { - m_log_field_ball = field.doubleArrayLogger(Level.COMP, "ball"); - m_robot = robot; - m_azimuth = azimuth; - m_speed = speed; - } + /** Evolves state one time step. */ + void fly(); - private void launch() { - // velocity due only to the gun - GlobalVelocityR2 v = GlobalVelocityR2.fromPolar(m_azimuth.get(), m_speed); - // velocity due to robot translation - GlobalVelocityR2 mv = GlobalVelocityR2.fromSe2(m_robot.get().velocity()); - // initial position - m_location = m_robot.get().pose().getTranslation(); - // initial velocity - m_velocity = v.plus(mv); - } + void reset(); - private void fly() { - // evolve state with zero acceleration - m_location = m_velocity.integrate(m_location, DT); - } - - private void reset() { - m_location = null; - } + void periodic(); /** Shoot the ball and continue its path as long as the command runs. */ - public Command shoot() { + default Command shoot() { return Commands.startRun(this::launch, this::fly) .finallyDo(this::reset); } - public void periodic() { - m_log_field_ball.log(this::poseArray); - } - - private double[] poseArray() { - Translation2d t = m_location == null ? m_robot.get().translation() : m_location; - return new double[] { t.getX(), t.getY(), 0 }; - } } diff --git a/lib/src/main/java/org/team100/lib/visualization/BallFactory.java b/lib/src/main/java/org/team100/lib/visualization/BallFactory.java new file mode 100644 index 00000000..94e8b68c --- /dev/null +++ b/lib/src/main/java/org/team100/lib/visualization/BallFactory.java @@ -0,0 +1,33 @@ +package org.team100.lib.visualization; + +import java.util.function.Supplier; + +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.state.ModelR3; +import org.team100.lib.targeting.Drag; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class BallFactory { + + public static Ball get2d( + LoggerFactory field, + Supplier robot, + Supplier azimuth, + double speed) { + return new BallR2(field, robot, azimuth, speed); + + } + + public static Ball get3d( + LoggerFactory field, + Supplier robot, + Supplier azimuth, + Supplier elevation, + double speed, + double omega) { + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + return new BallR3(field, d, robot, azimuth, elevation, speed, omega); + } + +} diff --git a/lib/src/main/java/org/team100/lib/visualization/BallR2.java b/lib/src/main/java/org/team100/lib/visualization/BallR2.java new file mode 100644 index 00000000..72c4c1b6 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/visualization/BallR2.java @@ -0,0 +1,88 @@ +package org.team100.lib.visualization; + +import java.util.function.Supplier; + +import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.geometry.GlobalVelocityR2; +import org.team100.lib.logging.Level; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; +import org.team100.lib.state.ModelR3; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * Simulated projectile in XY plane, uses constant velocity, continues forever. + * + * Provides Field2d visualization using the name "ball". + */ +public class BallR2 implements Ball { + private static final double DT = TimedRobot100.LOOP_PERIOD_S; + private final DoubleArrayLogger m_log_field_ball; + private final Supplier m_robot; + private final Supplier m_azimuth; + /** Projectile speed m/s */ + private final double m_speed; + + // null when contained in robot. + Translation2d m_location; + GlobalVelocityR2 m_velocity; + + /** + * @param field log + * @param robot state (pose2d, velocityR3) + * @param azimuth absolute + * @param speed muzzle speed + */ + public BallR2( + LoggerFactory field, + Supplier robot, + Supplier azimuth, + double speed) { + m_log_field_ball = field.doubleArrayLogger(Level.COMP, "ball"); + m_robot = robot; + m_azimuth = azimuth; + m_speed = speed; + } + + @Override + public void launch() { + // Velocity due only to the gun + GlobalVelocityR2 v = GlobalVelocityR2.fromPolar(m_azimuth.get(), m_speed); + // Velocity due to robot translation + GlobalVelocityR2 mv = GlobalVelocityR2.fromSe2(m_robot.get().velocity()); + // Initial position is at the robot center. TODO: offsets. + m_location = m_robot.get().pose().getTranslation(); + // Initial velocity. + m_velocity = v.plus(mv); + } + + @Override + public void fly() { + m_location = m_velocity.integrate(m_location, DT); + } + + @Override + public void reset() { + m_location = null; + } + + @Override + public void periodic() { + m_log_field_ball.log(this::poseArray); + } + + private double[] poseArray() { + Translation2d t = location(); + return new double[] { t.getX(), t.getY(), 0 }; + } + + private Translation2d location() { + if (m_location == null) { + // The ball is riding with the robot. + return m_robot.get().translation(); + } + return m_location; + } +} diff --git a/lib/src/main/java/org/team100/lib/visualization/BallR3.java b/lib/src/main/java/org/team100/lib/visualization/BallR3.java new file mode 100644 index 00000000..2a2c33a3 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/visualization/BallR3.java @@ -0,0 +1,136 @@ +package org.team100.lib.visualization; + +import java.util.function.Supplier; + +import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.geometry.GlobalVelocity3d; +import org.team100.lib.logging.Level; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger; +import org.team100.lib.state.ModelR3; +import org.team100.lib.targeting.Drag; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N6; +import edu.wpi.first.math.system.NumericalIntegration; + +/** + * Simulated projectile in three dimensions + */ +public class BallR3 implements Ball { + private static final double DT = TimedRobot100.LOOP_PERIOD_S; + private final DoubleArrayLogger m_log_field_ball; + private final Drag m_drag; + private final Supplier m_robot; + private final Supplier m_azimuth; + private final Supplier m_elevation; + private final double m_speed; + private final double m_omega; + + // null when contained in robot. + // robot location at launch + Translation3d m_location; + // current state for drag model + Matrix m_x; + // azimuth at launch + Rotation2d m_az; + + /** + * @param field log + * @param drag drag model + * @param robot state (pose2d, velocityR3) + * @param azimuth absolute + * @param elevation absolute + * @param speed muzzle speed + * @param omega spin + */ + public BallR3( + LoggerFactory field, + Drag drag, + Supplier robot, + Supplier azimuth, + Supplier elevation, + double speed, + double omega) { + m_log_field_ball = field.doubleArrayLogger(Level.COMP, "ball"); + m_drag = drag; + m_robot = robot; + m_azimuth = azimuth; + m_elevation = elevation; + m_speed = speed; + m_omega = omega; + } + + @Override + public void launch() { + // Velocity due only to the gun + GlobalVelocity3d v = GlobalVelocity3d.fromPolar( + m_azimuth.get(), m_elevation.get(), m_speed); + // velocity due to robot translation + GlobalVelocity3d mv = GlobalVelocity3d.fromSe2(m_robot.get().velocity()); + // Initial position is on the floor. TODO: offsets. + m_location = new Translation3d(m_robot.get().pose().getTranslation()); + GlobalVelocity3d m_velocity = v.plus(mv); + // velocity in the XY plane + double vxy = m_velocity.normXY(); + double vz = m_velocity.z(); + m_x = VecBuilder.fill(0, 0, 0, vxy, vz, m_omega); + m_az = m_azimuth.get(); + } + + @Override + public void fly() { + Matrix x = NumericalIntegration.rk4(m_drag, m_x, DT); + if (x.get(1, 0) >= 0) { + // only update if above the floor + m_x = x; + } + } + + @Override + public void reset() { + m_location = null; + m_x = null; + } + + @Override + public void periodic() { + m_log_field_ball.log(this::poseArray); + } + + private double[] poseArray() { + Translation3d t = location(); + return new double[] { t.getX(), t.getY(), 0 }; + } + + Translation3d location() { + if (m_x == null) { + // The ball is riding with the robot. + return new Translation3d(m_robot.get().translation()); + } + double xy = m_x.get(0, 0); + double z = m_x.get(1, 0); + Translation3d relative = new Translation3d( + xy * m_az.getCos(), + xy * m_az.getSin(), + z); + return m_location.plus(relative); + } + + // for testing + GlobalVelocity3d velocity() { + if (m_x == null) { + return GlobalVelocity3d.fromSe2(m_robot.get().velocity()); + } + double vxy = m_x.get(3, 0); + double vz = m_x.get(4, 0); + return new GlobalVelocity3d( + vxy * m_az.getCos(), + vxy * m_az.getSin(), + vz); + } +} diff --git a/lib/src/test/java/org/team100/lib/kinematics/urdf/URDFCartesianTest.java b/lib/src/test/java/org/team100/lib/kinematics/urdf/URDFCartesianTest.java index 10d7b237..3be8a346 100644 --- a/lib/src/test/java/org/team100/lib/kinematics/urdf/URDFCartesianTest.java +++ b/lib/src/test/java/org/team100/lib/kinematics/urdf/URDFCartesianTest.java @@ -1,6 +1,7 @@ package org.team100.lib.kinematics.urdf; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; import java.util.Map; @@ -55,13 +56,9 @@ void testInvBeyondLimit() { Pose3d end = new Pose3d(1.5, 0.5, 0.05, new Rotation3d()); // initial position is just somewhere random Vector q0 = VecBuilder.fill(0.1, 0.1, 0.1); - Map qMap = m.inverse( - q0, 1, "center_point", end); - assertEquals(3, qMap.size()); - assertEquals(0.5, qMap.get("base_gantry"), 1e-3); - // this is the coordinate that should be limited. - assertEquals(1.0, qMap.get("gantry_head"), 1e-3); - assertEquals(0.15, qMap.get("head_spindle"), 1e-3); + // solution is not possible + assertThrows(IllegalArgumentException.class, () -> m.inverse( + q0, 1, "center_point", end)); } /** 52 us per solve on my laptop, 2 solver iterations each. */ diff --git a/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java b/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java index 49b9b83a..3721d7d0 100644 --- a/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java +++ b/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java @@ -358,7 +358,7 @@ void testLinear1d() { NewtonsMethod s = new NewtonsMethod<>(Nat.N1(), Nat.N1(), f, minQ, maxQ, 1e-3, 10, 1); // f(-1) = -1 + 1 = 0 - Vector x = s.solve2(q0, 1); + Vector x = s.solve2(q0, 1, true); assertEquals(-1, x.get(0), 1e-3); } @@ -374,7 +374,7 @@ void testLinear1dPerformance() { int iter = 0; int maxIter = 100000; for (iter = 0; iter < maxIter; ++iter) { - s.solve2(q0, 1); + s.solve2(q0, 1, true); } long finishTime = System.nanoTime(); if (DEBUG) { @@ -393,7 +393,7 @@ void testQuadratic1d() { Vector minQ = VecBuilder.fill(-10); Vector maxQ = VecBuilder.fill(10); NewtonsMethod s = new NewtonsMethod<>(Nat.N1(), Nat.N1(), f, minQ, maxQ, 1e-3, 10, 1); - Vector x = s.solve2(q0, 1); + Vector x = s.solve2(q0, 1, true); // f(1.414) = 2 - 2 = 0 assertEquals(1.414, x.get(0), 1e-3); } @@ -429,7 +429,7 @@ void test4Pose2Solver2() { Vector minQ = VecBuilder.fill(-Math.PI, -Math.PI); Vector maxQ = VecBuilder.fill(Math.PI, Math.PI); NewtonsMethod s = new NewtonsMethod<>(Nat.N2(), Nat.N3(), err, minQ, maxQ, 1e-3, 10, 1); - Vector x = s.solve2(q0, 5); + Vector x = s.solve2(q0, 5, true); assertEquals(0.524, x.get(0), 1e-3); assertEquals(2.094, x.get(1), 1e-3); } @@ -612,7 +612,7 @@ void test62() { Vector minQ = VecBuilder.fill(-Math.PI, -Math.PI); Vector maxQ = VecBuilder.fill(Math.PI, Math.PI); NewtonsMethod s = new NewtonsMethod<>(Nat.N2(), Nat.N2(), err, minQ, maxQ, 1e-3, 10, 1); - Vector x = s.solve2(q0, 5); + Vector x = s.solve2(q0, 5, true); assertEquals(0.524, x.get(0), 1e-3); assertEquals(2.094, x.get(1), 1e-3); } @@ -635,7 +635,7 @@ void test72() { int iterations = 50000; long startTime = System.currentTimeMillis(); for (int i = 0; i < iterations; ++i) { - s.solve2(q0, 5); + s.solve2(q0, 5, true); } long finishTime = System.currentTimeMillis(); if (DEBUG) { @@ -685,7 +685,7 @@ void testConvergence() { 1.4888289270e-04); Vector q0 = c.toVec(); long startTime = System.nanoTime(); - solver.solve2(q0, restarts); + solver.solve2(q0, restarts, true); if (DEBUG) { long finishTime = System.nanoTime(); System.out.printf("ET (ms): %6.3f\n", diff --git a/lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java b/lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java new file mode 100644 index 00000000..34e3e436 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java @@ -0,0 +1,134 @@ +package org.team100.lib.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.Optional; + +import org.junit.jupiter.api.Test; +import org.team100.lib.geometry.GlobalVelocityR2; + +import edu.wpi.first.math.geometry.Translation2d; + +public class ShootingMethodTest { + private static final double DELTA = 0.001; + + @Test + void testMotionless() { + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + Range range = new Range(d, 10, 0); + // tight tolerance for testing + ShootingMethod m = new ShootingMethod(range, 0.0001); + Translation2d robotPosition = new Translation2d(); + GlobalVelocityR2 robotVelocity = GlobalVelocityR2.ZERO; + // target is 2m away along +x + Translation2d targetPosition = new Translation2d(2, 0); + GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO; + Optional o = m.solve( + robotPosition, robotVelocity, targetPosition, targetVelocity); + ShootingMethod.Solution x = o.orElseThrow(); + assertEquals(0, x.azimuth().getRadians(), 0.000001); + assertEquals(0.156812, x.elevation().getRadians(), 0.000001); + // check the range solution + Range.Solution s = range.get(x.elevation().getRadians()); + assertEquals(1.999999, s.range(), 0.000001); + assertEquals(0.2789, s.tof(), 0.0001); + } + + @Test + void testTowardsTarget() { + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + Range range = new Range(d, 10, 0); + // tight tolerance for testing + ShootingMethod m = new ShootingMethod(range, 0.0001); + Translation2d robotPosition = new Translation2d(); + // driving towards the target + GlobalVelocityR2 robotVelocity = new GlobalVelocityR2(1, 0); + // target is 2m away along +x + Translation2d targetPosition = new Translation2d(2, 0); + GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO; + Optional o = m.solve( + robotPosition, robotVelocity, targetPosition, targetVelocity); + ShootingMethod.Solution x = o.orElseThrow(); + assertEquals(0, x.azimuth().getRadians(), 0.000001); + // lower elevation + assertEquals(0.129574, x.elevation().getRadians(), 0.000001); + // check the range solution + Range.Solution s = range.get(x.elevation().getRadians()); + // closer range + assertEquals(1.764723, s.range(), 0.000001); + // less time + assertEquals(0.235276, s.tof(), 0.0001); + } + + @Test + void testAwayFromTarget() { + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + Range range = new Range(d, 10, 0); + // tight tolerance for testing + ShootingMethod m = new ShootingMethod(range, 0.0001); + Translation2d robotPosition = new Translation2d(); + // driving away from the target + GlobalVelocityR2 robotVelocity = new GlobalVelocityR2(-2, 0); + // target is 2m away along +x + Translation2d targetPosition = new Translation2d(2, 0); + GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO; + Optional o = m.solve( + robotPosition, robotVelocity, targetPosition, targetVelocity); + ShootingMethod.Solution x = o.orElseThrow(); + assertEquals(0, x.azimuth().getRadians(), 0.000001); + // higher elevation + assertEquals(0.386036, x.elevation().getRadians(), 0.000001); + // check the range solution + Range.Solution s = range.get(x.elevation().getRadians()); + // longer range + assertEquals(3.193693, s.range(), 0.000001); + // more time + assertEquals(0.596846, s.tof(), 0.0001); + } + + @Test + void testImpossible() { + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + Range range = new Range(d, 10, 0); + // tight tolerance for testing + ShootingMethod m = new ShootingMethod(range, 0.0001); + Translation2d robotPosition = new Translation2d(); + // driving fast away from the target + GlobalVelocityR2 robotVelocity = new GlobalVelocityR2(-10, 0); + // target is 2m away along +x + Translation2d targetPosition = new Translation2d(2, 0); + GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO; + Optional o = m.solve( + robotPosition, robotVelocity, targetPosition, targetVelocity); + assertTrue(o.isEmpty()); + } + + @Test + void testStrafing() { + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + Range range = new Range(d, 10, 0); + // tight tolerance for testing + ShootingMethod m = new ShootingMethod(range, 0.0001); + Translation2d robotPosition = new Translation2d(); + // driving to the left + GlobalVelocityR2 robotVelocity = new GlobalVelocityR2(0, 2); + // target is 2m away along +x + Translation2d targetPosition = new Translation2d(2, 0); + GlobalVelocityR2 targetVelocity = GlobalVelocityR2.ZERO; + Optional o = m.solve( + robotPosition, robotVelocity, targetPosition, targetVelocity); + ShootingMethod.Solution x = o.orElseThrow(); + // lag the target (to the right) + assertEquals(-0.287561, x.azimuth().getRadians(), 0.000001); + // a bit higher elevation + assertEquals(0.167844, x.elevation().getRadians(), 0.000001); + // check the range solution + Range.Solution s = range.get(x.elevation().getRadians()); + // a bit longer range + assertEquals(2.085642, s.range(), 0.000001); + // a bit more time + assertEquals(0.295766, s.tof(), 0.0001); + } + +} diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java index 77349a1d..9d365edc 100755 --- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java @@ -21,7 +21,7 @@ class Path100Test { private static final double DELTA = 0.001; - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final List HEADINGS = Arrays.asList( GeometryUtil.fromDegrees(0), diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java index 2292390c..9a73f4ca 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java +++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java @@ -27,7 +27,7 @@ import edu.wpi.first.math.geometry.Translation2d; class HolonomicSplineTest implements Timeless { - private static final boolean DEBUG = true; + private static final boolean DEBUG = false; private static final double DELTA = 0.001; private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger()); diff --git a/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java b/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java new file mode 100644 index 00000000..cc64e887 --- /dev/null +++ b/lib/src/test/java/org/team100/lib/visualization/BallR2Test.java @@ -0,0 +1,43 @@ +package org.team100.lib.visualization; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNull; + +import org.junit.jupiter.api.Test; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.state.ModelR3; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class BallR2Test { + private static final boolean DEBUG = false; + + ModelR3 robot = new ModelR3(); + Rotation2d azimuth = new Rotation2d(); + + /** 2d ball proceeds forever. */ + @Test + void testBall() { + LoggerFactory field = new TestLoggerFactory(new TestPrimitiveLogger()); + BallR2 b = new BallR2(field, () -> robot, () -> azimuth, 1); + assertNull(b.m_location); + assertNull(b.m_velocity); + b.launch(); + assertEquals(0, b.m_location.getX()); + assertEquals(0, b.m_location.getY()); + assertEquals(1, b.m_velocity.x()); + assertEquals(0, b.m_velocity.y()); + for (double t = 0; t < 10; t += 0.02) { + b.fly(); + if (DEBUG) + System.out.printf("%f %f %f %f\n", + b.m_location.getX(), + b.m_location.getY(), + b.m_velocity.x(), + b.m_velocity.y()); + } + } + +} diff --git a/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java b/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java new file mode 100644 index 00000000..ae9ecb8f --- /dev/null +++ b/lib/src/test/java/org/team100/lib/visualization/BallR3Test.java @@ -0,0 +1,54 @@ +package org.team100.lib.visualization; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNull; + +import org.junit.jupiter.api.Test; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.state.ModelR3; +import org.team100.lib.targeting.Drag; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class BallR3Test { + private static final boolean DEBUG = false; + private static final double DELTA = 0.001; + + ModelR3 robot = new ModelR3(); + Rotation2d azimuth = new Rotation2d(); + Rotation2d elevation = new Rotation2d(); + + @Test + void testBall() { + LoggerFactory field = new TestLoggerFactory(new TestPrimitiveLogger()); + Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1); + BallR3 b = new BallR3(field, d, () -> robot, () -> azimuth, () -> elevation, 10, 1); + assertNull(b.m_location); + elevation = new Rotation2d(Math.PI / 4); + b.launch(); + assertEquals(0, b.location().getX(), DELTA); + assertEquals(0, b.location().getY(), DELTA); + assertEquals(0, b.location().getZ(), DELTA); + assertEquals(7.071, b.velocity().x(), DELTA); + assertEquals(0, b.velocity().y(), DELTA); + assertEquals(7.071, b.velocity().z(), DELTA); + if (DEBUG) + System.out.println("t, x, y, z, vx, vy, vz"); + for (double t = 0; t < 1.1; t += 0.02) { + if (DEBUG) { + System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n", + t, + b.location().getX(), + b.location().getY(), + b.location().getZ(), + b.velocity().x(), + b.velocity().y(), + b.velocity().z()); + } + b.fly(); + } + } + +} diff --git a/studies/ballerina/src/main/java/org/team100/ballerina/Robot.java b/studies/ballerina/src/main/java/org/team100/ballerina/Robot.java index 665e6ee0..1c03a013 100644 --- a/studies/ballerina/src/main/java/org/team100/ballerina/Robot.java +++ b/studies/ballerina/src/main/java/org/team100/ballerina/Robot.java @@ -13,6 +13,7 @@ import org.team100.lib.util.Banner; import org.team100.lib.util.RoboRioChannel; import org.team100.lib.visualization.Ball; +import org.team100.lib.visualization.BallFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,7 +24,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; public class Robot extends TimedRobot { - private final double SPEED = 3; + private static final boolean BALL_2D = false; + private final double SPEED = 10; private final DriverXboxControl m_controller; private final Turret m_turret; private final ManualPose m_pose; @@ -43,8 +45,20 @@ public Robot() { m_turret = new Turret(rootLogger, fieldLogger, m_pose::getState, m_target::getTarget, SPEED); m_indicator = new SolidIndicator(new RoboRioChannel(0), 40); m_indicator.state(this::indicatorState); - m_ball = new Ball(fieldLogger, m_pose::getState, m_turret::getAzimuth, SPEED); - + if (BALL_2D) { + m_ball = BallFactory.get2d( + fieldLogger, m_pose::getState, m_turret::getAzimuth, SPEED); + } else { + // Always throws knuckleballs + // TODO: adjustable spin. + m_ball = BallFactory.get3d( + fieldLogger, + m_pose::getState, + m_turret::getAzimuth, + m_turret::getElevation, + SPEED, + 0); + } // button 1 new Trigger(m_controller::a).onTrue(m_target.a()); // button 2