if (withMinPitch.isPresent())
minHeight = Math.max(minHeight, withMinPitch.get().height());
- TurretState withMinHeight = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight),
- minHeight);
+ TurretState withMinHeight = getShotParams(robotVelocity, robotToTarget, minHeight);
if (withMinHeight.satisfies(constraints))
return Optional.of(withMinHeight);
- // 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.maxPitch());
- 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);
if (withMinSpeed.exitVel() > constraints.maxVel())
return Optional.empty();
- // 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 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
- if (!withMinSpeed.satisfies(constraints)) {
- throw new RuntimeException(
- "Impossible state: " + withMinSpeed + " does not satisfy " + constraints + ".");
- }
+ // 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);
- 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;
- TurretState guess = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, avgHeight), avgHeight);
- 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);
+ if (newRange.getSecond().satisfies(constraints))
+ return Optional.of(newRange.getSecond());
+ else
+ return Optional.empty();
+ }
}
public static TurretState cvtShot(Translation3d velocity, double height) {
}
private static double getVelocityDiff(TurretState shot, Translation2d initialVelocity, Translation3d target) {
- return (cvtShot(getRequiredExitVelocity(initialVelocity, target, shot.height() + .01), shot.height() + .01)
- .exitVel()
- - shot.exitVel()) / .01;
+ return (getShotParams(initialVelocity, target, shot.height() + .01).exitVel() - shot.exitVel()) / .01;
}
// call with default tolerance
// trying to calculate a shot for height=0 returns NaN
double effectiveMinHeight = Math.max(target.getZ(), 0.01);
- TurretState first = cvtShot(getRequiredExitVelocity(initialVelocity, target, effectiveMinHeight),
- effectiveMinHeight);
+ 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 = cvtShot(getRequiredExitVelocity(initialVelocity, target, 1000.), 1000.);
+ TurretState second = getShotParams(initialVelocity, target, 1000.);
// just return something
if (getVelocityDiff(second, initialVelocity, target) < 0)
return second;
assert range.getSecond().height() > range.getFirst().height();
double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2;
- TurretState guess = cvtShot(getRequiredExitVelocity(initialVelocity, target, guessHeight), guessHeight);
+ TurretState guess = getShotParams(initialVelocity, target, guessHeight);
double diff = getVelocityDiff(guess, initialVelocity, target);
// System.out.println("diff:" + diff + "\t\t" + range);
return withAngle(initialVelocity, target, pitch, 0.001);
}
+ // 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
public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
double pitch, double tolerance) {
if (pitch <= 0 || pitch >= Math.PI / 2)
// trying to calculate a shot for height=0 returns NaN
double effectiveMinHeight = Math.max(target.getZ(), 0.01);
- TurretState first = cvtShot(getRequiredExitVelocity(initialVelocity, target, effectiveMinHeight),
- effectiveMinHeight);
- // if a shot requires going up a kilometer, it's probably not doable
- TurretState second = cvtShot(getRequiredExitVelocity(initialVelocity, target, 1000.), 1000.);
+ TurretState first = getShotParams(initialVelocity, target, effectiveMinHeight);
+ TurretState second = getShotParams(initialVelocity, target, 1000.);
if (first.pitch() > pitch)
if (first.pitch() <= pitch + tolerance)
assert range.getSecond().height() > range.getFirst().height();
double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2;
- TurretState guess = cvtShot(getRequiredExitVelocity(initialVelocity, target, guessHeight), guessHeight);
+ TurretState guess = getShotParams(initialVelocity, target, guessHeight);
// System.out.println(range + "\t\tguess=" + guess);
if (range.getSecond().pitch() - range.getFirst().pitch() <= tolerance) {
// we've found a valid angle
- if (Math.abs(guess.pitch() - pitch) <= tolerance)
- return Optional.of(guess);
+ 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 narrowed the range but haven't found a valid angle
// should be covered by the checks before the loop
throw new RuntimeException(
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Disabled;
import org.junit.jupiter.api.Test;
import edu.wpi.first.math.geometry.Rotation2d;
public void cleanup() {
}
- @Disabled
@Test
public void basicImpulseTest() {
Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, Translation3d.kZero, 1);
assertEquals(Constants.GRAVITY_ACCELERATION, transform7.getZ(), epsilon);
}
- @Disabled
@Test
public void initialVelocityTest() {
Translation2d initialVelocityDiff = new Translation2d(5.1, -6.5);
assertEquals(transform.getZ(), transform2.getZ(), epsilon);
}
- @Disabled
@Test
public void yawTest() {
TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon);
}
- @Disabled
@Test
public void pitchTest() {
// check random values are within range
assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString());
}
- @Disabled
@Test
public void velocityTest() {
assertTrue(state3.exitVel() < state3Minus.exitVel(), state3Minus.toString());
}
- @Disabled
@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());
+ // 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);
assertEquals(state5.get().pitch(), 0.01, epsilon);
}
- @Disabled
@Test
public void simpleConstraintsTest() {
// a test where the optimal shot is just plain height
}
// test using a simple physics simulation
- @Disabled
@Test
public void simulatedTest() {
// test the simulation itself
public void simulatedConstraintsTest() {
Random rng = new Random(6328);
- for (int i = 0; i < 100000; i++) {
+ 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));
- double minAngle = rng.nextDouble(.01, Math.PI / 2 - .03);
+ // 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));
// 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));
- // check going up is okay
- if (state.get().pitch() + 0.1 < constraints.maxPitch()) {
- var higher = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() + .1);
- assertTrue(higher.isPresent());
- }
+ assertTrue(
+ lower.isEmpty() || !lower.get().satisfies(constraints)
+ || lower.get().height() > state.get().height(),
+ String.format("%s was better than %s", lower.toString(), state.toString()));
}
}
}