Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add nearest to Pose2d and Translation2d (#4882)

Browse Source 
Co-authored-by: David Vo <auscompgeek@users.noreply.github.com>
This commit is contained in:
Ryan Blue
2023-02-03 18:27:16 -05:00
committed by GitHub
parent 08a536291b
commit 7b828ce84f
10 changed files with 279 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -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<Pose2d> {
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<Pose2d> 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);

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -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<Translation2d> {
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<Translation2d> 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);

32
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -8,6 +8,8 @@
#include <wpi/json.h>
#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<const Pose2d> 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<Pose2d> 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()}};

16
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -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<const Translation2d> 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<Translation2d> 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()}};

17
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -4,6 +4,9 @@
#pragma once
#include <initializer_list>
#include <span>
#include <wpi/SymbolExports.h>
#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<const Pose2d> 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<Pose2d> poses) const;
private:
Translation2d m_translation;
Rotation2d m_rotation;

18
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -4,6 +4,9 @@
#pragma once
#include <initializer_list>
#include <span>
#include <wpi/SymbolExports.h>
#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<const Translation2d> 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<Translation2d> translations) const;
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;

47
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -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)));
}
}

17
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
View File

@@ -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);
}
}

68
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
#include <cstdlib>
#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},

31
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -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};