commit fa25c4c6b4645ca489eb11a18a25056c66e647b4 Author: Scott Barnes Date: Sat Mar 9 09:30:43 2024 -0600 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..42afabf --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..1701033 --- /dev/null +++ b/build.gradle @@ -0,0 +1,27 @@ +plugins { + id 'java-library' +} + +java { + sourceCompatibility = JavaVersion.VERSION_1_8 + targetCompatibility = JavaVersion.VERSION_1_8 +} + +repositories { + mavenCentral() + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + api 'org.projectlombok:lombok:1.18.30' + implementation 'org.apache.commons:commons-math-parent:4.0-beta1' + testImplementation "org.junit.jupiter:junit-jupiter:5.9.1" + implementation 'com.acmerobotics.roadrunner:core:0.5.6' + api 'org.apache.commons:commons-math4-core:4.0-beta1' + api 'org.apache.commons:commons-geometry-parent:1.0' + api 'org.apache.commons:commons-math3:3.6.1' +} + +test { + useJUnitPlatform() +} \ No newline at end of file diff --git a/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimatorCore.java b/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimatorCore.java new file mode 100644 index 0000000..464dfba --- /dev/null +++ b/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimatorCore.java @@ -0,0 +1,69 @@ +package com.tearabite.ielib.localization; + +import static java.lang.Math.PI; +import static java.lang.Math.asin; +import static java.lang.Math.atan2; +import static java.lang.Math.cos; +import static java.lang.Math.pow; +import static java.lang.Math.sin; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +public class AprilTagPoseEstimatorCore { + + /* + * This class is used to estimate the pose of the robot using the AprilTagDetection object. + * The math done here uses only trigonometric functions so that it can be better understood + * by students without knowledge of linear algebra or matrix operations. + * The math done hre can be visualized using the following Desmos graph: + * https://www.desmos.com/calculator/n2iyatwssg + */ + + /** + * Estimates the pose of the robot using the AprilTagDetection object. + * @param yaw The rotation about Z of the AprilTag within the camera's frame. + * @param bearing The angle between line projected to center of the april tag and the camera's center projection line. + * @param range The distance from the center of the camera lens to the center of the AprilTag. + * @param Tx The x position (in FTC field coordinates) of the AprilTag + * @param Ty The y position (in FTC field coordinates) of the AprilTag + * @param Ta The a component of the quaternion describing the orientation of the AprilTag + * @param Tb The b component of the quaternion describing the orientation of the AprilTag + * @param Tc The c component of the quaternion describing the orientation of the AprilTag + * @param Td The d component of the quaternion describing the orientation of the AprilTag + * @return The estimated pose of the robot. Heading is reported in radians. + */ + public static Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td, Pose2d robotOffset) { + double Tyaw = getTYaw(Ta, Tb, Tc, Td); + yaw = normalizeAngle(yaw); + bearing = normalizeAngle(bearing); + double ch = normalizeAngle((PI / 2) - Tyaw + yaw); + double cx = Tx + range * cos(-bearing) * cos(ch) + range * sin(-bearing) * sin(ch); + double cy = Ty + range * cos(-bearing) * sin(ch) - range * sin(-bearing) * cos(ch); + + double offsetX = robotOffset.getX(); + double offsetY = robotOffset.getY(); + double Cyaw = normalizeAngle(robotOffset.getHeading()); + double rh = (PI / 2) + Tyaw + yaw - Cyaw; + double rx = cx + offsetX * cos(rh) - offsetY * sin(rh); + double ry = cy + offsetX * sin(rh) + offsetY * cos(rh); + rh = normalizeAngle(rh); + + return new Pose2d(rx, ry, rh); + } + + private static double getTPitch(double a, double b, double c, double d) { + return normalizeAngle(asin(2 * ((a * d) - (b * c)))); + } + + private static double getTRoll(double a, double b, double c, double d) { + return normalizeAngle(atan2(2 * ((a * c) + (b * d)), 1 - 2 * (pow(c, 2) + pow(d, 2)))); + } + + private static double getTYaw(double a, double b, double c, double d) { + return normalizeAngle(atan2(2 * ((a * b) + (c * d)), 1 - 2 * ((pow(b, 2) + pow(c, 2))))); + } + + public static double normalizeAngle(double angle) { + return ((angle + PI) % (2 * PI)) - PI; + } +} diff --git a/src/main/java/com/tearabite/ielib/localization/InvisibleBarrierCore.java b/src/main/java/com/tearabite/ielib/localization/InvisibleBarrierCore.java new file mode 100644 index 0000000..0e28c4d --- /dev/null +++ b/src/main/java/com/tearabite/ielib/localization/InvisibleBarrierCore.java @@ -0,0 +1,46 @@ +package com.tearabite.ielib.localization; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.LineSegment; + +import org.apache.commons.math3.geometry.partitioning.BSPTree; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collections; +import java.util.List; + +public class InvisibleBarrierCore { + public static double BACKOFF_FACTOR = 0.2; + +// public static Pose2d limitDriverInputRayCast(Pose2d inputVector, Pose2d robotPose, BSPTree barriers) { +// return null; +// } + + + public static Pose2d limitDriveInput( + Pose2d driverInput, + Pose2d currentRobotPose, + double maximumXCoordinate, + double robotRadius, + double k) { + Pose2d currentPose = currentRobotPose.plus(new Pose2d(robotRadius, 0, 0)); + Vector2d inputVector = driverInput.vec().rotated(-currentPose.getHeading()); + if (inputVector.getX() < 0) { + return driverInput; + } + + double projectedX = currentPose.getX() + (inputVector.getX() * k); + double clampedX = Math.min(maximumXCoordinate, projectedX); + double ratio = Math.max(-1, Math.min(1, clampedX / projectedX)); + if (currentPose.getX() > maximumXCoordinate && inputVector.getX() > 0) { + ratio *= -1 * BACKOFF_FACTOR; + } + Vector2d clampedInputVector = new Vector2d(inputVector.getX() * ratio, inputVector.getY()).rotated(currentPose.getHeading()); + return new Pose2d(clampedInputVector, driverInput.getHeading()); + } + + + +} diff --git a/src/main/java/com/tearabite/ielib/localization/Walls.java b/src/main/java/com/tearabite/ielib/localization/Walls.java new file mode 100644 index 0000000..9b09a60 --- /dev/null +++ b/src/main/java/com/tearabite/ielib/localization/Walls.java @@ -0,0 +1,16 @@ +package com.tearabite.ielib.localization; + +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.LineSegment; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.*; +import org.apache.commons.math3.geometry.partitioning.BSPTree; + +public class Walls { + public Walls(BSPTree tree) { + + } +} diff --git a/src/test/AprilTagPoseEstimatorCoreTest.java b/src/test/AprilTagPoseEstimatorCoreTest.java new file mode 100644 index 0000000..9be31e4 --- /dev/null +++ b/src/test/AprilTagPoseEstimatorCoreTest.java @@ -0,0 +1,104 @@ +//package com.tearabite.ielib.localization; +// +//import static org.junit.jupiter.api.Assertions.assertThrows; +//import static org.junit.jupiter.api.Assertions.fail; +// +//import com.acmerobotics.roadrunner.Pose2d; +// +//import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +//import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +//import org.firstinspires.ftc.vision.apriltag.AprilTagMetadata; +//import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; +//import org.junit.jupiter.api.Test; +//import org.junit.jupiter.params.ParameterizedTest; +//import org.junit.jupiter.params.provider.Arguments; +//import org.junit.jupiter.params.provider.MethodSource; +// +//import java.security.InvalidParameterException; +//import java.util.stream.Stream; +// +//class AprilTagPoseEstimatorTest { +// +// private static final AprilTagMetadata metadata = new AprilTagMetadata( +// 2, +// "testTag", +// 0, +// new VectorF(60.25f, 35.41f, 4f), DistanceUnit.INCH, +// new Quaternion(0.3536f, -0.6124f, 0.6124f, -0.3536f, 0)); +// +// @Test +// public void estimatePose_null_throws() { +// AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(); +// assertThrows(InvalidParameterException.class, () -> estimator.estimatePose(null)); +// } +// +// @ParameterizedTest +// @MethodSource("provideEstimatePosTestValues") +// public void estimatePos_notNull_returnsPose(AprilTagMetadata metadata, AprilTagPoseFtc poseFtc, Pose2d robotOffsets, Pose2d expectedPose) { +// AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(robotOffsets); +// AprilTagDetection detection = new AprilTagDetection( +// 1, +// 0, +// 0, +// null, +// null, +// metadata, +// poseFtc, +// null, +// 0); +// +// Pose2d estimatedPose = estimator.estimatePose(detection); +// +// assertIsClose(estimatedPose, expectedPose); +// } +// +// private static Stream provideEstimatePosTestValues() { +// return Stream.of( +// Arguments.of( +// metadata, +// new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), +// new Pose2d(-7.77, 0.505, 0), +// new Pose2d(28.5, 35.9, 0)), +// Arguments.of( +// metadata, +// new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, -45, 0), +// new Pose2d(-7.77, 0.505, 0), +// new Pose2d(35.5, 18.9, 0)), +// Arguments.of( +// metadata, +// new AprilTagPoseFtc(0, 0, 0, -45, 0, 0, 24, -45, 0), +// new Pose2d(-7.77, 0.505, 0), +// new Pose2d(31.1, 41.3, -Math.PI / 4)), +// +// Arguments.of( +// metadata, +// new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), +// new Pose2d(8.9, -1.5, Math.PI), +// new Pose2d(27.4, 36.9, Math.PI)), +// +// Arguments.of( +// metadata, +// new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), +// new Pose2d(8.9, -1.5, Math.PI * 3 / 4), +// new Pose2d(28.9, 30.2, -Math.PI * 3 / 4)) +// ); +// } +// +// private boolean isClose(double a, double b) { +// return Math.abs(a - b) < 0.1; +// } +// +// private void assertIsClose(Pose2d a, Pose2d b) { +// boolean isClose = isClose(a.position.x, b.position.x) +// && isClose(a.position.y, b.position.y) +// && isClose(a.heading.toDouble(), b.heading.toDouble()); +// +// if (!isClose) { +// fail(String.format("Expected (%.1f, %.1f, %.1f) to be close to (%.1f, %.1f, %.1f)", +// a.position.x, a.position.y, a.heading.toDouble(), +// b.position.x, b.position.y, b.heading.toDouble())); +// } +// } +//} \ No newline at end of file