eaglesfe
/
ielib
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fdfddfd

This commit is contained in:
Scott Barnes 2024-03-08 06:24:50 -06:00
parent 3b33d797cb
commit 038974fff0
4 changed files with 8 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
build.gradle
View File

@ -47,11 +47,11 @@ dependencies {
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.2'
//noinspection AnnotationProcessorOnCompilePath
compileOnly 'org.projectlombok:lombok:1.18.30'
implementation 'org.projectlombok:lombok:1.18.30'
annotationProcessor 'org.projectlombok:lombok:1.18.30'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
api project(':ielib-core')
}
task androidSourcesJar(type: Jar) {

2
settings.gradle
View File

@ -1 +1 @@
include ':ftctearabits'
include ':ielib'

63
src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimator.java
View File

@ -1,13 +1,7 @@
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;
import com.tearabite.ielib.localization.AprilTagPoseEstimatorCore;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
@ -16,16 +10,12 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
import java.security.InvalidParameterException;
import lombok.AllArgsConstructor;
import lombok.Builder;
import lombok.Getter;
import lombok.NoArgsConstructor;
import lombok.Setter;
import lombok.experimental.SuperBuilder;
@NoArgsConstructor
@AllArgsConstructor
@Builder(toBuilder = true)
public class AprilTagPoseEstimator {
@Getter
@SuperBuilder(toBuilder=true)
public class AprilTagPoseEstimator extends AprilTagPoseEstimatorCore {
/*
* This class is used to estimate the pose of the robot using the AprilTagDetection object.
@ -35,8 +25,6 @@ public class AprilTagPoseEstimator {
* https://www.desmos.com/calculator/n2iyatwssg
*/
@Getter @Setter @Builder.Default private Pose2d robotOffset = new Pose2d(0,0,0);
/**
* Estimates the pose of the robot using the AprilTagDetection object.
* @param detection The AprilTagDetection object
@ -61,45 +49,4 @@ public class AprilTagPoseEstimator {
fieldOrientation.z,
fieldOrientation.w);
}
/**
* 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.
*/
private Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td) {
double Tyaw = getTYaw(Ta, Tb, Tc, Td);
double ch = (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 = this.robotOffset.getX();
double offsetY = this.robotOffset.getY();
double Cyaw = this.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);
return new Pose2d(rx, ry, rh);
}
private static double getTPitch(double a, double b, double c, double d) {
return asin(2 * ((a * d) - (b * c)));
}
private static double getTRoll(double a, double b, double c, double d) {
return 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 atan2(2 * ((a * b) + (c * d)), 1 - 2 * ((pow(b, 2) + pow(c, 2))));
}
}

105
src/test/java/com/tearabite/ielib/localization/localization/AprilTagPoseEstimatorTest.java
View File

@ -1,105 +0,0 @@
package com.tearabite.ielib.localization.localization;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.fail;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.tearabite.ielib.localization.AprilTagPoseEstimator;
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;
public 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.getX(), b.getX())
&& isClose(a.getY(), b.getY())
&& isClose(a.getHeading(), b.getHeading());
if (!isClose) {
fail(String.format("Expected (%.1f, %.1f, %.1f) to be close to (%.1f, %.1f, %.1f)",
a.getX(), a.getY(), a.getHeading(),
b.getX(), b.getY(), b.getHeading()));
}
}
}