public class ShooterPhysics {
// pitch in radians, going up from the horizontal
// exit velocity speed in m/s
- public record TurretState(Rotation2d yaw, double pitch, double exitVel, double height) {
+ public record TurretState(Rotation2d yaw, double pitch, double exitVel, double height, double timeOfFlight) {
public boolean satisfies(Constraints constraints) {
if (height < constraints.height())
return false;
}
};
- // note: minPitch must be < 75°
+ /**
+ * Specifies the constraints of the shot.
+ *
+ * @param height The minimum apex height of the shot.
+ * @param maxVel The maximum exit velocity of the shooter
+ * @param minPitch The minimum pitch covered by the hood
+ * @param maxPitch The maximum pitch covered by the hood
+ */
public record Constraints(double height, double maxVel, double minPitch, double maxPitch) {
- // performs some sanity checks
- public boolean check() {
- if (height <= 0)
- return false;
- if (maxVel <= 0)
- return false;
- if (minPitch <= 0)
- return false;
- if (maxPitch >= Math.PI / 2)
- return false;
- if (minPitch >= maxPitch)
- return false;
-
- // this causes problems and isn't likely to actually occur
- if (minPitch > Units.degreesToRadians(75))
- return false;
-
- return true;
- }
};
/**
- * Calculates shot parameters for SOTM for a peak height using Physics™.
+ * Calculates shot parameters for SOTM using Physics™.
*
* @param robotVelocity The x and y velocity of the robot, field relative.
* @param robotToTarget The robot to target transform. Angles are field
* relative, positions are with the robot as the origin.
- * @param height The peak height the trajectory should reach.
+ * @param peakHeight The peak height the trajectory should reach.
* @return A TurretState that represents the shot the robot should take.
*/
public static TurretState getShotParams(Translation2d robotVelocity, Translation3d robotToTarget,
- double height) {
- Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height);
- return cvtShot(exitVel, height);
- }
-
- // provided for backwards compatability
- public static Pair<TurretState, Double> getShotParamsWithT(Translation2d robotVelocity, Translation3d robotToTarget,
- double height) {
- Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height);
- double t = (-exitVel.getZ()
- - Math.sqrt(Math.pow(exitVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ()))
- / -Constants.GRAVITY_ACCELERATION;
- return new Pair<TurretState, Double>(cvtShot(exitVel, height), t);
+ double peakHeight) {
+ double zExitVel = Math.sqrt(2 * peakHeight * Constants.GRAVITY_ACCELERATION);
+ double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, peakHeight);
+ return cvtShot(exitVel, peakHeight, t);
}
/**
- * Calculates shot parameters for SOTM for a peak height using Physics™.
+ * Calculates shot parameters for stationary shooting using Physics™.
*
- * @param robotVelocity The x and y velocity of the robot, field relative.
* @param robotToTarget The robot to target transform. Angles are field
* relative, positions are with the robot as the origin.
- * @param constraints The constraints on the shooter.
+ * @param peakHeight The peak height the trajectory should reach.
* @return A TurretState that represents the shot the robot should take.
*/
+ public static TurretState getShotParams(Translation3d robotToTarget, double peakHeight) {
+ return getShotParams(new Translation2d(0, 0), robotToTarget, peakHeight);
+ }
+
+ public static Optional<TurretState> getConstrainedParams(Translation3d robotToTarget, Constraints constraints){
+ return getConstrainedParams(new Translation2d(0, 0), robotToTarget, constraints);
+ }
+
public static Optional<TurretState> getConstrainedParams(Translation2d robotVelocity, Translation3d robotToTarget,
Constraints constraints) {
- if (!constraints.check())
- throw new IllegalArgumentException("Provided constraints are invalid (" + constraints + ").");
-
// establish a lower bound
- double minHeight = Math.max(Math.max(robotToTarget.getZ(), constraints.height()), 0.01);
+ double minHeight = Math.max(robotToTarget.getZ(), constraints.height());
Optional<TurretState> withMinPitch = withAngle(robotVelocity, robotToTarget, constraints.minPitch());
- if (withMinPitch.isPresent())
- minHeight = Math.max(minHeight, withMinPitch.get().height());
-
- TurretState withMinHeight = getShotParams(robotVelocity, robotToTarget, minHeight);
+ if (withMinPitch.isPresent()) {
+ minHeight = Math.min(minHeight, withMinPitch.get().height());
+ }
+ Translation3d minVel = getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight);
+ double minT = (-minVel.getZ() - Math.sqrt(Math.pow(minVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ TurretState withMinHeight = cvtShot(minVel, minHeight, minT);
if (withMinHeight.satisfies(constraints))
return Optional.of(withMinHeight);
- TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget);
- if (withMinSpeed.exitVel() > constraints.maxVel())
+ // the only reason this is empty is if the highest posible trajectory is too low
+ // to intersect the goal
+ Optional<TurretState> withMaxPitchOpt = withAngle(robotVelocity, robotToTarget, constraints.minPitch());
+ if (withMaxPitchOpt.isEmpty())
return Optional.empty();
+ TurretState withMaxPitch = withMaxPitchOpt.get();
+
+ // the range from withMinHeight to withMaxPitch will satisfy pitch and height
+ // constraints
+ // now we need to satisfy the speed constraint
+
+ TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget);
+ // ordered such that the first element is valid and the second is not
+ Pair<TurretState, TurretState> newRange;
if (withMinSpeed.height() < withMinHeight.height()) {
// the minimum speed is below the lower bound, but doesn't satisfy constraints
return Optional.empty();
- } else {
- // the first element is lower than the second, the first satisfies the angle but
- // not the velocity and the second satisfies the velocity but may or may not
- // satisfy the angle
- var newRange = new Pair<TurretState, TurretState>(withMinHeight, withMinSpeed);
-
- // now we binary search the new range to find the lowest value that satisfies
- // the velocity constraint, we know velocity is decreasing on the interval
-
- // use a 1cm tolerance
- while (Math.abs(newRange.getFirst().height() - newRange.getSecond().height()) > .01) {
- double avgHeight = (newRange.getFirst().height() + newRange.getSecond().height()) / 2;
- TurretState guess = getShotParams(robotVelocity, robotToTarget, avgHeight);
- if (guess.exitVel() > constraints.maxVel())
- newRange = new Pair<TurretState, TurretState>(guess, newRange.getSecond());
- else
- newRange = new Pair<TurretState, TurretState>(newRange.getFirst(), guess);
-
- }
- if (newRange.getSecond().satisfies(constraints))
- return Optional.of(newRange.getSecond());
+ } else if (withMinSpeed.height() > withMaxPitch.height()) {
+ // the minimum speed is above the upper bound
+ if (withMaxPitch.satisfies(constraints))
+ // keep optimizing to find the lowest height
+ newRange = new Pair<TurretState, TurretState>(withMaxPitch, withMinHeight);
else
return Optional.empty();
+
+ } else {
+ // the minimum speed is within the ok range
+ assert withMinSpeed.satisfies(constraints);
+ newRange = new Pair<TurretState, TurretState>(withMinSpeed, withMinHeight);
}
+
+ // now we binary search the new range
+ TurretState lastValid = newRange.getFirst();
+ // use a 5cm tolerance
+ while (Math.abs(newRange.getFirst().height() - newRange.getSecond().height()) < .05) {
+ double avgHeight = (newRange.getFirst().height() + newRange.getSecond().height()) / 2;
+ Translation3d guessVel = getRequiredExitVelocity(robotVelocity, robotToTarget, avgHeight);
+ double guessT = (-guessVel.getZ() - Math.sqrt(Math.pow(guessVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ TurretState guess = cvtShot(guessVel, avgHeight, guessT);
+ if (guess.satisfies(constraints)) {
+ if (guess.height() < lastValid.height()) lastValid = guess;
+ newRange = new Pair<TurretState, TurretState>(guess, newRange.getSecond());
+ } else {
+ newRange = new Pair<TurretState, TurretState>(newRange.getFirst(), guess);
+ }
+ }
+
+ return Optional.of(lastValid);
}
- static TurretState cvtShot(Translation3d velocity, double height) {
+ public static TurretState cvtShot(Translation3d velocity, double height, double timeOfFlight) {
Translation2d onGround = velocity.toTranslation2d();
Rotation2d yaw = onGround.getAngle();
double magnitude2d = onGround.getNorm();
double pitch = new Translation2d(magnitude2d, velocity.getZ()).getAngle().getRadians();
+ pitch %= Math.PI * 2;
double speed = velocity.getDistance(Translation3d.kZero);
- return new TurretState(yaw, pitch, speed, height);
+ return new TurretState(yaw, pitch, speed, height, timeOfFlight);
}
/**
* Actually does the SOTM math. Assumes shots are from (0, 0, 0). This is only
- * package scope so it can be unit tested, and shouldn't be called directly.
+ * public so it can be unit tested, and shouldn't be called directly.
*
* @param robotVelocity The velocity of the robot, field relative.
* @param target The translation from the robot to the target, field
* trajectory.
* @return Velocity that should be imparted on the ball, field relative.
*/
- static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target,
+ public static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target,
double peakZ) {
if (target.getZ() > peakZ)
throw new IllegalArgumentException(
// peakZ = .5 * v_z_exit_vel² / g
// v_z_exit_vel² = 2 * peakZ * g
// v_z_exit_vel = √(2 * peakZ * g)
- // keep a second variable to avoid floating point precision issues where the
- // sqrt for t ends up being NaN sometimes when peakZ = target.getZ()
- double zExitVelSquared = 2 * peakZ * Constants.GRAVITY_ACCELERATION;
- double zExitVel = Math.sqrt(zExitVelSquared);
+ double zExitVel = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION);
// now we need time to hit target
// z_target = v_z_exit_vel * t - .5 * g * t²
// t = (-v_z_exit_vel ± √(v_z_exit_vel² - 2 * g * z_target)) / -g
// onlz use - because we only want the part where it's coming down, and that
// gives the longer time
- double t = (-zExitVel - Math.sqrt(zExitVelSquared - 2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+ double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
/ -Constants.GRAVITY_ACCELERATION;
- // this is not equivalent to t <= 0 because of NaNs
- if (!(t > 0))
- throw new RuntimeException("Time should never be negative (got t=" + t + " with target: " + target
- + " and peakZ: " + peakZ + ").");
+ if (t < 0)
+ throw new RuntimeException("Time should never be negative (got t=" + t + ").");
// calculate x and z exit_vel
// x = (v_x_robot + v_x_exit_vel) * t
return new Translation3d(xExitVel, yExitVel, zExitVel);
}
- // gets the derivative of velocity with respect to height at shot
- private static double getVelocityDiff(TurretState shot, Translation2d initialVelocity, Translation3d target) {
- return (getShotParams(initialVelocity, target, shot.height() + .01).exitVel() - shot.exitVel()) / .01;
- }
-
// call with default tolerance
- static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) {
- return withMinimumSpeed(initialVelocity, target, 0.001);
+ public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) {
+ return withMinimumSpeed(initialVelocity, target, 0.1);
}
- static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target,
+ public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target,
double tolerance) {
- // System.out.println("!!! inv:" + initialVelocity + " tgt:" + target + " tlr:"
- // + tolerance);
- // trying to calculate a shot for height=0 returns NaN
- double effectiveMinHeight = Math.max(target.getZ(), 0.01);
-
- TurretState first = getShotParams(initialVelocity, target, effectiveMinHeight);
- // if the minimum velocity is below our minimum height, that's the closest we
- // can get
- if (getVelocityDiff(first, initialVelocity, target) >= 0)
- return first;
-
- // if a shot requires going up a kilometer, it's probably not doable
- TurretState second = getShotParams(initialVelocity, target, 1000.);
- // just return something
- if (getVelocityDiff(second, initialVelocity, target) < 0)
- return second;
-
- int maxIters = 50;
- var range = new Pair<TurretState, TurretState>(first, second);
+
+ // calculate minimum velocity: v² = g * (R + √(R² + h²))
+ double horizontalDist = target.toTranslation2d().getNorm();
+ double verticalDist = target.getZ();
+ double g = Constants.GRAVITY_ACCELERATION;
+ double robotSpeed = initialVelocity.getNorm();
+
+ double minProjectileSpeed = Math
+ .sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist)));
+ double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed);
+
+ // guess a peak height
+ double guess = target.getZ() + 2;
+ int maxIters = 20;
while (maxIters >= 0) {
maxIters--;
- assert range.getSecond().height() > range.getFirst().height();
- double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2;
- TurretState guess = getShotParams(initialVelocity, target, guessHeight);
- double diff = getVelocityDiff(guess, initialVelocity, target);
+ // this will throw an exception, so avoid it
+ // we still might have just overshot, so keep checking
+ if (guess < target.getZ())
+ guess = target.getZ();
- // System.out.println("diff:" + diff + "\t\t" + range);
+ Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
+ double guessT = (-guessVelocity.getZ() - Math.sqrt(Math.pow(guessVelocity.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ double guessSpeed = guessVelocity.getNorm();
+ double difference = minSpeed - guessSpeed;
- if (Math.abs(range.getSecond().exitVel() - range.getFirst().exitVel()) <= tolerance)
- return guess;
+ // we've already hit minimum height and are trying to go lower
+ if (guess <= target.getZ() && difference < 0)
+ throw new RuntimeException("Incorrect minimum speed calculation in ShooterPhysics.java");
- if (diff > 0)
- range = new Pair<TurretState, TurretState>(range.getFirst(), guess);
- else
- range = new Pair<TurretState, TurretState>(guess, range.getSecond());
- }
+ if (Math.abs(difference) <= tolerance)
+ return cvtShot(guessVelocity, guess, guessT);
- throw new RuntimeException(
- "Solving for minumum velocity did not converge (velocity: " + initialVelocity + ", target: "
- + target + ", tolerance: " + tolerance + ").");
+ guess += difference * 1.7; // experimentally determined value
+ }
+ throw new RuntimeException("Failed to compute a trajectory for a minimum speed.");
}
- static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
+ public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
double pitch) {
- return withAngle(initialVelocity, target, pitch, 0.001);
+ return withAngle(initialVelocity, target, pitch, Units.degreesToRadians(1));
}
- // note: this behaves badly with high angles
- // this isn't a problem when this is called from getConstrainedParams because
- // that will only use this method to solve for the lower bound
- static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
+ public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
double pitch, double tolerance) {
- if (pitch <= 0 || pitch >= Math.PI / 2)
- throw new IllegalArgumentException("Pitch must be in the range 0 < pitch < pi/2 (got: " + pitch + ").");
-
- // System.out.println(
- // "Solving for pitch=" + pitch + " with target=" + target + " and
- // initialVelocity=" + initialVelocity);
-
- // trying to calculate a shot for height=0 returns NaN
- double effectiveMinHeight = Math.max(target.getZ(), 0.01);
-
- TurretState first = getShotParams(initialVelocity, target, effectiveMinHeight);
- TurretState second = getShotParams(initialVelocity, target, 1000.);
- if (first.pitch() > pitch)
- if (first.pitch() <= pitch + tolerance)
- return Optional.of(first);
- else
- return Optional.empty();
- else if (second.pitch() < pitch)
- return Optional.of(second); // it's close enough
-
- int maxIters = 50;
- var range = new Pair<TurretState, TurretState>(first, second);
+ // guess a peak height
+ double guess = target.getZ() + 2;
+ int maxIters = 20;
while (maxIters >= 0) {
maxIters--;
- assert range.getSecond().height() > range.getFirst().height();
- double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2;
- TurretState guess = getShotParams(initialVelocity, target, guessHeight);
+ // this will throw an exception, so avoid it
+ // we still might have just overshot, so keep checking
+ if (guess < target.getZ())
+ guess = target.getZ();
- // System.out.println(range + "\t\tguess=" + guess);
+ Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
+ double guessT = (-guessVelocity.getZ() - Math.sqrt(Math.pow(guessVelocity.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ TurretState polar = cvtShot(guessVelocity, guess, guessT);
+ double difference = pitch - polar.pitch();
- if (range.getSecond().pitch() - range.getFirst().pitch() <= tolerance) {
- // we've found a valid angle
- if (Math.abs(range.getFirst().pitch() - pitch) <= tolerance)
- return Optional.of(range.getFirst());
- if (Math.abs(range.getSecond().pitch() - pitch) <= tolerance)
- return Optional.of(range.getSecond());
+ // we've already hit minimum height and are trying to go lower
+ if (guess <= target.getZ() && difference < 0)
+ return Optional.empty();
- // we've narrowed the range but haven't found a valid angle
- // should be covered by the checks before the loop
- throw new RuntimeException(
- "Solving for angle resulted in an empty range " + range + " (pitch: " + pitch + ").");
- }
+ if (Math.abs(difference) <= tolerance)
+ return Optional.of(polar);
- if (guess.pitch() > pitch)
- range = new Pair<TurretState, TurretState>(range.getFirst(), guess);
- else
- range = new Pair<TurretState, TurretState>(guess, range.getSecond());
+ guess += difference * 0.7; // TODO: find better value
}
- throw new RuntimeException("Solving for angle did not converge (velocity: " + initialVelocity + ", target: "
- + target + ", pitch: " + pitch + ", tolerance: " + tolerance + ").");
+ throw new RuntimeException("Solving for angle did not converge.");
}
}
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
import frc.robot.constants.Constants;
-import frc.robot.util.ShooterPhysics.Constraints;
-import frc.robot.util.ShooterPhysics.TurretState;
class ShooterPhysicsTest {
- private static final double epsilon = .01;
+ private static final double epsilon = .001;
@BeforeEach
public void prepare() {
@Test
public void yawTest() {
- TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(1, 0, 0), 1);
// different for this one because it's close to 0, so the angle wraps and
// assertEquals can't handle that
assertTrue(Math.abs((state1.yaw()).getRadians()) <= epsilon, state1.toString());
- TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(0, 1, 0), 1);
assertEquals(new Rotation2d(Math.PI / 2).getRadians(), state2.yaw().getRadians(), epsilon);
- TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(-1, 0, 0), 1);
assertEquals(new Rotation2d(Math.PI).getRadians(), state3.yaw().getRadians(), epsilon);
- TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(0, -1, 0), 1);
assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon);
}
@Test
public void pitchTest() {
// check random values are within range
- TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(1, 0, 0), 1);
assertTrue(state1.pitch() >= 0 && state1.pitch() <= Math.PI / 2, state1.toString());
- TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3),
+ ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3),
new Translation3d(1.1, 12, 11.1).minus(new Translation3d(.2, 1.2, 0)), 22.1);
assertTrue(state2.pitch() >= 0 && state2.pitch() <= Math.PI / 2, state2.toString());
- TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1),
+ ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1),
new Translation3d(11.2, -13.1, 4.1).minus(new Translation3d(-.3, -8.4, 0)), 5.6);
assertTrue(state3.pitch() >= 0 && state3.pitch() <= Math.PI / 2, state3.toString());
// try a steep shot
- TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(1, 0, 99), 100);
assertTrue(state4.pitch() >= Math.PI * 7 / 16 && state4.pitch() <= Math.PI / 2, state4.toString());
- TurretState state5 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state5 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(1, 0, 0), 100);
assertTrue(state5.pitch() >= Math.PI * 7 / 16 && state5.pitch() <= Math.PI / 2, state5.toString());
// try a shallow shot
- TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ ShooterPhysics.TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero,
new Translation3d(100, 50, 1), 2);
assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString());
}
- @Test
- public void velocityTest() {
-
- // var t1 = new Translation3d(100, 0, 0);
- // for (int i = 1; i < 1000; i++) {
- // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.);
- // System.out.println(i / 10. + ", " + x.exitVel());
- // }
-
- var t1 = new Translation3d(100, 0, 0);
- var state1 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t1);
- // check moving either way is higher velocity
- var state1Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t1,
- state1.height() + 0.2);
- var state1Minus = ShooterPhysics.getShotParams(Translation2d.kZero, t1,
- state1.height() - 0.2);
- assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString());
- assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString());
- assertEquals(Math.PI / 4, state1.pitch(), epsilon);
-
- var t2 = new Translation3d(1, 1, 100);
- var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2);
- // this should get to the minimum height
- var state2Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t2,
- state2.height() + 0.1);
- assertTrue(state2.exitVel() < state2Plus.exitVel(), state2Plus.toString());
- assertEquals(t2.getZ(), state2.height(), epsilon);
-
- // test with an initial velocity
- var t3 = new Translation3d(100, 0, 0);
- var v3 = new Translation2d(10, -20);
- var state3 = ShooterPhysics.withMinimumSpeed(v3, t3);
- // check moving either way is higher velocity
- var state3Plus = ShooterPhysics.getShotParams(v3, t3, state3.height() + 0.2);
- var state3Minus = ShooterPhysics.getShotParams(v3, t3, state3.height() - 0.2);
- assertTrue(state3.exitVel() < state3Plus.exitVel(), state3Plus.toString());
- assertTrue(state3.exitVel() < state3Minus.exitVel(), state3Minus.toString());
- }
-
- @Test
- public void angleTest() {
- // for (int i = 21; i < 1000; i++) {
- // var x = ShooterPhysics.getShotParams(new Translation2d(-3.81, -3.52), new
- // Translation3d(-8.43, -5.58, 2.09),
- // i / 10.);
- // System.out.println(i / 10. + ", " + x.pitch() + ", " + x.exitVel());
- // }
-
- var t1 = new Translation3d(10, 0, 0);
- var state1 = ShooterPhysics.withAngle(Translation2d.kZero, t1,
- Units.degreesToRadians(30));
- assertTrue(state1.isPresent());
- assertEquals(state1.get().pitch(), Units.degreesToRadians(30), epsilon);
- // get this as a velocity vector
- Translation3d v1 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, t1,
- state1.get().height());
- assertEquals(ShooterPhysics.cvtShot(v1, state1.get().height()),
- state1.get());
- checkTrajectory(Translation3d.kZero, v1, t1, state1.get().height());
-
- var t2 = new Translation3d(1, -1, 10);
- var state2 = ShooterPhysics.withAngle(Translation2d.kZero, t2,
- Units.degreesToRadians(60));
- assertTrue(state2.isEmpty());
-
- var t3 = new Translation3d(1.34, 4.2, 2.3);
- var iv3 = new Translation2d(12.1, -2.1);
- var state3 = ShooterPhysics.withAngle(iv3, t3,
- Units.degreesToRadians(45));
- assertTrue(state3.isPresent());
- assertEquals(state3.get().pitch(), Units.degreesToRadians(45), epsilon);
- // get this as a velocity vector
- Translation3d v3 = ShooterPhysics.getRequiredExitVelocity(iv3, t3,
- state3.get().height());
- assertEquals(ShooterPhysics.cvtShot(v3, state3.get().height()),
- state3.get());
- checkTrajectory(Translation3d.kZero, v3.plus(new Translation3d(iv3)), t3,
- state3.get().height());
-
- // just check this converges
- var state4 = ShooterPhysics.withAngle(Translation2d.kZero, new Translation3d(10, 10, 3), Math.PI / 2 - 0.01);
- assertTrue(state4.isPresent());
- assertEquals(state4.get().pitch(), Math.PI / 2 - 0.01, epsilon);
- var state5 = ShooterPhysics.withAngle(Translation2d.kZero, new Translation3d(10, -10, 0), 0.01);
- assertTrue(state5.isPresent());
- assertEquals(state5.get().pitch(), 0.01, epsilon);
- }
-
- @Test
- public void simpleConstraintsTest() {
- // a test where the optimal shot is just plain height
- Constraints constraints1 = new Constraints(3, 20, .1, Math.PI / 2 - .1);
- var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints1);
- assertTrue(val1.isPresent());
- var direct1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 2, 3),
- constraints1.height());
- assertEquals(direct1.pitch(), val1.get().pitch(), epsilon);
- assertEquals(direct1.yaw().getRadians(), val1.get().yaw().getRadians(), epsilon);
- assertEquals(direct1.exitVel(), val1.get().exitVel(), epsilon);
-
- // optimal is the minimum angle
- Constraints constraints2 = new Constraints(3, 30, Units.degreesToRadians(45), Math.PI / 2 - .1);
- var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints2);
- assertTrue(val2.isPresent());
- assertEquals(45, Units.radiansToDegrees(val2.get().pitch()), .1);
-
- // something impossible
- Constraints constraints3 = new Constraints(3, 3, Units.degreesToRadians(45), Math.PI / 2 - .1);
- var val3 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints3);
- assertTrue(val3.isEmpty(), val3.toString());
- }
-
// test using a simple physics simulation
@Test
public void simulatedTest() {
// compute 1000 random shots and simulate them
for (int i = 0; i < 1000; i++) {
- Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10));
- Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10),
- rng.nextDouble(1, 11));
- Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5));
+ Translation2d initPos = new Translation2d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10);
+ Translation3d target = new Translation3d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10,
+ rng.nextDouble() * 10 + 1);
+ Translation2d initVel = new Translation2d(rng.nextDouble() * 10 - 5, rng.nextDouble() * 10 - 5);
Translation3d robotToTarget = target.minus(new Translation3d(initPos));
- double arcHeight = target.getZ() + rng.nextDouble(10);
+ double arcHeight = target.getZ() + rng.nextDouble() * 10;
Translation3d exitVel = ShooterPhysics.getRequiredExitVelocity(initVel, robotToTarget, arcHeight);
}
}
- @Test
- public void simulatedConstraintsTest() {
- Random rng = new Random(6328);
-
- for (int i = 0; i < 10_000; i++) {
- Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10));
- Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10),
- rng.nextDouble(1, 11));
- Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5));
- Translation3d robotToTarget = target.minus(new Translation3d(initPos));
-
- // can't have a min angle lower than 75° because that breaks things
- double minAngle = rng.nextDouble(.01, Math.PI / 2 - Units.degreesToRadians(15));
- Constraints constraints = new Constraints(rng.nextDouble(20) + .001, rng.nextDouble(30) + .001, minAngle,
- rng.nextDouble(minAngle + .01, Math.PI / 2 - .01));
-
- var state = ShooterPhysics.getConstrainedParams(initVel, robotToTarget, constraints);
-
- if (state.isPresent()) {
- assertTrue(state.get().satisfies(constraints));
-
- // check going down breaks a constraint; we've found the minimum
- var lower = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() - .1);
- assertTrue(
- lower.isEmpty() || !lower.get().satisfies(constraints)
- || lower.get().height() > state.get().height(),
- String.format("%s was better than %s", lower.toString(), state.toString()));
- }
- }
- }
-
private class PhysicsObject {
private Translation3d pos;
private Translation3d vel;
+ " and peak height " + requiredHeight + ".");
messages.add("position, velocity"); // column headers
- while (object.pos.getZ() > -epsilon || firstLoop) {
+ while (object.pos.getZ() > 0 || firstLoop) {
messages.add("" + object.pos + ", " + object.vel);
if (object.pos.getZ() + tolerance >= requiredHeight)