From 29389c249a891c8429e402ff77c37563c908222f Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 12 Jan 2025 14:07:54 -0500 Subject: [PATCH] added math utils --- .../org/usfirst/frc4048/common/Constants.java | 3 + .../frc4048/common/util/math/ArrayUtils.java | 73 +++++++ .../frc4048/common/util/math/PoseUtils.java | 182 ++++++++++++++++ .../frc4048/common/util/math/RangeUtils.java | 9 + .../frc4048/common/util/math/VectorUtils.java | 197 ++++++++++++++++++ .../common/util/math/VelocityVector.java | 26 +++ 6 files changed, 490 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4048/common/util/math/ArrayUtils.java create mode 100644 src/main/java/org/usfirst/frc4048/common/util/math/PoseUtils.java create mode 100644 src/main/java/org/usfirst/frc4048/common/util/math/RangeUtils.java create mode 100644 src/main/java/org/usfirst/frc4048/common/util/math/VectorUtils.java create mode 100644 src/main/java/org/usfirst/frc4048/common/util/math/VelocityVector.java diff --git a/src/main/java/org/usfirst/frc4048/common/Constants.java b/src/main/java/org/usfirst/frc4048/common/Constants.java index 85feb03..a140df8 100644 --- a/src/main/java/org/usfirst/frc4048/common/Constants.java +++ b/src/main/java/org/usfirst/frc4048/common/Constants.java @@ -23,4 +23,7 @@ public class Constants { public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; public static final long MAX_LOG_TIME_WAIT = 10; public static final int LIGHTSTRIP_PORT = 0; + public static final double GRAVITY = -9.8; + public static final double SHOOTER_VELOCITY = 10.56;//m/s @ 5500rpm, 3500rpm // 8.9 or 8.1 or 10.65 or + public static final double RAMP_FROM_CENTER = 0.17; } diff --git a/src/main/java/org/usfirst/frc4048/common/util/math/ArrayUtils.java b/src/main/java/org/usfirst/frc4048/common/util/math/ArrayUtils.java new file mode 100644 index 0000000..d630d34 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/math/ArrayUtils.java @@ -0,0 +1,73 @@ +package org.usfirst.frc4048.common.util.math; + +import java.lang.reflect.Array; +import java.util.Arrays; +import java.util.List; + +public class ArrayUtils { + public static boolean contains(T[] array, T value){ + for (T t : array) { + if (t.equals(value)) { + return true; + } + } + return false; + } + public static boolean contains(double[] array, double value){ + for (double d : array){ + if (d == value){ + return true; + } + } + return false; + } + public static boolean contains(int[] array, int value){ + for (int d : array){ + if (d == value){ + return true; + } + } + return false; + } + public static boolean allMatch(double[] array, double value){ + for (double d : array){ + if (d != value){ + return false; + } + } + return true; + } + public static boolean allMatch(int[] array, int value){ + for (int d : array){ + if (d != value){ + return false; + } + } + return true; + } + public static boolean allMatch(T[] array, T value){ + for (T t : array) { + if (!t.equals(value)) { + return false; + } + } + return true; + } + @SuppressWarnings("unchecked") + public static T[] toArray(List list, Class tClass){ + T[] t = (T[]) Array.newInstance(tClass, list.size()); + return list.toArray(t); + } + + public static double[] unwrap(Double[] a){ + return Arrays.stream(a).mapToDouble(Double::doubleValue).toArray(); + } + + public static int[] unwrap(Integer[] a){ + return Arrays.stream(a).mapToInt(Integer::intValue).toArray(); + } + + public static long[] unwrap(Long[] a){ + return Arrays.stream(a).mapToLong(Long::longValue).toArray(); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/math/PoseUtils.java b/src/main/java/org/usfirst/frc4048/common/util/math/PoseUtils.java new file mode 100644 index 0000000..c7d9a4b --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/math/PoseUtils.java @@ -0,0 +1,182 @@ +package org.usfirst.frc4048.common.util.math; + +import edu.wpi.first.math.geometry.*; + +public class PoseUtils { + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation2d getFieldEstimatedFuturePose(Translation2d pose, double vx, double vy, double time) { + return pose.plus(new Translation2d(vx * time, vy * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation3d getFieldEstimatedFuturePose(Translation3d pose, double vx, double vy, double vz, double time) { + return pose.plus(new Translation3d(vx * time, vy * time, vz * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double vYaw, double time) { + return pose.transformBy(new Transform2d(vx * time, vy * time, new Rotation2d(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double vYaw, double time) { + return pose.plus(new Transform2d(vx * time, vy * time, Rotation2d.fromDegrees(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) { + return pose.transformBy(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) { + return pose.plus(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + + /** + * @param pose to modify + * @param z to add + * @param pitch to add + * @param roll to add + * @return 3D pose that inherits 2D attributes + */ + public static Pose3d addDimension(Pose2d pose, double z, double pitch, double roll) { + return new Pose3d(addDimension(pose.getTranslation(), z), addDimension(pose.getRotation(), pitch, roll)); + } + + /** + * @param pose to modify + * @param z to add + * @return 3D pose that inherits 2D attributes + */ + public static Translation3d addDimension(Translation2d pose, double z) { + return new Translation3d(pose.getX(), pose.getY(), z); + } + + /** + * @param rotation to modify + * @param pitch to add + * @param roll to add + * @return 3D rotation that inherits yaw of 2D rotation + */ + public static Rotation3d addDimension(Rotation2d rotation, double pitch, double roll) { + return new Rotation3d(roll, pitch, rotation.getRadians()); + } + public static double slope(Translation2d startPose, Translation2d endPose){ + return (endPose.getY() - startPose.getY()) / (endPose.getX() - startPose.getX()); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/math/RangeUtils.java b/src/main/java/org/usfirst/frc4048/common/util/math/RangeUtils.java new file mode 100644 index 0000000..61ca027 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/math/RangeUtils.java @@ -0,0 +1,9 @@ +package org.usfirst.frc4048.common.util.math; + +public class RangeUtils { + public static double map(double value, double startMin, double startMax, double endMin, double endMax){ + double endRange = endMax - endMin; + double startRange = startMax - startMin; + return ((value - startMin) * endRange / startRange) + endMin; + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/math/VectorUtils.java b/src/main/java/org/usfirst/frc4048/common/util/math/VectorUtils.java new file mode 100644 index 0000000..45097b7 --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/math/VectorUtils.java @@ -0,0 +1,197 @@ +package org.usfirst.frc4048.common.util.math; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.usfirst.frc4048.common.Constants; + +/** + * Utility class for vectors used in projectile motion + */ +public class VectorUtils { + + /** + * The projectile motion converts the given coords to 2d after applying the change in base coords from the arc provided. + * Because shooter angle depends on position (not point math) and position depends on angle (not point math), + * the function will approximate to a specified certainty by calling the function at 45 degrees, + * getting that pose and then looping until the delta angle result is within a specified margin, + * or you have exceeded the max iterations specified. + * + * @param speed the initial velocity of the projectile + * @param centerArcX the x cord of the arc created by the shooter moving at different angles + * @param centerArcY the y cord of the arc created by the shooter moving at different angles + * @param centerArcZ the z cord of the arc created by the shooter moving at different angles + * @param arcRadius the length of the shooter, which forms an arc when changing angle + * @param destX the target position in the x direction + * @param destY the target position in the y direction + * @param destZ the target position in the z direction + * @param degreeThreshold threshold before angles are considered equal when iterating over positions angle relationship created by arc + * @param maxIterations the max amount of iterations before an angle is returned + * @param maxFractionalRange double representing what fraction of range of the parabola is acceptable for hitting your target.
+ * * For example if you have to hit your target from bottom, you can constrain the parabola to only be acceptable in the first third of the parabola. + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
Impossible parameters will produce a null result + */ + @Deprecated + public static VelocityVector fromArcAndDestAndVel(double speed, double centerArcX, double centerArcY, double centerArcZ, double arcRadius, double destX, double destY, double destZ, double degreeThreshold, int maxIterations, double maxFractionalRange) { + double xDist = destX - centerArcX; + double yDist = destY - centerArcY; + VelocityVector lastVel = null; + VelocityVector currVel = null; + int i = 0; + do { + i++; + double theta = Math.PI / 4; + if (currVel != null) { + lastVel = currVel; + theta = currVel.getAngle().getRadians(); + } + double rampXYOffset = (Math.cos(theta) * arcRadius); + double rampZOffset = Math.sin(theta) * arcRadius; + double xyDist = Math.hypot(xDist, yDist) - rampXYOffset; + double z1 = centerArcZ + rampZOffset; + double deltaZ = Math.abs(destZ - z1); + currVel = fromVelAndDist(speed, xyDist, deltaZ, maxFractionalRange); + } while (i < maxIterations && (lastVel == null || currVel == null || Math.abs(lastVel.getAngle().getDegrees() - currVel.getAngle().getDegrees()) > degreeThreshold)); + return currVel; + } + + /** + * The projectile motion converts the given coords to 2d after applying the change in base coords from the arc provided. + * Because shooter angle depends on position (not point math) and position depends on angle (not point math), + * the function will approximate to a specified certainty by calling the function at 45 degrees, + * getting that pose and then looping until the delta angle result is within 0.01 degrees, + * or you have exceeded 7 iterations.
+ * To use custom certainty see {@link #fromArcAndDestAndVel(double, double, double, double, double, double, double, double, double, int, double)} + * + * @param speed the initial velocity of the projectile + * @param centerArcX the x cord of the arc created by the shooter moving at different angles + * @param centerArcY the y cord of the arc created by the shooter moving at different angles + * @param centerArcZ the z cord of the arc created by the shooter moving at different angles + * @param arcRadius the length of the shooter, which forms an arc when changing angle + * @param destX the target position in the x direction + * @param destY the target position in the y direction + * @param destZ the target position in the z direction + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
Impossible parameters will produce a null result + */ + @Deprecated + public static VelocityVector fromArcAndDestAndVel(double speed, double centerArcX, double centerArcY, double centerArcZ, double arcRadius, double destX, double destY, double destZ) { + return fromArcAndDestAndVel(speed, centerArcX, centerArcY, centerArcZ, arcRadius, destX, destY, destZ, 0.01, 1, 1.0 / 2); + } + + /** + * @param speed speed the projectile is moving at
Must be > 0 + * @param deltaX the X distance between the starting and ending positions
Must be > 0 + * @param deltaY the Y distance between the starting and ending positions
Must be > 0 + * @param maxFractionalRange double representing what fraction of range of the parabola is acceptable for hitting your target.
+ * For example if you have to hit your target from bottom, you can constrain the parabola to only be acceptable in the first third of the parabola. + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
Impossible parameters will produce a null result + */ + public static VelocityVector fromVelAndDist(double speed, double deltaX, double deltaY, double maxFractionalRange) { + if (speed <= 0 || deltaX <= 0 || deltaY <= 0) return null; + double tanOfAngle = ( + ((deltaY >= 0) ? 1 : -1) * Math.sqrt( + Math.pow(deltaX, 2) * ( + (-1 * Math.pow(Constants.GRAVITY, 2) * Math.pow(deltaX, 2)) + + (2 * Constants.GRAVITY * Math.pow(speed, 2) * deltaY) + + (Math.pow(speed, 4)) + ) + ) - (Math.pow(speed, 2) * deltaX)) / + (Constants.GRAVITY * Math.pow(deltaX, 2)); + VelocityVector velocityVector = new VelocityVector(speed, new Rotation2d(Math.atan(tanOfAngle))); + return horizontalRange(velocityVector) * maxFractionalRange > deltaX ? velocityVector : null; + } + + /** + * @param velocityVector the initial velocity and angle of the projectile + * @return the max range of a projectile following a theoretical parabola + */ + public static double horizontalRange(VelocityVector velocityVector) { + return ((Math.pow(velocityVector.getVelocity(), 2) * Math.sin(velocityVector.getAngle().getRadians() * 2))) / (-1 * Constants.GRAVITY); + } + + /** + * @param velocityVector the initial velocity and angle of the projectile + * @return the max height of a projectile following a theoretical parabola + */ + public static double maximumHeight(VelocityVector velocityVector) { + return (Math.pow(velocityVector.getVelocity(), 2) * Math.pow(Math.sin(velocityVector.getAngle().getRadians()), 2)) / (2 * Constants.GRAVITY * -1); + } + + /** + * The projectile motion converts the given coords to 2d and after applying the drivetrain x velocity, to the x component of the projectile's initial velocity.
+ * Because shooter component velocities depends on the angle we shoot at, + * and the angle we shoot depends on the composition of the component velocities. if we want to incorporate the drivetrains velocity we must use an iterative approach.
+ * This function will approximate to a specified certainty by assuming a shot of 45 degrees and splitting the velocity into its components. + * Then the function adds the drivetrains x velocity to the x component and returns a new shooting angle based the combined vectors.
+ * This process repeats until max iterations is reached (returns the current angle) or the degree threshold between iterations is less than the threshold provided + * + * @param speed the initial velocity of the projectile + * @param startX the x cord of the base of the ramp + * @param startY the y cord of the base of the ramp + * @param startZ the z cord of the base of the ramp + * @param driveSpeedX the drivetrain velocity in meters per second + * @param destX the destination x position + * @param destY the destination y position + * @param destZ the destination z position + * @param degreeThreshold the degree threshold between iteration before function is considered done + * @param maxIterations the max iteration before iteration is considered done. Note it does not require that max iterations was reached if degree threshold predicate is met. + * @param maxFractionalRange double representing what fraction of range of the parabola is acceptable for hitting your target.
+ * For example if you have to hit your target from bottom, you can constrain the parabola to only be acceptable in the first third of the parabola. + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
Impossible parameters will produce a null result + */ + public static VelocityVector fromDestAndCompoundVel(double speed, double startX, double startY, double startZ, double driveSpeedX, double destX, double destY, double destZ,double distOffset ,double degreeThreshold, int maxIterations, double maxFractionalRange) { + double xDist = destX - startX; + double yDist = destY - startY; + double xyDist = Math.hypot(xDist, yDist) + distOffset; + double deltaZ = Math.abs(destZ - startZ); + VelocityVector lastVel = null; + VelocityVector currVel = null; + int i = 0; + do { + i++; + lastVel = currVel; + double theta = lastVel == null ? Math.PI / 4 : lastVel.getAngle().getRadians(); + double xySpeed = (Math.cos(theta) * speed) + driveSpeedX; + double zSpeed = Math.sin(theta) * speed; + double appliedSpeed = Math.hypot(xySpeed, zSpeed); + currVel = fromVelAndDist(appliedSpeed, xyDist, deltaZ, maxFractionalRange); + } while (i < maxIterations && (lastVel == null || currVel == null || Math.abs(lastVel.getAngle().getDegrees() - currVel.getAngle().getDegrees()) > degreeThreshold)); + return currVel == null ? new VelocityVector(Constants.SHOOTER_VELOCITY, new Rotation2d()) : currVel; + } + + /** + * The projectile motion converts the given coords to 2d and after applying the drivetrain x velocity, to the x component of the projectile's initial velocity.
+ * Because shooter component velocities depends on the angle we shoot at, + * and the angle we shoot depends on the composition of the component velocities. if we want to incorporate the drivetrains velocity we must use an iterative approach.
+ * This function will approximate to a specified certainty by assuming a shot of 45 degrees and splitting the velocity into its components. + * Then the function adds the drivetrains x velocity to the x component and returns a new shooting angle based the combined vectors.
+ * This process repeats until the 10 iterations is reached (returns the current angle) or the degree threshold of 0.01 between iterations is less than the threshold provided + * + * @param speed the initial velocity of the projectile + * @param startX the x cord of the base of the ramp + * @param startY the y cord of the base of the ramp + * @param startZ the z cord of the base of the ramp + * @param driveSpeedX the drivetrain velocity in meters per second + * @param destX the destination x position + * @param destY the destination y position + * @param destZ the destination z position + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
Impossible parameters will produce a null result + */ + public static VelocityVector fromDestAndCompoundVel(double speed, double startX, double startY, double startZ, double driveSpeedX, double destX, double destY, double destZ) { + return fromDestAndCompoundVel(speed, startX, startY, startZ, driveSpeedX, destX, destY, destZ, Constants.RAMP_FROM_CENTER,0.01, 10, 2.0/5); + } + + public static boolean isErrorSafe(VelocityVector velocityVector, Rotation2d angleError, double destX, double destZ, double maxErrorZ) { + double highY = getYAtX(new VelocityVector(velocityVector.getVelocity(), velocityVector.getAngle().plus(angleError)), destX); + double lowY = getYAtX(new VelocityVector(velocityVector.getVelocity(), velocityVector.getAngle().minus(angleError)), destX); + SmartDashboard.putNumber("maxDifY", highY - destZ); + SmartDashboard.putNumber("minDifY", destZ - lowY); + return highY < destZ + maxErrorZ && lowY > destZ - maxErrorZ; + } + public static double getYAtX(VelocityVector velocityVector, double x){ + double time = getTimeForXDist(velocityVector, x); + return velocityVector.getYSpeed() * time + (Constants.GRAVITY * 0.5 * Math.pow(time,2)); + } + public static double getTimeForXDist(VelocityVector velocityVector, double x){ + return x / velocityVector.getXSpeed(); + } +} diff --git a/src/main/java/org/usfirst/frc4048/common/util/math/VelocityVector.java b/src/main/java/org/usfirst/frc4048/common/util/math/VelocityVector.java new file mode 100644 index 0000000..5dfbddb --- /dev/null +++ b/src/main/java/org/usfirst/frc4048/common/util/math/VelocityVector.java @@ -0,0 +1,26 @@ +package org.usfirst.frc4048.common.util.math; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class VelocityVector { + private final Rotation2d angle; + private final double velocity; + + public VelocityVector(double speed, Rotation2d angle) { + this.velocity = speed; + this.angle = angle; + } + + public double getXSpeed(){ + return Math.cos(angle.getRadians()) * velocity; + } + public double getYSpeed(){ + return Math.sin(angle.getRadians()) * velocity; + } + public double getVelocity(){ + return velocity; + } + public Rotation2d getAngle(){ + return angle; + } +}