diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp new file mode 100644 index 0000000000..0158050ba0 --- /dev/null +++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/geometry/Pose2d.h" + +#include + +using namespace frc; + +Pose2d::Pose2d(Translation2d translation, Rotation2d rotation) + : m_translation(translation), m_rotation(rotation) {} + +Pose2d::Pose2d(double x, double y, Rotation2d rotation) + : m_translation(x, y), m_rotation(rotation) {} + +Pose2d Pose2d::operator+(const Transform2d& other) const { + return TransformBy(other); +} + +Pose2d& Pose2d::operator+=(const Transform2d& other) { + m_translation += other.Translation().RotateBy(m_rotation); + m_rotation += other.Rotation(); + return *this; +} + +Pose2d Pose2d::TransformBy(const Transform2d& other) const { + return {m_translation + (other.Translation().RotateBy(m_rotation)), + m_rotation + other.Rotation()}; +} + +Pose2d Pose2d::RelativeTo(const Pose2d& other) const { + const Transform2d transform{other, *this}; + return {transform.Translation(), transform.Rotation()}; +} + +Pose2d Pose2d::Exp(const Twist2d& twist) const { + const auto dx = twist.dx; + const auto dy = twist.dy; + const auto dtheta = twist.dtheta; + + const auto sinTheta = std::sin(dtheta); + const auto cosTheta = std::cos(dtheta); + + double s, c; + if (std::abs(dtheta) < 1E-9) { + s = 1.0 - 1.0 / 6.0 * dtheta * dtheta; + c = 0.5 * dtheta; + } else { + s = sinTheta / dtheta; + c = (1 - cosTheta) / dtheta; + } + + const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s}, + Rotation2d{cosTheta, sinTheta}}; + + return *this + transform; +} diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp new file mode 100644 index 0000000000..081a4bf10f --- /dev/null +++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/geometry/Rotation2d.h" + +#include + +using namespace frc; + +Rotation2d::Rotation2d(double value) + : m_value(value), m_cos(std::cos(value)), m_sin(std::sin(value)) {} + +Rotation2d::Rotation2d(double x, double y) { + const auto magnitude = std::hypot(x, y); + if (magnitude > 1e-6) { + m_sin = y / magnitude; + m_cos = x / magnitude; + } else { + m_sin = 0.0; + m_cos = 1.0; + } + m_value = std::atan2(m_sin, m_cos); +} + +Rotation2d Rotation2d::FromDegrees(double degrees) { + return Rotation2d(Deg2Rad(degrees)); +} + +Rotation2d Rotation2d::operator+(const Rotation2d& other) const { + return RotateBy(other); +} + +Rotation2d& Rotation2d::operator+=(const Rotation2d& other) { + double cos = Cos() * other.Cos() - Sin() * other.Sin(); + double sin = Cos() * other.Sin() + Sin() * other.Cos(); + m_cos = cos; + m_sin = sin; + m_value = std::atan2(m_sin, m_cos); + return *this; +} + +Rotation2d Rotation2d::operator-(const Rotation2d& other) const { + return *this + -other; +} + +Rotation2d& Rotation2d::operator-=(const Rotation2d& other) { + *this += -other; + return *this; +} + +Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); } + +Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const { + return {Cos() * other.Cos() - Sin() * other.Sin(), + Cos() * other.Sin() + Sin() * other.Cos()}; +} diff --git a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp new file mode 100644 index 0000000000..b184d1b98f --- /dev/null +++ b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/geometry/Transform2d.h" + +#include "frc/geometry/Pose2d.h" + +using namespace frc; + +Transform2d::Transform2d(Pose2d initial, Pose2d final) { + // We are rotating the difference between the translations + // using a clockwise rotation matrix. This transforms the global + // delta into a local delta (relative to the initial pose). + m_translation = (final.Translation() - initial.Translation()) + .RotateBy(-initial.Rotation()); + + m_rotation = final.Rotation() - initial.Rotation(); +} + +Transform2d::Transform2d(Translation2d translation, Rotation2d rotation) + : m_translation(translation), m_rotation(rotation) {} diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp new file mode 100644 index 0000000000..dc53195f22 --- /dev/null +++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/geometry/Translation2d.h" + +#include + +using namespace frc; + +Translation2d::Translation2d(double x, double y) : m_x(x), m_y(y) {} + +double Translation2d::Distance(const Translation2d& other) const { + return std::hypot(other.m_x - m_x, other.m_y - m_y); +} + +double Translation2d::Norm() const { return std::hypot(m_x, m_y); } + +Translation2d Translation2d::RotateBy(const Rotation2d& other) const { + return {m_x * other.Cos() - m_y * other.Sin(), + m_x * other.Sin() + m_y * other.Cos()}; +} + +Translation2d Translation2d::operator+(const Translation2d& other) const { + return {X() + other.X(), Y() + other.Y()}; +} + +Translation2d& Translation2d::operator+=(const Translation2d& other) { + m_x += other.m_x; + m_y += other.m_y; + return *this; +} + +Translation2d Translation2d::operator-(const Translation2d& other) const { + return *this + -other; +} + +Translation2d& Translation2d::operator-=(const Translation2d& other) { + *this += -other; + return *this; +} + +Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; } + +Translation2d Translation2d::operator*(double scalar) const { + return {scalar * m_x, scalar * m_y}; +} + +Translation2d& Translation2d::operator*=(double scalar) { + m_x *= scalar; + m_y *= scalar; + return *this; +} + +Translation2d Translation2d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + +Translation2d& Translation2d::operator/=(double scalar) { + *this *= (1.0 / scalar); + return *this; +} diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h new file mode 100644 index 0000000000..38912c69f0 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h @@ -0,0 +1,136 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Transform2d.h" +#include "Translation2d.h" +#include "Twist2d.h" + +namespace frc { + +/** + * Represents a 2d pose containing translational and rotational elements. + */ +class Pose2d { + public: + /** + * Constructs a pose at the origin facing toward the positive X axis. + * (Translation2d{0, 0} and Rotation{0}) + */ + constexpr Pose2d() = default; + + /** + * Constructs a pose with the specified translation and rotation. + * + * @param translation The translational component of the pose. + * @param rotation The rotational component of the pose. + */ + Pose2d(Translation2d translation, Rotation2d rotation); + + /** + * Convenience constructors that takes in x and y values directly instead of + * having to construct a Translation2d. + * + * @param x The x component of the translational component of the pose. + * @param y The y component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + Pose2d(double x, double y, Rotation2d rotation); + + /** + * Transforms the pose by the given transformation and returns the new + * transformed pose. + * + * [x_new] [cos, -sin, 0][transform.x] + * [y_new] += [sin, cos, 0][transform.y] + * [t_new] [0, 0, 1][transform.t] + * + * @param other The transform to transform the pose by. + * + * @return The transformed pose. + */ + Pose2d operator+(const Transform2d& other) const; + + /** + * Transforms the current pose by the transformation. + * + * This is similar to the + operator, except that it mutates the current + * object. + * + * @param other The transform to transform the pose by. + * + * @return Reference to the new mutated object. + */ + Pose2d& operator+=(const Transform2d& other); + + /** + * Returns the underlying translation. + * + * @return Reference to the translational component of the pose. + */ + const Translation2d& Translation() const { return m_translation; } + + /** + * Returns the underlying rotation. + * + * @return Reference to the rotational component of the pose. + */ + const Rotation2d& Rotation() const { return m_rotation; } + + /** + * Transforms the pose by the given transformation and returns the new pose. + * See + operator for the matrix multiplication performed. + * + * @param other The transform to transform the pose by. + * + * @return The transformed pose. + */ + Pose2d TransformBy(const Transform2d& other) const; + + /** + * Returns the other pose relative to the current pose. + * + * This function can often be used for trajectory tracking or pose + * stabilization algorithms to get the error between the reference and the + * current pose. + * + * @param other The pose that is the origin of the new coordinate frame that + * the current pose will be converted into. + * + * @return The current pose relative to the new origin pose. + */ + Pose2d RelativeTo(const Pose2d& other) const; + + /** + * Obtain a new Pose2d from a (constant curvature) velocity. + * + * See section on + * nonlinear pose estimation for derivation. + * + * The twist is a change in pose in the robot's coordinate frame since the + * previous pose update. When the user runs exp() on the previous known + * field-relative pose with the argument being the twist, the user will + * receive the new field-relative pose. + * + * "Exp" represents the pose exponential, which is solving a differential + * equation moving the pose forward in time. + * + * @param twist The change in pose in the robot's coordinate frame since the + * previous pose update. For example, if a non-holonomic robot moves forward + * 0.01 meters and changes angle by .5 degrees since the previous pose update, + * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)} + * + * @return The new pose of the robot. + */ + Pose2d Exp(const Twist2d& twist) const; + + private: + Translation2d m_translation; + Rotation2d m_rotation; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h new file mode 100644 index 0000000000..324cd83c94 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h @@ -0,0 +1,172 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace frc { + +/** + * A rotation in a 2d coordinate frame represented a point on the unit circle + * (cosine and sine). + */ +class Rotation2d { + public: + /** + * Constructs a Rotation2d with a default angle of 0 degrees. + */ + constexpr Rotation2d() = default; + + /** + * Constructs a Rotation2d with the given radian value. + * + * @param value The value of the angle in radians. + */ + explicit Rotation2d(double value); + + /** + * Constructs a Rotation2d with the given x and y (cosine and sine) + * components. The x and y don't have to be normalized. + * + * @param x The x component or cosine of the rotation. + * @param y The y component or sine of the rotation. + */ + Rotation2d(double x, double y); + + /** + * Constructs and returns a Rotation2d with the given degree value. + * + * @param degrees The value of the angle in degrees. + * + * @return The rotation object with the desired angle value. + */ + static Rotation2d FromDegrees(double degrees); + + /** + * Adds two rotations together, with the result being bounded between -kPi and + * kPi. + * + * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) = + * Rotation2d{-kPi/2} + * + * @param other The rotation to add. + * + * @return The sum of the two rotations. + */ + Rotation2d operator+(const Rotation2d& other) const; + + /** + * Adds a rotation to the current rotation. + * + * This is similar to the + operator except that it mutates the current + * object. + * + * @param other The rotation to add. + * + * @return The reference to the new mutated object. + */ + Rotation2d& operator+=(const Rotation2d& other); + + /** + * Subtracts the new rotation from the current rotation and returns the new + * rotation. + * + * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) = + * Rotation2d{-kPi/2} + * + * @param other The rotation to subtract. + * + * @return The difference between the two rotations. + */ + Rotation2d operator-(const Rotation2d& other) const; + + /** + * Subtracts the new rotation from the current rotation. + * + * This is similar to the - operator except that it mutates the current + * object. + * + * @param other The rotation to subtract. + * + * @return The reference to the new mutated object. + */ + Rotation2d& operator-=(const Rotation2d& other); + + /** + * Takes the inverse of the current rotation. This is simply the negative of + * the current angular value. + * + * @return The inverse of the current rotation. + */ + Rotation2d operator-() const; + + /** + * Adds the new rotation to the current rotation using a rotation matrix. + * + * [cos_new] [other.cos, -other.sin][cos] + * [sin_new] = [other.sin, other.cos][sin] + * + * value_new = std::atan2(cos_new, sin_new) + * + * @param other The rotation to rotate by. + * + * @return The new rotated Rotation2d. + */ + Rotation2d RotateBy(const Rotation2d& other) const; + + /** + * Returns the radian value of the rotation. + * + * @return The radian value of the rotation. + */ + double Radians() const { return m_value; } + + /** + * Returns the degree value of the rotation. + * + * @return The degree value of the rotation. + */ + double Degrees() const { return Rad2Deg(m_value); } + + /** + * Returns the cosine of the rotation. + * + * @return The cosine of the rotation. + */ + double Cos() const { return m_cos; } + + /** + * Returns the sine of the rotation. + * + * @return The sine of the rotation. + */ + double Sin() const { return m_sin; } + + /** + * Returns the tangent of the rotation. + * + * @return The tangent of the rotation. + */ + double Tan() const { return m_sin / m_cos; } + + private: + double m_value = 0; + double m_cos = 1; + double m_sin = 0; + + constexpr static double kPi = 3.14159265358979323846; + + template + static T Rad2Deg(const T& rad) { + return rad * 180.0 / kPi; + } + + template + static T Deg2Rad(const T& deg) { + return deg * kPi / 180.0; + } +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h new file mode 100644 index 0000000000..1fbfcf4e09 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Translation2d.h" + +namespace frc { + +class Pose2d; + +/** + * Represents a transformation for a Pose2d. + */ +class Transform2d { + public: + /** + * Constructs the transform that maps the initial pose to the final pose. + * + * @param initial The initial pose for the transformation. + * @param final The final pose for the transformation. + */ + Transform2d(Pose2d initial, Pose2d final); + + /** + * Constructs a transform with the given translation and rotation components. + * + * @param translation Translational component of the transform. + * @param rotation Rotational component of the transform. + */ + Transform2d(Translation2d translation, Rotation2d rotation); + + /** + * Constructs the identity transform -- maps an initial pose to itself. + */ + constexpr Transform2d() = default; + + /** + * Returns the translation component of the transformation. + * + * @return Reference to the translational component of the transform. + */ + const Translation2d& Translation() const { return m_translation; } + + /** + * Returns the rotational component of the transformation. + * + * @return Reference to the rotational component of the transform. + */ + const Rotation2d& Rotation() const { return m_rotation; } + + private: + Translation2d m_translation; + Rotation2d m_rotation; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h new file mode 100644 index 0000000000..52f65a5bb7 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h @@ -0,0 +1,196 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Rotation2d.h" + +namespace frc { + +/** + * Represents a translation in 2d space. + * This object can be used to represent a point or a vector. + * + * This assumes that you are using conventional mathematical axes. + * When the robot is placed on the origin, facing toward the X direction, + * moving forward increases the X, whereas moving to the left increases the Y. + */ +class Translation2d { + public: + /** + * Constructs a Translation2d with X and Y components equal to zero. + */ + constexpr Translation2d() = default; + + /** + * Constructs a Translation2d with the X and Y components equal to the + * provided values. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + */ + Translation2d(double x, double y); + + /** + * Calculates the distance between two translations in 2d space. + * + * This function uses the pythagorean theorem to calculate the distance. + * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2) + * + * @param other The translation to compute the distance to. + * + * @return The distance between the two translations. + */ + double Distance(const Translation2d& other) const; + + /** + * Returns the X component of the translation. + * + * @return The x component of the translation. + */ + double X() const { return m_x; } + + /** + * Returns the Y component of the translation. + * + * @return The y component of the translation. + */ + double Y() const { return m_y; } + + /** + * Returns the norm, or distance from the origin to the translation. + * + * @return The norm of the translation. + */ + double Norm() const; + + /** + * Applies a rotation to the translation in 2d space. + * + * This multiplies the translation vector by a counterclockwise rotation + * matrix of the given angle. + * + * [x_new] [other.cos, -other.sin][x] + * [y_new] = [other.sin, other.cos][y] + * + * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a + * Translation2d of {0, 2}. + * + * @param other The rotation to rotate the translation by. + * + * @return The new rotated translation. + */ + Translation2d RotateBy(const Rotation2d& other) const; + + /** + * Adds two translations in 2d space and returns the sum. This is similar to + * vector addition. + * + * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = + * Translation2d{3.0, 8.0} + * + * @param other The translation to add. + * + * @return The sum of the translations. + */ + Translation2d operator+(const Translation2d& other) const; + + /** + * Adds the new translation to the current translation. + * + * This is similar to the + operator, except that the current object is + * mutated. + * + * @param other The translation to add. + * + * @return The reference to the new mutated object. + */ + Translation2d& operator+=(const Translation2d& other); + + /** + * Subtracts the other translation from the other translation and returns the + * difference. + * + * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = + * Translation2d{4.0, 2.0} + * + * @param other The translation to subtract. + * + * @return The difference between the two translations. + */ + Translation2d operator-(const Translation2d& other) const; + + /** + * Subtracts the new translation from the current translation. + * + * This is similar to the - operator, except that the current object is + * mutated. + * + * @param other The translation to subtract. + * + * @return The reference to the new mutated object. + */ + Translation2d& operator-=(const Translation2d& other); + + /** + * Returns the inverse of the current translation. This is equivalent to + * rotating by 180 degrees, flipping the point over both axes, or simply + * negating both components of the translation. + * + * @return The inverse of the current translation. + */ + Translation2d operator-() const; + + /** + * Multiplies the translation by a scalar and returns the new translation. + * + * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0} + * + * @param scalar The scalar to multiply by. + * + * @return The scaled translation. + */ + Translation2d operator*(double scalar) const; + + /** + * Multiplies the current translation by a scalar. + * + * This is similar to the * operator, except that current object is mutated. + * + * @param scalar The scalar to multiply by. + * + * @return The reference to the new mutated object. + */ + Translation2d& operator*=(double scalar); + + /** + * Divides the translation by a scalar and returns the new translation. + * + * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25} + * + * @param scalar The scalar to divide by. + * + * @return The scaled translation. + */ + Translation2d operator/(double scalar) const; + + /* + * Divides the current translation by a scalar. + * + * This is similar to the / operator, except that current object is mutated. + * + * @param scalar The scalar to divide by. + * + * @return The reference to the new mutated object. + */ + Translation2d& operator/=(double scalar); + + private: + double m_x = 0; + double m_y = 0; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h new file mode 100644 index 0000000000..f61945a291 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace frc { +/** + * A change in distance along arc since the last pose update. We can use ideas + * from differential calculus to create new Pose2ds from a Twist2d and vise + * versa. + * + * A Twist can be used to represent a difference between two poses. + */ +struct Twist2d { + /** + * Linear "dx" component + */ + double dx = 0; + + /** + * Linear "dy" component + */ + double dy = 0; + + /** + * Angular "dtheta" component (radians) + */ + double dtheta = 0; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp new file mode 100644 index 0000000000..bf2d3dae62 --- /dev/null +++ b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/geometry/Pose2d.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(Pose2dTest, TransformBy) { + const Pose2d initial{1.0, 2.0, Rotation2d::FromDegrees(45.0)}; + const Transform2d transform{Translation2d{5.0, 0.0}, + Rotation2d::FromDegrees(5.0)}; + + const auto transformed = initial + transform; + + EXPECT_NEAR(transformed.Translation().X(), 1 + 5 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transformed.Translation().Y(), 2 + 5 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transformed.Rotation().Degrees(), 50.0, kEpsilon); +} + +TEST(Pose2dTest, RelativeTo) { + const Pose2d initial{0.0, 0.0, Rotation2d::FromDegrees(45.0)}; + const Pose2d final{5.0, 5.0, Rotation2d::FromDegrees(45.0)}; + + const auto finalRelativeToInitial = final.RelativeTo(initial); + + EXPECT_NEAR(finalRelativeToInitial.Translation().X(), 5.0 * std::sqrt(2.0), + kEpsilon); + EXPECT_NEAR(finalRelativeToInitial.Translation().Y(), 0.0, kEpsilon); + EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees(), 0.0, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp new file mode 100644 index 0000000000..24a4caf6e3 --- /dev/null +++ b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/geometry/Rotation2d.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; +static constexpr double kPi = 3.14159265358979323846; + +TEST(Rotation2dTest, RadiansToDegrees) { + const Rotation2d one{kPi / 3}; + const Rotation2d two{kPi / 4}; + + EXPECT_NEAR(one.Degrees(), 60.0, kEpsilon); + EXPECT_NEAR(two.Degrees(), 45.0, kEpsilon); +} + +TEST(Rotation2dTest, DegreesToRadians) { + const auto one = Rotation2d::FromDegrees(45.0); + const auto two = Rotation2d::FromDegrees(30.0); + + EXPECT_NEAR(one.Radians(), kPi / 4.0, kEpsilon); + EXPECT_NEAR(two.Radians(), kPi / 6.0, kEpsilon); +} + +TEST(Rotation2dTest, RotateByFromZero) { + const Rotation2d zero; + auto sum = zero + Rotation2d::FromDegrees(90.0); + + EXPECT_NEAR(sum.Radians(), kPi / 2.0, kEpsilon); + EXPECT_NEAR(sum.Degrees(), 90.0, kEpsilon); +} + +TEST(Rotation2dTest, RotateByNonZero) { + auto rot = Rotation2d::FromDegrees(90.0); + rot += Rotation2d::FromDegrees(30.0); + + EXPECT_NEAR(rot.Degrees(), 120.0, kEpsilon); +} + +TEST(Rotation2dTest, Minus) { + const auto one = Rotation2d::FromDegrees(70.0); + const auto two = Rotation2d::FromDegrees(30.0); + + EXPECT_NEAR((one - two).Degrees(), 40.0, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp new file mode 100644 index 0000000000..c1c54ad7cb --- /dev/null +++ b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/geometry/Translation2d.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(Translation2dTest, Sum) { + const Translation2d one{1.0, 3.0}; + const Translation2d two{2.0, 5.0}; + + const auto sum = one + two; + + EXPECT_NEAR(sum.X(), 3.0, kEpsilon); + EXPECT_NEAR(sum.Y(), 8.0, kEpsilon); +} + +TEST(Translation2dTest, Difference) { + const Translation2d one{1.0, 3.0}; + const Translation2d two{2.0, 5.0}; + + const auto difference = one - two; + + EXPECT_NEAR(difference.X(), -1.0, kEpsilon); + EXPECT_NEAR(difference.Y(), -2.0, kEpsilon); +} + +TEST(Translation2dTest, RotateBy) { + const Translation2d another{3.0, 0.0}; + const auto rotated = another.RotateBy(Rotation2d::FromDegrees(90.0)); + + EXPECT_NEAR(rotated.X(), 0.0, kEpsilon); + EXPECT_NEAR(rotated.Y(), 3.0, kEpsilon); +} + +TEST(Translation2dTest, Multiplication) { + const Translation2d original{3.0, 5.0}; + const auto mult = original * 3; + + EXPECT_NEAR(mult.X(), 9.0, kEpsilon); + EXPECT_NEAR(mult.Y(), 15.0, kEpsilon); +} + +TEST(Translation2d, Division) { + const Translation2d original{3.0, 5.0}; + const auto div = original / 2; + + EXPECT_NEAR(div.X(), 1.5, kEpsilon); + EXPECT_NEAR(div.Y(), 2.5, kEpsilon); +} + +TEST(Translation2dTest, Norm) { + const Translation2d one{3.0, 5.0}; + EXPECT_NEAR(one.Norm(), std::hypot(3, 5), kEpsilon); +} + +TEST(Translation2dTest, Distance) { + const Translation2d one{1, 1}; + const Translation2d two{6, 6}; + EXPECT_NEAR(one.Distance(two), 5 * std::sqrt(2), kEpsilon); +} + +TEST(Translation2dTest, UnaryMinus) { + const Translation2d original{-4.5, 7}; + const auto inverted = -original; + + EXPECT_NEAR(inverted.X(), 4.5, kEpsilon); + EXPECT_NEAR(inverted.Y(), -7, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp new file mode 100644 index 0000000000..01d64a1686 --- /dev/null +++ b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/geometry/Pose2d.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(Twist2dTest, Straight) { + const Twist2d straight{5.0, 0.0, 0.0}; + const auto straightPose = Pose2d().Exp(straight); + + EXPECT_NEAR(straightPose.Translation().X(), 5.0, kEpsilon); + EXPECT_NEAR(straightPose.Translation().Y(), 0.0, kEpsilon); + EXPECT_NEAR(straightPose.Rotation().Radians(), 0.0, kEpsilon); +} + +TEST(Twist2dTest, QuarterCircle) { + const Twist2d quarterCircle{5.0 / 2.0 * 3.14159265358979323846, 0, + 3.14159265358979323846 / 2.0}; + const auto quarterCirclePose = Pose2d().Exp(quarterCircle); + + EXPECT_NEAR(quarterCirclePose.Translation().X(), 5.0, kEpsilon); + EXPECT_NEAR(quarterCirclePose.Translation().Y(), 5.0, kEpsilon); + EXPECT_NEAR(quarterCirclePose.Rotation().Degrees(), 90.0, kEpsilon); +} + +TEST(Twist2dTest, DiagonalNoDtheta) { + const Twist2d diagonal{2.0, 2.0, 0.0}; + const auto diagonalPose = Pose2d().Exp(diagonal); + + EXPECT_NEAR(diagonalPose.Translation().X(), 2.0, kEpsilon); + EXPECT_NEAR(diagonalPose.Translation().Y(), 2.0, kEpsilon); + EXPECT_NEAR(diagonalPose.Rotation().Degrees(), 0.0, kEpsilon); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java new file mode 100644 index 0000000000..2d55ea0930 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java @@ -0,0 +1,157 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +/** + * Represents a 2d pose containing translational and rotational elements. + */ +public class Pose2d { + private final Translation2d m_translation; + private final Rotation2d m_rotation; + + /** + * Constructs a pose at the origin facing toward the positive X axis. + * (Translation2d{0, 0} and Rotation{0}) + */ + public Pose2d() { + m_translation = new Translation2d(); + m_rotation = new Rotation2d(); + } + + /** + * Constructs a pose with the specified translation and rotation. + * + * @param translation The translational component of the pose. + * @param rotation The rotational component of the pose. + */ + public Pose2d(Translation2d translation, Rotation2d rotation) { + m_translation = translation; + m_rotation = rotation; + } + + /** + * Convenience constructors that takes in x and y values directly instead of + * having to construct a Translation2d. + * + * @param x The x component of the translational component of the pose. + * @param y The y component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + @SuppressWarnings("ParameterName") + public Pose2d(double x, double y, Rotation2d rotation) { + m_translation = new Translation2d(x, y); + m_rotation = rotation; + } + + /** + * Transforms the pose by the given transformation and returns the new + * transformed pose. + * + *

The matrix multiplication is as follows + * [x_new] [cos, -sin, 0][transform.x] + * [y_new] += [sin, cos, 0][transform.y] + * [t_new] [0, 0, 1][transform.t] + * + * @param other The transform to transform the pose by. + * @return The transformed pose. + */ + public Pose2d plus(Transform2d other) { + return transformBy(other); + } + + /** + * Returns the translation component of the transformation. + * + * @return The translational component of the pose. + */ + public Translation2d getTranslation() { + return m_translation; + } + + /** + * Returns the rotational component of the transformation. + * + * @return The rotational component of the pose. + */ + public Rotation2d getRotation() { + return m_rotation; + } + + /** + * Transforms the pose by the given transformation and returns the new pose. + * See + operator for the matrix multiplication performed. + * + * @param other The transform to transform the pose by. + * @return The transformed pose. + */ + public Pose2d transformBy(Transform2d other) { + return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)), + m_rotation.plus(other.getRotation())); + } + + /** + * Returns the other pose relative to the current pose. + * + *

This function can often be used for trajectory tracking or pose + * stabilization algorithms to get the error between the reference and the + * current pose. + * + * @param other The pose that is the origin of the new coordinate frame that + * the current pose will be converted into. + * @return The current pose relative to the new origin pose. + */ + public Pose2d relativeTo(Pose2d other) { + var transform = new Transform2d(other, this); + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Obtain a new Pose2d from a (constant curvature) velocity. + * + *

See + * Controls Engineering in the FIRST Robotics Competition + * section on nonlinear pose estimation for derivation. + * + *

The twist is a change in pose in the robot's coordinate frame since the + * previous pose update. When the user runs exp() on the previous known + * field-relative pose with the argument being the twist, the user will + * receive the new field-relative pose. + * + *

"Exp" represents the pose exponential, which is solving a differential + * equation moving the pose forward in time. + * + * @param twist The change in pose in the robot's coordinate frame since the + * previous pose update. For example, if a non-holonomic robot moves forward + * 0.01 meters and changes angle by .5 degrees since the previous pose update, + * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)} + * @return The new pose of the robot. + */ + @SuppressWarnings("LocalVariableName") + public Pose2d exp(Twist2d twist) { + double dx = twist.dx; + double dy = twist.dy; + double dtheta = twist.dtheta; + + double sinTheta = Math.sin(dtheta); + double cosTheta = Math.cos(dtheta); + + double s; + double c; + if (Math.abs(dtheta) < 1E-9) { + s = 1.0 - 1.0 / 6.0 * dtheta * dtheta; + c = 0.5 * dtheta; + } else { + s = sinTheta / dtheta; + c = (1 - cosTheta) / dtheta; + } + var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s), + new Rotation2d(cosTheta, sinTheta)); + + return this.plus(transform); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java new file mode 100644 index 0000000000..5bab29d232 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -0,0 +1,170 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +/** + * A rotation in a 2d coordinate frame represented a point on the unit circle + * (cosine and sine). + */ +public class Rotation2d { + private final double m_value; + private final double m_cos; + private final double m_sin; + + /** + * Constructs a Rotation2d with a default angle of 0 degrees. + */ + public Rotation2d() { + m_value = 0.0; + m_cos = 1.0; + m_sin = 0.0; + } + + /** + * Constructs a Rotation2d with the given radian value. + * The x and y don't have to be normalized. + * + * @param value The value of the angle in radians. + */ + public Rotation2d(double value) { + m_value = value; + m_cos = Math.cos(value); + m_sin = Math.sin(value); + } + + /** + * Constructs a Rotation2d with the given x and y (cosine and sine) + * components. + * + * @param x The x component or cosine of the rotation. + * @param y The y component or sine of the rotation. + */ + @SuppressWarnings("ParameterName") + public Rotation2d(double x, double y) { + double magnitude = Math.hypot(x, y); + if (magnitude > 1e-6) { + m_sin = y / magnitude; + m_cos = x / magnitude; + } else { + m_sin = 0.0; + m_cos = 1.0; + } + m_value = Math.atan2(m_sin, m_cos); + } + + /** + * Constructs and returns a Rotation2d with the given degree value. + * + * @param degrees The value of the angle in degrees. + * @return The rotation object with the desired angle value. + */ + public static Rotation2d fromDegrees(double degrees) { + return new Rotation2d(Math.toRadians(degrees)); + } + + /** + * Adds two rotations together, with the result being bounded between -kPi and + * kPi. + * + *

For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = + * Rotation2d{-kPi/2} + * + * @param other The rotation to add. + * @return The sum of the two rotations. + */ + public Rotation2d plus(Rotation2d other) { + return rotateBy(other); + } + + /** + * Subtracts the new rotation from the current rotation and returns the new + * rotation. + * + *

For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = + * Rotation2d{-kPi/2} + * + * @param other The rotation to subtract. + * @return The difference between the two rotations. + */ + public Rotation2d minus(Rotation2d other) { + return rotateBy(other.unaryMinus()); + } + + /** + * Takes the inverse of the current rotation. This is simply the negative of + * the current angular value. + * + * @return The inverse of the current rotation. + */ + public Rotation2d unaryMinus() { + return new Rotation2d(-m_value); + } + + /** + * Adds the new rotation to the current rotation using a rotation matrix. + * + *

The matrix multiplication is as follows: + * [cos_new] [other.cos, -other.sin][cos] + * [sin_new] = [other.sin, other.cos][sin] + * value_new = atan2(cos_new, sin_new) + * + * @param other The rotation to rotate by. + * @return The new rotated Rotation2d. + */ + public Rotation2d rotateBy(Rotation2d other) { + return new Rotation2d( + m_cos * other.m_cos - m_sin * other.m_sin, + m_cos * other.m_sin + m_sin * other.m_cos + ); + } + + /* + * Returns the radian value of the rotation. + * + * @return The radian value of the rotation. + */ + public double getRadians() { + return m_value; + } + + /** + * Returns the degree value of the rotation. + * + * @return The degree value of the rotation. + */ + public double getDegrees() { + return Math.toDegrees(m_value); + } + + /** + * Returns the cosine of the rotation. + * + * @return The cosine of the rotation. + */ + public double getCos() { + return m_cos; + } + + /** + * Returns the sine of the rotation. + * + * @return The sine of the rotation. + */ + public double getSin() { + return m_sin; + } + + /** + * Returns the tangent of the rotation. + * + * @return The tangent of the rotation. + */ + public double getTan() { + return m_sin / m_cos; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java new file mode 100644 index 0000000000..ddb90b040c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +/** + * Represents a transformation for a Pose2d. + */ +public class Transform2d { + private final Translation2d m_translation; + private final Rotation2d m_rotation; + + /** + * Constructs the transform that maps the initial pose to the final pose. + * + * @param initial The initial pose for the transformation. + * @param last The final pose for the transformation. + */ + public Transform2d(Pose2d initial, Pose2d last) { + // We are rotating the difference between the translations + // using a clockwise rotation matrix. This transforms the global + // delta into a local delta (relative to the initial pose). + m_translation = last.getTranslation().minus(initial.getTranslation()) + .rotateBy(initial.getRotation().unaryMinus()); + + m_rotation = last.getRotation().minus(initial.getRotation()); + } + + /** + * Constructs a transform with the given translation and rotation components. + * + * @param translation Translational component of the transform. + * @param rotation Rotational component of the transform. + */ + public Transform2d(Translation2d translation, Rotation2d rotation) { + m_translation = translation; + m_rotation = rotation; + } + + /** + * Constructs the identity transform -- maps an initial pose to itself. + */ + public Transform2d() { + m_translation = new Translation2d(); + m_rotation = new Rotation2d(); + } + + /** + * Returns the translation component of the transformation. + * + * @return The translational component of the transform. + */ + public Translation2d getTranslation() { + return m_translation; + } + + /** + * Returns the rotational component of the transformation. + * + * @return Reference to the rotational component of the transform. + */ + public Rotation2d getRotation() { + return m_rotation; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java new file mode 100644 index 0000000000..fd175c267d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java @@ -0,0 +1,165 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +/** + * Represents a translation in 2d space. + * This object can be used to represent a point or a vector. + * + *

This assumes that you are using conventional mathematical axes. + * When the robot is placed on the origin, facing toward the X direction, + * moving forward increases the X, whereas moving to the left increases the Y. + */ +@SuppressWarnings({"ParameterName", "MemberName"}) +public class Translation2d { + private final double m_x; + private final double m_y; + + /** + * Constructs a Translation2d with X and Y components equal to zero. + */ + public Translation2d() { + this(0.0, 0.0); + } + + /** + * Constructs a Translation2d with the X and Y components equal to the + * provided values. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + */ + public Translation2d(double x, double y) { + m_x = x; + m_y = y; + } + + /** + * Calculates the distance between two translations in 2d space. + * + *

This function uses the pythagorean theorem to calculate the distance. + * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2) + * + * @param other The translation to compute the distance to. + * @return The distance between the two translations. + */ + public double getDistance(Translation2d other) { + return Math.hypot(other.m_x - m_x, other.m_y - m_y); + } + + /** + * Returns the X component of the translation. + * + * @return The x component of the translation. + */ + public double getX() { + return m_x; + } + + /** + * Returns the Y component of the translation. + * + * @return The y component of the translation. + */ + public double getY() { + return m_y; + } + + /** + * Returns the norm, or distance from the origin to the translation. + * + * @return The norm of the translation. + */ + public double getNorm() { + return Math.hypot(m_x, m_y); + } + + /** + * Applies a rotation to the translation in 2d space. + * + *

This multiplies the translation vector by a counterclockwise rotation + * matrix of the given angle. + * [x_new] [other.cos, -other.sin][x] + * [y_new] = [other.sin, other.cos][y] + * + *

For example, rotating a Translation2d of {2, 0} by 90 degrees will return a + * Translation2d of {0, 2}. + * + * @param other The rotation to rotate the translation by. + * @return The new rotated translation. + */ + public Translation2d rotateBy(Rotation2d other) { + return new Translation2d( + m_x * other.getCos() - m_y * other.getSin(), + m_x * other.getSin() + m_y * other.getCos() + ); + } + + /** + * Adds two translations in 2d space and returns the sum. This is similar to + * vector addition. + * + *

For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = + * Translation2d{3.0, 8.0} + * + * @param other The translation to add. + * @return The sum of the translations. + */ + public Translation2d plus(Translation2d other) { + return new Translation2d(m_x + other.m_x, m_y + other.m_y); + } + + /** + * Subtracts the other translation from the other translation and returns the + * difference. + * + *

For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = + * Translation2d{4.0, 2.0} + * + * @param other The translation to subtract. + * @return The difference between the two translations. + */ + public Translation2d minus(Translation2d other) { + return new Translation2d(m_x - other.m_x, m_y - other.m_y); + } + + /** + * Returns the inverse of the current translation. This is equivalent to + * rotating by 180 degrees, flipping the point over both axes, or simply + * negating both components of the translation. + * + * @return The inverse of the current translation. + */ + public Translation2d unaryMinus() { + return new Translation2d(-m_x, -m_y); + } + + /** + * Multiplies the translation by a scalar and returns the new translation. + * + *

For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0} + * + * @param scalar The scalar to multiply by. + * @return The scaled translation. + */ + public Translation2d times(double scalar) { + return new Translation2d(m_x * scalar, m_y * scalar); + } + + /** + * Divides the translation by a scalar and returns the new translation. + * + *

For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25} + * + * @param scalar The scalar to multiply by. + * @return The reference to the new mutated object. + */ + public Translation2d div(double scalar) { + return new Translation2d(m_x / scalar, m_y / scalar); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java new file mode 100644 index 0000000000..65351f2068 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +/** + * A change in distance along arc since the last pose update. We can use ideas + * from differential calculus to create new Pose2ds from a Twist2d and vise + * versa. + * + *

A Twist can be used to represent a difference between two poses. + */ +@SuppressWarnings("MemberName") +public class Twist2d { + /** + * Linear "dx" component. + */ + public double dx; + + /** + * Linear "dy" component. + */ + public double dy; + + /** + * Angular "dtheta" component (radians). + */ + public double dtheta; + + public Twist2d() { + } + + /** + * Constructs a Twist2d with the given values. + * @param dx Change in x direction relative to robot. + * @param dy Change in y direction relative to robot. + * @param dtheta Change in angle relative to robot. + */ + public Twist2d(double dx, double dy, double dtheta) { + this.dx = dx; + this.dy = dy; + this.dtheta = dtheta; + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java new file mode 100644 index 0000000000..21377d004d --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class Pose2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testTransformBy() { + var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); + var transformation = new Transform2d(new Translation2d(5.0, 0.0), + Rotation2d.fromDegrees(5.0)); + + var transformed = initial.plus(transformation); + + assertAll( + () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon), + () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon), + () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon) + ); + } + + @Test + void testRelativeTo() { + var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0)); + var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0)); + + var finalRelativeToInitial = last.relativeTo(initial); + + assertAll( + () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0), + kEpsilon), + () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon), + () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon) + ); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java new file mode 100644 index 0000000000..d1265db8ef --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class Rotation2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testRadiansToDegrees() { + var one = new Rotation2d(Math.PI / 3); + var two = new Rotation2d(Math.PI / 4); + + assertAll( + () -> assertEquals(one.getDegrees(), 60.0, kEpsilon), + () -> assertEquals(two.getDegrees(), 45.0, kEpsilon) + ); + } + + @Test + void testRadiansAndDegrees() { + var one = Rotation2d.fromDegrees(45.0); + var two = Rotation2d.fromDegrees(30.0); + + assertAll( + () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon), + () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon) + ); + } + + @Test + void testRotateByFromZero() { + var zero = new Rotation2d(); + var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0)); + + assertAll( + () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon), + () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon) + ); + } + + @Test + void testRotateByNonZero() { + var rot = Rotation2d.fromDegrees(90.0); + rot = rot.plus(Rotation2d.fromDegrees(30.0)); + + assertEquals(rot.getDegrees(), 120.0, kEpsilon); + } + + @Test + void testMinus() { + var one = Rotation2d.fromDegrees(70.0); + var two = Rotation2d.fromDegrees(30.0); + + assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java new file mode 100644 index 0000000000..a2385f283f --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class Translation2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testSum() { + var one = new Translation2d(1.0, 3.0); + var two = new Translation2d(2.0, 5.0); + + var sum = one.plus(two); + + assertAll( + () -> assertEquals(sum.getX(), 3.0, kEpsilon), + () -> assertEquals(sum.getY(), 8.0, kEpsilon) + ); + } + + @Test + void testDifference() { + var one = new Translation2d(1.0, 3.0); + var two = new Translation2d(2.0, 5.0); + + var difference = one.minus(two); + + assertAll( + () -> assertEquals(difference.getX(), -1.0, kEpsilon), + () -> assertEquals(difference.getY(), -2.0, kEpsilon) + ); + } + + @Test + void testRotateBy() { + var another = new Translation2d(3.0, 0.0); + var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0)); + + assertAll( + () -> assertEquals(rotated.getX(), 0.0, kEpsilon), + () -> assertEquals(rotated.getY(), 3.0, kEpsilon) + ); + } + + @Test + void testMultiplication() { + var original = new Translation2d(3.0, 5.0); + var mult = original.times(3); + + assertAll( + () -> assertEquals(mult.getX(), 9.0, kEpsilon), + () -> assertEquals(mult.getY(), 15.0, kEpsilon) + ); + } + + @Test + void testDivision() { + var original = new Translation2d(3.0, 5.0); + var div = original.div(2); + + assertAll( + () -> assertEquals(div.getX(), 1.5, kEpsilon), + () -> assertEquals(div.getY(), 2.5, kEpsilon) + ); + } + + @Test + void testNorm() { + var one = new Translation2d(3.0, 5.0); + assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon); + } + + @Test + void testDistance() { + var one = new Translation2d(1, 1); + var two = new Translation2d(6, 6); + assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon); + } + + @Test + void testUnaryMinus() { + var original = new Translation2d(-4.5, 7); + var inverted = original.unaryMinus(); + + assertAll( + () -> assertEquals(inverted.getX(), 4.5, kEpsilon), + () -> assertEquals(inverted.getY(), -7, kEpsilon) + ); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java new file mode 100644 index 0000000000..59a20aae54 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.geometry; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class Twist2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testStraightLineTwist() { + var straight = new Twist2d(5.0, 0.0, 0.0); + var straightPose = new Pose2d().exp(straight); + + assertAll( + () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon), + () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon), + () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon) + ); + } + + @Test + void testQuarterCirleTwist() { + var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0); + var quarterCirclePose = new Pose2d().exp(quarterCircle); + + assertAll( + () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon), + () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon), + () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon) + ); + } + + @Test + void testDiagonalNoDtheta() { + var diagonal = new Twist2d(2.0, 2.0, 0.0); + var diagonalPose = new Pose2d().exp(diagonal); + + assertAll( + () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon), + () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon), + () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon) + ); + } +}