From 7b828ce84ffd144b224591bddf9d628e32342503 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Fri, 3 Feb 2023 18:27:16 -0500 Subject: [PATCH] [wpimath] Add nearest to Pose2d and Translation2d (#4882) Co-authored-by: David Vo --- .../edu/wpi/first/math/geometry/Pose2d.java | 20 ++++++ .../first/math/geometry/Translation2d.java | 13 ++++ .../src/main/native/cpp/geometry/Pose2d.cpp | 32 +++++++++ .../native/cpp/geometry/Translation2d.cpp | 16 +++++ .../main/native/include/frc/geometry/Pose2d.h | 17 +++++ .../include/frc/geometry/Translation2d.h | 18 +++++ .../wpi/first/math/geometry/Pose2dTest.java | 47 +++++++++++++ .../math/geometry/Translation2dTest.java | 17 +++++ .../test/native/cpp/geometry/Pose2dTest.cpp | 68 +++++++++++++++++++ .../native/cpp/geometry/Translation2dTest.cpp | 31 +++++++++ 10 files changed, 279 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index bce832e8c4..e744ffe656 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -9,6 +9,9 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.interpolation.Interpolatable; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; import java.util.Objects; /** Represents a 2D pose containing translational and rotational elements. */ @@ -238,6 +241,23 @@ public class Pose2d implements Interpolatable { return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); } + /** + * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same + * distance from this pose, return the one with the closest rotation component. + * + * @param poses The list of poses to find the nearest. + * @return The nearest Pose2d from the list. + */ + public Pose2d nearest(List poses) { + return Collections.min( + poses, + Comparator.comparing( + (Pose2d other) -> this.getTranslation().getDistance(other.getTranslation())) + .thenComparing( + (Pose2d other) -> + Math.abs(this.getRotation().minus(other.getRotation()).getRadians()))); + } + @Override public String toString() { return String.format("Pose2d(%s, %s)", m_translation, m_rotation); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 2d57edca1c..406a192b63 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -10,6 +10,9 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; import java.util.Objects; /** @@ -185,6 +188,16 @@ public class Translation2d implements Interpolatable { return new Translation2d(m_x / scalar, m_y / scalar); } + /** + * Returns the nearest Translation2d from a list of translations. + * + * @param translations The list of translations. + * @return The nearest Translation2d from the list. + */ + public Translation2d nearest(List translations) { + return Collections.min(translations, Comparator.comparing(this::getDistance)); + } + @Override public String toString() { return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y); diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 2648a909de..53779d05c8 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -8,6 +8,8 @@ #include +#include "frc/MathUtil.h" + using namespace frc; Transform2d Pose2d::operator-(const Pose2d& other) const { @@ -67,6 +69,36 @@ Twist2d Pose2d::Log(const Pose2d& end) const { return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}}; } +Pose2d Pose2d::Nearest(std::span poses) const { + return *std::min_element( + poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) { + auto aDistance = this->Translation().Distance(a.Translation()); + auto bDistance = this->Translation().Distance(b.Translation()); + + // If the distances are equal sort by difference in rotation + if (aDistance == bDistance) { + return std::abs((this->Rotation() - a.Rotation()).Radians().value()) < + std::abs((this->Rotation() - b.Rotation()).Radians().value()); + } + return aDistance < bDistance; + }); +} + +Pose2d Pose2d::Nearest(std::initializer_list poses) const { + return *std::min_element( + poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) { + auto aDistance = this->Translation().Distance(a.Translation()); + auto bDistance = this->Translation().Distance(b.Translation()); + + // If the distances are equal sort by difference in rotation + if (aDistance == bDistance) { + return std::abs((this->Rotation() - a.Rotation()).Radians().value()) < + std::abs((this->Rotation() - b.Rotation()).Radians().value()); + } + return aDistance < bDistance; + }); +} + void frc::to_json(wpi::json& json, const Pose2d& pose) { json = wpi::json{{"translation", pose.Translation()}, {"rotation", pose.Rotation()}}; diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index d463696e6d..c8897d9101 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -23,6 +23,22 @@ bool Translation2d::operator==(const Translation2d& other) const { units::math::abs(m_y - other.m_y) < 1E-9_m; } +Translation2d Translation2d::Nearest( + std::span translations) const { + return *std::min_element(translations.begin(), translations.end(), + [this](Translation2d a, Translation2d b) { + return this->Distance(a) < this->Distance(b); + }); +} + +Translation2d Translation2d::Nearest( + std::initializer_list translations) const { + return *std::min_element(translations.begin(), translations.end(), + [this](Translation2d a, Translation2d b) { + return this->Distance(a) < this->Distance(b); + }); +} + void frc::to_json(wpi::json& json, const Translation2d& translation) { json = wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}}; diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index d096e8c9ed..12e82eddd1 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -4,6 +4,9 @@ #pragma once +#include +#include + #include #include "Transform2d.h" @@ -176,6 +179,20 @@ class WPILIB_DLLEXPORT Pose2d { */ Twist2d Log(const Pose2d& end) const; + /** + * Returns the nearest Pose2d from a collection of poses + * @param poses The collection of poses. + * @return The nearest Pose2d from the collection. + */ + Pose2d Nearest(std::span poses) const; + + /** + * Returns the nearest Pose2d from a collection of poses + * @param poses The collection of poses. + * @return The nearest Pose2d from the collection. + */ + Pose2d Nearest(std::initializer_list poses) const; + private: Translation2d m_translation; Rotation2d m_rotation; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index e1685108a3..d6a7f20b41 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -4,6 +4,9 @@ #pragma once +#include +#include + #include #include "Rotation2d.h" @@ -170,6 +173,21 @@ class WPILIB_DLLEXPORT Translation2d { */ bool operator==(const Translation2d& other) const; + /** + * Returns the nearest Translation2d from a collection of translations + * @param translations The collection of translations. + * @return The nearest Translation2d from the collection. + */ + Translation2d Nearest(std::span translations) const; + + /** + * Returns the nearest Translation2d from a collection of translations + * @param translations The collection of translations. + * @return The nearest Translation2d from the collection. + */ + Translation2d Nearest( + std::initializer_list translations) const; + private: units::meter_t m_x = 0_m; units::meter_t m_y = 0_m; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 780c816eaa..19fc76aac5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import java.util.List; import org.junit.jupiter.api.Test; class Pose2dTest { @@ -65,4 +66,50 @@ class Pose2dTest { () -> assertEquals(0.0, transform.getY(), kEpsilon), () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon)); } + + @Test + void testNearest() { + var origin = new Pose2d(); + + // Distance sort + // each poseX is X units away from the origin at a random angle. + final var pose1 = + new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), new Rotation2d()); + final var pose2 = + new Pose2d(new Translation2d(2, Rotation2d.fromDegrees(90)), new Rotation2d()); + final var pose3 = + new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), new Rotation2d()); + final var pose4 = + new Pose2d(new Translation2d(4, Rotation2d.fromDegrees(180)), new Rotation2d()); + final var pose5 = + new Pose2d(new Translation2d(5, Rotation2d.fromDegrees(270)), new Rotation2d()); + + assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4))); + assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3))); + assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3))); + + // Rotation component sort (when distance is the same) + // Use the same translation because using different angles at the same distance can cause + // rounding error. + final var translation = new Translation2d(1, new Rotation2d()); + + final var poseA = new Pose2d(translation, Rotation2d.fromDegrees(0)); + final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30)); + final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120)); + final var poseD = new Pose2d(translation, Rotation2d.fromDegrees(90)); + final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180)); + + assertEquals( + poseA, new Pose2d(0, 0, Rotation2d.fromDegrees(360)).nearest(List.of(poseA, poseB, poseD))); + assertEquals( + poseB, + new Pose2d(0, 0, Rotation2d.fromDegrees(-335)).nearest(List.of(poseB, poseC, poseD))); + assertEquals( + poseC, + new Pose2d(0, 0, Rotation2d.fromDegrees(-120)).nearest(List.of(poseB, poseC, poseD))); + assertEquals( + poseD, new Pose2d(0, 0, Rotation2d.fromDegrees(85)).nearest(List.of(poseA, poseC, poseD))); + assertEquals( + poseE, new Pose2d(0, 0, Rotation2d.fromDegrees(170)).nearest(List.of(poseA, poseD, poseE))); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java index c8f5690506..6ed8a61478 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java @@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import java.util.List; import org.junit.jupiter.api.Test; class Translation2dTest { @@ -114,4 +115,20 @@ class Translation2dTest { () -> assertEquals(1.0, two.getX(), kEpsilon), () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon)); } + + @Test + void testNearest() { + var origin = new Translation2d(); + + // each translationX is X units away from the origin at a random angle. + var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45)); + var translation2 = new Translation2d(2, Rotation2d.fromDegrees(90)); + var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135)); + var translation4 = new Translation2d(4, Rotation2d.fromDegrees(180)); + var translation5 = new Translation2d(5, Rotation2d.fromDegrees(270)); + + assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3); + assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1); + assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 5ce6819167..be5055decc 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include "frc/geometry/Pose2d.h" #include "gtest/gtest.h" @@ -54,6 +55,73 @@ TEST(Pose2dTest, Minus) { EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value()); } +TEST(Pose2dTest, Nearest) { + const Pose2d origin{0_m, 0_m, 0_deg}; + + const Pose2d pose1{Translation2d{1_m, Rotation2d{45_deg}}, 0_deg}; + const Pose2d pose2{Translation2d{2_m, Rotation2d{90_deg}}, 0_deg}; + const Pose2d pose3{Translation2d{3_m, Rotation2d{135_deg}}, 0_deg}; + const Pose2d pose4{Translation2d{4_m, Rotation2d{180_deg}}, 0_deg}; + const Pose2d pose5{Translation2d{5_m, Rotation2d{270_deg}}, 0_deg}; + + EXPECT_DOUBLE_EQ(pose3.X().value(), + origin.Nearest({pose5, pose3, pose4}).X().value()); + EXPECT_DOUBLE_EQ(pose3.Y().value(), + origin.Nearest({pose5, pose3, pose4}).Y().value()); + + EXPECT_DOUBLE_EQ(pose1.X().value(), + origin.Nearest({pose1, pose2, pose3}).X().value()); + EXPECT_DOUBLE_EQ(pose1.Y().value(), + origin.Nearest({pose1, pose2, pose3}).Y().value()); + + EXPECT_DOUBLE_EQ(pose2.X().value(), + origin.Nearest({pose4, pose2, pose3}).X().value()); + EXPECT_DOUBLE_EQ(pose2.Y().value(), + origin.Nearest({pose4, pose2, pose3}).Y().value()); + + // Rotation component sort (when distance is the same) + // Use the same translation because using different angles at the same + // distance can cause rounding error. + const Translation2d translation{1_m, Rotation2d{0_deg}}; + + const Pose2d poseA{translation, 0_deg}; + const Pose2d poseB{translation, Rotation2d{30_deg}}; + const Pose2d poseC{translation, Rotation2d{120_deg}}; + const Pose2d poseD{translation, Rotation2d{90_deg}}; + const Pose2d poseE{translation, Rotation2d{-180_deg}}; + + EXPECT_DOUBLE_EQ(poseA.Rotation().Degrees().value(), + Pose2d(0_m, 0_m, Rotation2d{360_deg}) + .Nearest({poseA, poseB, poseD}) + .Rotation() + .Degrees() + .value()); + EXPECT_DOUBLE_EQ(poseB.Rotation().Degrees().value(), + Pose2d(0_m, 0_m, Rotation2d{-335_deg}) + .Nearest({poseB, poseC, poseD}) + .Rotation() + .Degrees() + .value()); + EXPECT_DOUBLE_EQ(poseC.Rotation().Degrees().value(), + Pose2d(0_m, 0_m, Rotation2d{-120_deg}) + .Nearest({poseB, poseC, poseD}) + .Rotation() + .Degrees() + .value()); + EXPECT_DOUBLE_EQ(poseD.Rotation().Degrees().value(), + Pose2d(0_m, 0_m, Rotation2d{85_deg}) + .Nearest({poseA, poseC, poseD}) + .Rotation() + .Degrees() + .value()); + EXPECT_DOUBLE_EQ(poseE.Rotation().Degrees().value(), + Pose2d(0_m, 0_m, Rotation2d{170_deg}) + .Nearest({poseA, poseD, poseE}) + .Rotation() + .Degrees() + .value()); +} + TEST(Pose2dTest, Constexpr) { constexpr Pose2d defaultConstructed; constexpr Pose2d translationRotation{Translation2d{0_m, 1_m}, diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 5493c43c19..209e343c26 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -94,6 +94,37 @@ TEST(Translation2dTest, PolarConstructor) { EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value()); } +TEST(Translation2dTest, Nearest) { + const Translation2d origin{0_m, 0_m}; + + const Translation2d translation1{1_m, Rotation2d{45_deg}}; + const Translation2d translation2{2_m, Rotation2d{90_deg}}; + const Translation2d translation3{3_m, Rotation2d{135_deg}}; + const Translation2d translation4{4_m, Rotation2d{180_deg}}; + const Translation2d translation5{5_m, Rotation2d{270_deg}}; + + EXPECT_DOUBLE_EQ( + origin.Nearest({translation5, translation3, translation4}).X().value(), + translation3.X().value()); + EXPECT_DOUBLE_EQ( + origin.Nearest({translation5, translation3, translation4}).Y().value(), + translation3.Y().value()); + + EXPECT_DOUBLE_EQ( + origin.Nearest({translation1, translation2, translation3}).X().value(), + translation1.X().value()); + EXPECT_DOUBLE_EQ( + origin.Nearest({translation1, translation2, translation3}).Y().value(), + translation1.Y().value()); + + EXPECT_DOUBLE_EQ( + origin.Nearest({translation4, translation2, translation3}).X().value(), + translation2.X().value()); + EXPECT_DOUBLE_EQ( + origin.Nearest({translation4, translation2, translation3}).Y().value(), + translation2.Y().value()); +} + TEST(Translation2dTest, Constexpr) { constexpr Translation2d defaultCtor; constexpr Translation2d componentCtor{1_m, 2_m};