Initial commit

2024-03-09 09:30:43 -06:00 · 2024-03-09 09:30:43 -06:00 · fa25c4c6b4
commit fa25c4c6b4
6 changed files with 263 additions and 0 deletions
--- a/.gitignore
+++ b/.gitignore
@ -0,0 +1 @@
+/build
--- a/build.gradle
+++ 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()
+}
--- a/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimatorCore.java
+++ 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;
+    }
+}
--- a/src/main/java/com/tearabite/ielib/localization/InvisibleBarrierCore.java
+++ 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<Vector2d> 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());
+    }
+
+
+
+}
--- a/src/main/java/com/tearabite/ielib/localization/Walls.java
+++ 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) {
+
+    }
+}
--- a/src/test/AprilTagPoseEstimatorCoreTest.java
+++ 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<Arguments> 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()));
+//        }
+//    }
+//}