Initial commit
This commit is contained in:
commit
fa25c4c6b4
|
|
@ -0,0 +1 @@
|
|||
/build
|
||||
|
|
@ -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()
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -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) {
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -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()));
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
Loading…
Reference in New Issue