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 f64eff41a0..fb4d7867ef 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 @@ -11,17 +11,14 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; -/** Represents a 2d pose containing translational and rotational elements. */ +/** Represents a 2D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Pose2d implements Interpolatable { 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}) - */ + /** Constructs a pose at the origin facing toward the positive X axis. */ public Pose2d() { m_translation = new Translation2d(); m_rotation = new Rotation2d(); @@ -42,8 +39,7 @@ public class Pose2d implements Interpolatable { } /** - * Convenience constructors that takes in x and y values directly instead of having to construct a - * Translation2d. + * Constructs a pose with x and y translations instead of a separate 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. @@ -57,8 +53,11 @@ public class Pose2d implements Interpolatable { /** * 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] + * 

+   * [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. @@ -160,8 +159,8 @@ public class Pose2d implements Interpolatable { * * @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 0.5 - * degrees since the previous pose update, the twist would be Twist2d{0.01, 0.0, - * toRadians(0.5)} + * degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0, + * Units.degreesToRadians(0.5)). * @return The new pose of the robot. */ public Pose2d exp(Twist2d twist) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java new file mode 100644 index 0000000000..2be3dbfd3a --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -0,0 +1,326 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N3; +import java.util.Objects; + +/** Represents a 3D pose containing translational and rotational elements. */ +public class Pose3d implements Interpolatable { + private final Translation3d m_translation; + private final Rotation3d m_rotation; + + /** Constructs a pose at the origin facing toward the positive X axis. */ + public Pose3d() { + m_translation = new Translation3d(); + m_rotation = new Rotation3d(); + } + + /** + * 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 Pose3d(Translation3d translation, Rotation3d rotation) { + m_translation = translation; + m_rotation = rotation; + } + + /** + * Constructs a pose with x, y, and z translations instead of a separate Translation3d. + * + * @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 z The z component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + public Pose3d(double x, double y, double z, Rotation3d rotation) { + m_translation = new Translation3d(x, y, z); + m_rotation = rotation; + } + + /** + * Transforms the pose by the given transformation and returns the new transformed pose. + * + * @param other The transform to transform the pose by. + * @return The transformed pose. + */ + public Pose3d plus(Transform3d other) { + return transformBy(other); + } + + /** + * Returns the Transform3d that maps the one pose to another. + * + * @param other The initial pose of the transformation. + * @return The transform that maps the other pose to the current pose. + */ + public Transform3d minus(Pose3d other) { + final var pose = this.relativeTo(other); + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Returns the translation component of the transformation. + * + * @return The translational component of the pose. + */ + public Translation3d getTranslation() { + return m_translation; + } + + /** + * Returns the X component of the pose's translation. + * + * @return The x component of the pose's translation. + */ + public double getX() { + return m_translation.getX(); + } + + /** + * Returns the Y component of the pose's translation. + * + * @return The y component of the pose's translation. + */ + public double getY() { + return m_translation.getY(); + } + + /** + * Returns the Z component of the pose's translation. + * + * @return The z component of the pose's translation. + */ + public double getZ() { + return m_translation.getZ(); + } + + /** + * Returns the rotational component of the transformation. + * + * @return The rotational component of the pose. + */ + public Rotation3d 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 Pose3d transformBy(Transform3d other) { + return new Pose3d( + 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 Pose3d relativeTo(Pose3d other) { + var transform = new Transform3d(other, this); + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Obtain a new Pose3d from a (constant curvature) velocity. + * + *

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 0.5 + * degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new + * Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))). + * @return The new pose of the robot. + */ + @SuppressWarnings("LocalVariableName") + public Pose3d exp(Twist3d twist) { + final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz)); + final var OmegaSq = Omega.times(Omega); + + double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz; + + // Get left Jacobian of SO3. See first line in right column of + // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf + Matrix J; + if (thetaSq < 1E-9 * 1E-9) { + // J = I + 0.5ω + J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5)); + } else { + double theta = Math.sqrt(thetaSq); + // J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω² + J = + Matrix.eye(Nat.N3()) + .plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq)) + .plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta))); + } + + // Get translation component + final var translation = + J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz)); + + final var transform = + new Transform3d( + new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)), + new Rotation3d(twist.rx, twist.ry, twist.rz)); + + return this.plus(transform); + } + + /** + * Returns a Twist3d that maps this pose to the end pose. If c is the output of a.Log(b), then + * a.Exp(c) would yield b. + * + * @param end The end pose for the transformation. + * @return The twist that maps this to end. + */ + @SuppressWarnings("LocalVariableName") + public Twist3d log(Pose3d end) { + final var transform = end.relativeTo(this); + + final var rotVec = transform.getRotation().getQuaternion().toRotationVector(); + + final var Omega = rotationVectorToMatrix(rotVec); + final var OmegaSq = Omega.times(Omega); + + double thetaSq = + rotVec.get(0, 0) * rotVec.get(0, 0) + + rotVec.get(1, 0) * rotVec.get(1, 0) + + rotVec.get(2, 0) * rotVec.get(2, 0); + + // Get left Jacobian inverse of SO3. See fourth line in right column of + // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf + Matrix Jinv; + if (thetaSq < 1E-9 * 1E-9) { + // J⁻¹ = I − 0.5ω + 1/12 ω² + Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0)); + } else { + double theta = Math.sqrt(thetaSq); + double halfTheta = 0.5 * theta; + + // J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω² + Jinv = + Matrix.eye(Nat.N3()) + .minus(Omega.times(0.5)) + .plus( + OmegaSq.times( + (1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq)); + } + + // Get dtranslation component + final var dtranslation = + Jinv.times( + new MatBuilder<>(Nat.N3(), Nat.N1()) + .fill(transform.getX(), transform.getY(), transform.getZ())); + + return new Twist3d( + dtranslation.get(0, 0), + dtranslation.get(1, 0), + dtranslation.get(2, 0), + rotVec.get(0, 0), + rotVec.get(1, 0), + rotVec.get(2, 0)); + } + + /** + * Returns a Pose2d representing this Pose3d projected into the X-Y plane. + * + * @return A Pose2d representing this Pose3d projected into the X-Y plane. + */ + public Pose2d toPose2d() { + return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d()); + } + + @Override + public String toString() { + return String.format("Pose3d(%s, %s)", m_translation, m_rotation); + } + + /** + * Checks equality between this Pose3d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Pose3d) { + return ((Pose3d) obj).m_translation.equals(m_translation) + && ((Pose3d) obj).m_rotation.equals(m_rotation); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_translation, m_rotation); + } + + @Override + @SuppressWarnings("ParameterName") + public Pose3d interpolate(Pose3d endValue, double t) { + if (t < 0) { + return this; + } else if (t >= 1) { + return endValue; + } else { + var twist = this.log(endValue); + var scaledTwist = + new Twist3d( + twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t); + return this.exp(scaledTwist); + } + } + + /** + * Applies the hat operator to a rotation vector. + * + *

It takes a rotation vector and returns the corresponding matrix representation of the Lie + * algebra element (a 3x3 rotation matrix). + * + * @param rotation The rotation vector. + * @return The rotation vector as a 3x3 rotation matrix. + */ + private Matrix rotationVectorToMatrix(Vector rotation) { + // Given a rotation vector , + // [ 0 -c b] + // Omega = [ c 0 -a] + // [-b a 0] + return new MatBuilder<>(Nat.N3(), Nat.N3()) + .fill( + 0.0, + -rotation.get(2, 0), + rotation.get(1, 0), + rotation.get(2, 0), + 0.0, + -rotation.get(0, 0), + -rotation.get(1, 0), + rotation.get(0, 0), + 0.0); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java new file mode 100644 index 0000000000..c38bc8a2e7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -0,0 +1,187 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; +import java.util.Objects; + +public class Quaternion { + private final double m_r; + private final Vector m_v; + + /** Constructs a quaternion with a default angle of 0 degrees. */ + public Quaternion() { + m_r = 1.0; + m_v = VecBuilder.fill(0.0, 0.0, 0.0); + } + + /** + * Constructs a quaternion with the given components. + * + * @param w W component of the quaternion. + * @param x X component of the quaternion. + * @param y Y component of the quaternion. + * @param z Z component of the quaternion. + */ + public Quaternion(double w, double x, double y, double z) { + m_r = w; + m_v = VecBuilder.fill(x, y, z); + } + + /** + * Multiply with another quaternion. + * + * @param other The other quaternion. + * @return The quaternion product. + */ + public Quaternion times(Quaternion other) { + // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts + final var r1 = m_r; + final var v1 = m_v; + final var r2 = other.m_r; + final var v2 = other.m_v; + + final var v1x = v1.get(0, 0); + final var v1y = v1.get(1, 0); + final var v1z = v1.get(2, 0); + + final var v2x = v2.get(0, 0); + final var v2y = v2.get(1, 0); + final var v2z = v2.get(2, 0); + + var cross = + VecBuilder.fill(v1y * v2z - v2y * v1z, v2x * v1z - v1x * v2z, v1x * v2y - v2x * v1y); + double dot = v1x * v2x + v1y * v2y + v1z * v2z; + + final var v = v2.times(r1).plus(v1.times(r2)).plus(cross); + return new Quaternion(r1 * r2 - dot, v.get(0, 0), v.get(1, 0), v.get(2, 0)); + } + + @Override + public String toString() { + return String.format( + "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0)); + } + + /** + * Checks equality between this Quaternion and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Quaternion) { + var other = (Quaternion) obj; + + final var r1 = m_r; + final var v1 = m_v; + final var r2 = other.m_r; + final var v2 = other.m_v; + + final var v1x = v1.get(0, 0); + final var v1y = v1.get(1, 0); + final var v1z = v1.get(2, 0); + + final var v2x = v2.get(0, 0); + final var v2y = v2.get(1, 0); + final var v2z = v2.get(2, 0); + + return Math.abs(r1 * r2 + v1x * v2x + v1y * v2y + v1z * v2z) > 1.0 - 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_r, m_v); + } + + /** + * Returns the inverse of the quaternion. + * + * @return The inverse quaternion. + */ + public Quaternion inverse() { + return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0)); + } + + /** + * Normalizes the quaternion. + * + * @return The normalized quaternion. + */ + public Quaternion normalize() { + double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ()); + if (norm == 0.0) { + return new Quaternion(); + } else { + return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm); + } + } + + /** + * Returns W component of the quaternion. + * + * @return W component of the quaternion. + */ + public double getW() { + return m_r; + } + + /** + * Returns X component of the quaternion. + * + * @return X component of the quaternion. + */ + public double getX() { + return m_v.get(0, 0); + } + + /** + * Returns Y component of the quaternion. + * + * @return Y component of the quaternion. + */ + public double getY() { + return m_v.get(1, 0); + } + + /** + * Returns Z component of the quaternion. + * + * @return Z component of the quaternion. + */ + public double getZ() { + return m_v.get(2, 0); + } + + /** + * Returns the rotation vector representation of this quaternion. + * + *

This is also the log operator of SO(3). + * + * @return The rotation vector representation of this quaternion. + */ + public Vector toRotationVector() { + // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with + // Sound State Representation through Encapsulation of Manifolds" + // + // https://arxiv.org/pdf/1107.1119.pdf + double norm = m_v.normF(); + + if (norm < 1e-9) { + return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW())); + } else { + if (getW() < 0.0) { + return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm); + } else { + return m_v.times(2.0 * Math.atan2(norm, getW()) / norm); + } + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 5f37740e44..6daab9d8a7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -13,7 +13,7 @@ import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.util.Units; import java.util.Objects; -/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */ +/** A rotation in a 2D coordinate frame represented a point on the unit circle (cosine and sine). */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation2d implements Interpolatable { @@ -29,7 +29,7 @@ public class Rotation2d implements Interpolatable { } /** - * Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized. + * Constructs a Rotation2d with the given radian value. * * @param value The value of the angle in radians. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java new file mode 100644 index 0000000000..73b3e9ee7f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -0,0 +1,259 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N3; +import java.util.Objects; + +/** A rotation in a 3D coordinate. */ +public class Rotation3d implements Interpolatable { + private Quaternion m_q = new Quaternion(); + + /** Constructs a Rotation3d with a default angle of 0 degrees. */ + public Rotation3d() {} + + /** + * Constructs a Rotation3d from a quaternion. + * + * @param q The quaternion. + */ + public Rotation3d(Quaternion q) { + m_q = q.normalize(); + } + + /** + * Constructs a Rotation3d from extrinsic roll, pitch, and yaw. + * + *

Extrinsic rotations occur in that order around the axes in the fixed global frame rather + * than the body frame. + * + * @param roll The counterclockwise rotation angle around the X axis (roll) in radians. + * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians. + * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians. + */ + public Rotation3d(double roll, double pitch, double yaw) { + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion + double cr = Math.cos(roll * 0.5); + double sr = Math.sin(roll * 0.5); + + double cp = Math.cos(pitch * 0.5); + double sp = Math.sin(pitch * 0.5); + + double cy = Math.cos(yaw * 0.5); + double sy = Math.sin(yaw * 0.5); + + m_q = + new Quaternion( + cr * cp * cy + sr * sp * sy, + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy); + } + + /** + * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be + * normalized. + * + * @param axis The rotation axis. + * @param angleRadians The rotation around the axis in radians. + */ + public Rotation3d(Vector axis, double angleRadians) { + double norm = axis.normF(); + if (norm == 0.0) { + return; + } + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition + var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0)); + m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0)); + } + + /** + * Adds two rotations together. + * + * @param other The rotation to add. + * @return The sum of the two rotations. + */ + public Rotation3d plus(Rotation3d other) { + return rotateBy(other); + } + + /** + * Subtracts the new rotation from the current rotation and returns the new rotation. + * + * @param other The rotation to subtract. + * @return The difference between the two rotations. + */ + public Rotation3d minus(Rotation3d other) { + return rotateBy(other.unaryMinus()); + } + + /** + * Takes the inverse of the current rotation. + * + * @return The inverse of the current rotation. + */ + public Rotation3d unaryMinus() { + return new Rotation3d(m_q.inverse()); + } + + /** + * Multiplies the current rotation by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Rotation3d. + */ + public Rotation3d times(double scalar) { + // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp + if (m_q.getW() >= 0.0) { + return new Rotation3d( + VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()), + 2.0 * scalar * Math.acos(m_q.getW())); + } else { + return new Rotation3d( + VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()), + 2.0 * scalar * Math.acos(-m_q.getW())); + } + } + + /** + * Adds the new rotation to the current rotation. + * + * @param other The rotation to rotate by. + * @return The new rotated Rotation3d. + */ + public Rotation3d rotateBy(Rotation3d other) { + return new Rotation3d(other.m_q.times(m_q)); + } + + /** + * Returns the quaternion representation of the Rotation3d. + * + * @return The quaternion representation of the Rotation3d. + */ + public Quaternion getQuaternion() { + return m_q; + } + + /** + * Returns the counterclockwise rotation angle around the X axis (roll) in radians. + * + * @return The counterclockwise rotation angle around the X axis (roll) in radians. + */ + public double getX() { + final var w = m_q.getW(); + final var x = m_q.getX(); + final var y = m_q.getY(); + final var z = m_q.getZ(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion + return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y)); + } + + /** + * Returns the counterclockwise rotation angle around the Y axis (pitch) in radians. + * + * @return The counterclockwise rotation angle around the Y axis (pitch) in radians. + */ + public double getY() { + final var w = m_q.getW(); + final var x = m_q.getX(); + final var y = m_q.getY(); + final var z = m_q.getZ(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion + double ratio = 2.0 * (w * y - z * x); + if (Math.abs(ratio) >= 1.0) { + return Math.copySign(Math.PI / 2.0, ratio); + } else { + return Math.asin(ratio); + } + } + + /** + * Returns the counterclockwise rotation angle around the Z axis (yaw) in radians. + * + * @return The counterclockwise rotation angle around the Z axis (yaw) in radians. + */ + public double getZ() { + final var w = m_q.getW(); + final var x = m_q.getX(); + final var y = m_q.getY(); + final var z = m_q.getZ(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion + return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + } + + /** + * Returns the axis in the axis-angle representation of this rotation. + * + * @return The axis in the axis-angle representation. + */ + public Vector getAxis() { + double norm = + Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ()); + if (norm == 0.0) { + return VecBuilder.fill(0.0, 0.0, 0.0); + } else { + return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm); + } + } + + /** + * Returns the angle in radians in the axis-angle representation of this rotation. + * + * @return The angle in radians in the axis-angle representation of this rotation. + */ + public double getAngle() { + double norm = + Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ()); + return 2.0 * Math.atan2(norm, m_q.getW()); + } + + /** + * Returns a Rotation2d representing this Rotation3d projected into the X-Y plane. + * + * @return A Rotation2d representing this Rotation3d projected into the X-Y plane. + */ + public Rotation2d toRotation2d() { + return new Rotation2d(getZ()); + } + + @Override + public String toString() { + return String.format("Rotation3d(%s)", m_q); + } + + /** + * Checks equality between this Rotation3d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Rotation3d) { + var other = (Rotation3d) obj; + return m_q.equals(other.m_q); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_q); + } + + @Override + @SuppressWarnings("ParameterName") + public Rotation3d interpolate(Rotation3d endValue, double t) { + return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java new file mode 100644 index 0000000000..a80fbb2171 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -0,0 +1,152 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import java.util.Objects; + +/** Represents a transformation for a Pose3d. */ +public class Transform3d { + private final Translation3d m_translation; + private final Rotation3d 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 Transform3d(Pose3d initial, Pose3d 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 Transform3d(Translation3d translation, Rotation3d rotation) { + m_translation = translation; + m_rotation = rotation; + } + + /** Constructs the identity transform -- maps an initial pose to itself. */ + public Transform3d() { + m_translation = new Translation3d(); + m_rotation = new Rotation3d(); + } + + /** + * Scales the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform3d. + */ + public Transform3d times(double scalar) { + return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar)); + } + + /** + * Composes two transformations. + * + * @param other The transform to compose with this one. + * @return The composition of the two transformations. + */ + public Transform3d plus(Transform3d other) { + return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other)); + } + + /** + * Returns the translation component of the transformation. + * + * @return The translational component of the transform. + */ + public Translation3d getTranslation() { + return m_translation; + } + + /** + * Returns the X component of the transformation's translation. + * + * @return The x component of the transformation's translation. + */ + public double getX() { + return m_translation.getX(); + } + + /** + * Returns the Y component of the transformation's translation. + * + * @return The y component of the transformation's translation. + */ + public double getY() { + return m_translation.getY(); + } + + /** + * Returns the Z component of the transformation's translation. + * + * @return The z component of the transformation's translation. + */ + public double getZ() { + return m_translation.getZ(); + } + + /** + * Returns the rotational component of the transformation. + * + * @return Reference to the rotational component of the transform. + */ + public Rotation3d getRotation() { + return m_rotation; + } + + /** + * Invert the transformation. This is useful for undoing a transformation. + * + * @return The inverted transformation. + */ + public Transform3d inverse() { + // 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). + return new Transform3d( + getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()), + getRotation().unaryMinus()); + } + + @Override + public String toString() { + return String.format("Transform3d(%s, %s)", m_translation, m_rotation); + } + + /** + * Checks equality between this Transform3d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Transform3d) { + return ((Transform3d) obj).m_translation.equals(m_translation) + && ((Transform3d) obj).m_rotation.equals(m_rotation); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(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 84eaea8ccb..dcdfb06ab4 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 @@ -13,11 +13,10 @@ import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; /** - * Represents a translation in 2d space. This object can be used to represent a point or a vector. + * 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. + *

This assumes that you are using conventional mathematical axes. When the robot is at the + * origin facing in the positive X direction, forward is positive X and left is positive Y. */ @SuppressWarnings({"ParameterName", "MemberName"}) @JsonIgnoreProperties(ignoreUnknown = true) @@ -58,10 +57,9 @@ public class Translation2d implements Interpolatable { } /** - * Calculates the distance between two translations in 2d space. + * 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) + *

The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²). * * @param other The translation to compute the distance to. * @return The distance between the two translations. @@ -73,7 +71,7 @@ public class Translation2d implements Interpolatable { /** * Returns the X component of the translation. * - * @return The x component of the translation. + * @return The X component of the translation. */ @JsonProperty public double getX() { @@ -83,7 +81,7 @@ public class Translation2d implements Interpolatable { /** * Returns the Y component of the translation. * - * @return The y component of the translation. + * @return The Y component of the translation. */ @JsonProperty public double getY() { @@ -100,13 +98,18 @@ public class Translation2d implements Interpolatable { } /** - * Applies a rotation to the translation in 2d space. + * 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] + * angle. * - *

For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of - * {0, 2}. + * 

+   * [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. @@ -117,9 +120,9 @@ public class Translation2d implements Interpolatable { } /** - * Adds two translations in 2d space and returns the sum. This is similar to vector addition. + * Returns the sum of two translations in 2D space. * - *

For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0} + *

For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0). * * @param other The translation to add. * @return The sum of the translations. @@ -129,9 +132,9 @@ public class Translation2d implements Interpolatable { } /** - * Subtracts the other translation from the other translation and returns the difference. + * Returns the difference between two translations. * - *

For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0} + *

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. @@ -142,7 +145,7 @@ public class Translation2d implements Interpolatable { /** * 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. + * flipping the point over both axes, or negating all components of the translation. * * @return The inverse of the current translation. */ @@ -151,9 +154,9 @@ public class Translation2d implements Interpolatable { } /** - * Multiplies the translation by a scalar and returns the new translation. + * Returns the translation multiplied by a scalar. * - *

For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0} + *

For example, Translation2d(2.0, 2.5) * 2 = Translation2d(4.0, 5.0). * * @param scalar The scalar to multiply by. * @return The scaled translation. @@ -163,9 +166,9 @@ public class Translation2d implements Interpolatable { } /** - * Divides the translation by a scalar and returns the new translation. + * Returns the translation divided by a scalar. * - *

For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25} + *

For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25). * * @param scalar The scalar to multiply by. * @return The reference to the new mutated object. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java new file mode 100644 index 0000000000..f8f8957b12 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -0,0 +1,222 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; +import java.util.Objects; + +/** + * Represents a translation in 3D 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 at the + * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is + * positive Z. + */ +@SuppressWarnings({"ParameterName", "MemberName"}) +public class Translation3d implements Interpolatable { + private final double m_x; + private final double m_y; + private final double m_z; + + /** Constructs a Translation3d with X, Y, and Z components equal to zero. */ + public Translation3d() { + this(0.0, 0.0, 0.0); + } + + /** + * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + * @param z The z component of the translation. + */ + public Translation3d(double x, double y, double z) { + m_x = x; + m_y = y; + m_z = z; + } + + /** + * Constructs a Translation3d with the provided distance and angle. This is essentially converting + * from polar coordinates to Cartesian coordinates. + * + * @param distance The distance from the origin to the end of the translation. + * @param angle The angle between the x-axis and the translation vector. + */ + public Translation3d(double distance, Rotation3d angle) { + final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle); + m_x = rectangular.getX(); + m_y = rectangular.getY(); + m_z = rectangular.getZ(); + } + + /** + * Calculates the distance between two translations in 3D space. + * + *

The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²). + * + * @param other The translation to compute the distance to. + * @return The distance between the two translations. + */ + public double getDistance(Translation3d other) { + return Math.sqrt( + Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2)); + } + + /** + * 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 Z component of the translation. + * + * @return The Z component of the translation. + */ + public double getZ() { + return m_z; + } + + /** + * Returns the norm, or distance from the origin to the translation. + * + * @return The norm of the translation. + */ + public double getNorm() { + return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z); + } + + /** + * Applies a rotation to the translation in 3D space. + * + *

For example, rotating a Translation3d of <2, 0, 0> by 90 degrees around the Z axis + * will return a Translation3d of <0, 2, 0>. + * + * @param other The rotation to rotate the translation by. + * @return The new rotated translation. + */ + public Translation3d rotateBy(Rotation3d other) { + final var p = new Quaternion(0.0, m_x, m_y, m_z); + final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse()); + return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ()); + } + + /** + * Returns a Translation2d representing this Translation3d projected into the X-Y plane. + * + * @return A Translation2d representing this Translation3d projected into the X-Y plane. + */ + public Translation2d toTranslation2d() { + return new Translation2d(m_x, m_y); + } + + /** + * Returns the sum of two translations in 3D space. + * + *

For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) = + * Translation3d{3.0, 8.0, 11.0). + * + * @param other The translation to add. + * @return The sum of the translations. + */ + public Translation3d plus(Translation3d other) { + return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z); + } + + /** + * Returns the difference between two translations. + * + *

For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) = + * Translation3d(4.0, 2.0, 0.0). + * + * @param other The translation to subtract. + * @return The difference between the two translations. + */ + public Translation3d minus(Translation3d other) { + return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z); + } + + /** + * Returns the inverse of the current translation. This is equivalent to negating all components + * of the translation. + * + * @return The inverse of the current translation. + */ + public Translation3d unaryMinus() { + return new Translation3d(-m_x, -m_y, -m_z); + } + + /** + * Returns the translation multiplied by a scalar. + * + *

For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0). + * + * @param scalar The scalar to multiply by. + * @return The scaled translation. + */ + public Translation3d times(double scalar) { + return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar); + } + + /** + * Returns the translation divided by a scalar. + * + *

For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25). + * + * @param scalar The scalar to multiply by. + * @return The reference to the new mutated object. + */ + public Translation3d div(double scalar) { + return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar); + } + + @Override + public String toString() { + return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z); + } + + /** + * Checks equality between this Translation3d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Translation3d) { + return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9 + && Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9 + && Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_x, m_y, m_z); + } + + @Override + public Translation3d interpolate(Translation3d endValue, double t) { + return new Translation3d( + MathUtil.interpolate(this.getX(), endValue.getX(), t), + MathUtil.interpolate(this.getY(), endValue.getY(), t), + MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java index c73d23629a..87ac108985 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java @@ -7,8 +7,8 @@ package edu.wpi.first.math.geometry; import java.util.Objects; /** - * 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 change in distance along a 2D 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. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java new file mode 100644 index 0000000000..3845abbb42 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import java.util.Objects; + +/** + * A change in distance along a 3D arc since the last pose update. We can use ideas from + * differential calculus to create new Pose3ds from a Twist3d and vise versa. + * + *

A Twist can be used to represent a difference between two poses. + */ +@SuppressWarnings("MemberName") +public class Twist3d { + /** Linear "dx" component. */ + public double dx; + + /** Linear "dy" component. */ + public double dy; + + /** Linear "dz" component. */ + public double dz; + + /** Rotation vector x component (radians). */ + public double rx; + + /** Rotation vector y component (radians). */ + public double ry; + + /** Rotation vector z component (radians). */ + public double rz; + + public Twist3d() {} + + /** + * Constructs a Twist3d with the given values. + * + * @param dx Change in x direction relative to robot. + * @param dy Change in y direction relative to robot. + * @param dz Change in z direction relative to robot. + * @param rx Rotation vector x component. + * @param ry Rotation vector y component. + * @param rz Rotation vector z component. + */ + public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) { + this.dx = dx; + this.dy = dy; + this.dz = dz; + this.rx = rx; + this.ry = ry; + this.rz = rz; + } + + @Override + public String toString() { + return String.format( + "Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)", + dx, dy, dz, rx, ry, rz); + } + + /** + * Checks equality between this Twist3d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Twist3d) { + return Math.abs(((Twist3d) obj).dx - dx) < 1E-9 + && Math.abs(((Twist3d) obj).dy - dy) < 1E-9 + && Math.abs(((Twist3d) obj).dz - dz) < 1E-9 + && Math.abs(((Twist3d) obj).rx - rx) < 1E-9 + && Math.abs(((Twist3d) obj).ry - ry) < 1E-9 + && Math.abs(((Twist3d) obj).rz - rz) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(dx, dy, dz, rx, ry, rz); + } +} diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index b7176cd4a4..1bf9829ea1 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -11,10 +11,10 @@ using namespace frc; Pose2d::Pose2d(Translation2d translation, Rotation2d rotation) - : m_translation(translation), m_rotation(rotation) {} + : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation) - : m_translation(x, y), m_rotation(rotation) {} + : m_translation(x, y), m_rotation(std::move(rotation)) {} Pose2d Pose2d::operator+(const Transform2d& other) const { return TransformBy(other); diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp new file mode 100644 index 0000000000..d532908b97 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -0,0 +1,139 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Pose3d.h" + +#include + +using namespace frc; + +namespace { + +/** + * Applies the hat operator to a rotation vector. + * + * It takes a rotation vector and returns the corresponding matrix + * representation of the Lie algebra element (a 3x3 rotation matrix). + * + * @param rotation The rotation vector. + * @return The rotation vector as a 3x3 rotation matrix. + */ +Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) { + // Given a rotation vector , + // [ 0 -c b] + // Omega = [ c 0 -a] + // [-b a 0] + return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)}, + {rotation(2), 0.0, -rotation(0)}, + {-rotation(1), rotation(0), 0.0}}; +} +} // namespace + +Pose3d::Pose3d(Translation3d translation, Rotation3d rotation) + : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} + +Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, + Rotation3d rotation) + : m_translation(x, y, z), m_rotation(std::move(rotation)) {} + +Pose3d Pose3d::operator+(const Transform3d& other) const { + return TransformBy(other); +} + +Transform3d Pose3d::operator-(const Pose3d& other) const { + const auto pose = this->RelativeTo(other); + return Transform3d(pose.Translation(), pose.Rotation()); +} + +bool Pose3d::operator==(const Pose3d& other) const { + return m_translation == other.m_translation && m_rotation == other.m_rotation; +} + +bool Pose3d::operator!=(const Pose3d& other) const { + return !operator==(other); +} + +Pose3d Pose3d::TransformBy(const Transform3d& other) const { + return {m_translation + (other.Translation().RotateBy(m_rotation)), + m_rotation + other.Rotation()}; +} + +Pose3d Pose3d::RelativeTo(const Pose3d& other) const { + const Transform3d transform{other, *this}; + return {transform.Translation(), transform.Rotation()}; +} + +Pose3d Pose3d::Exp(const Twist3d& twist) const { + Matrixd<3, 3> Omega = RotationVectorToMatrix( + Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()}); + Matrixd<3, 3> OmegaSq = Omega * Omega; + + double thetaSq = + (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value(); + + // Get left Jacobian of SO3. See first line in right column of + // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf + Matrixd<3, 3> J; + if (thetaSq < 1E-9 * 1E-9) { + // V = I + 0.5ω + J = Matrixd<3, 3>::Identity() + 0.5 * Omega; + } else { + double theta = std::sqrt(thetaSq); + // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω² + J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega + + (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq; + } + + // Get translation component + Vectord<3> translation = + J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()}; + + const Transform3d transform{Translation3d{units::meter_t{translation(0)}, + units::meter_t{translation(1)}, + units::meter_t{translation(2)}}, + Rotation3d{twist.rx, twist.ry, twist.rz}}; + + return *this + transform; +} + +Twist3d Pose3d::Log(const Pose3d& end) const { + const auto transform = end.RelativeTo(*this); + + Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector(); + + Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec); + Matrixd<3, 3> OmegaSq = Omega * Omega; + + double thetaSq = rotVec.squaredNorm(); + + // Get left Jacobian inverse of SO3. See fourth line in right column of + // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf + Matrixd<3, 3> Jinv; + if (thetaSq < 1E-9 * 1E-9) { + // J⁻¹ = I − 0.5ω + 1/12 ω² + Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq; + } else { + double theta = std::sqrt(thetaSq); + double halfTheta = 0.5 * theta; + + // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω² + Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + + (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) / + thetaSq * OmegaSq; + } + + // Get dtranslation component + Vectord<3> dtranslation = + Jinv * Vectord<3>{transform.X().value(), transform.Y().value(), + transform.Z().value()}; + + return Twist3d{ + units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)}, + units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)}, + units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}}; +} + +Pose2d Pose3d::ToPose2d() const { + return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()}; +} diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp new file mode 100644 index 0000000000..0f44e58ba2 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -0,0 +1,80 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Quaternion.h" + +using namespace frc; + +Quaternion::Quaternion(double w, double x, double y, double z) + : m_r{w}, m_v{x, y, z} {} + +Quaternion Quaternion::operator*(const Quaternion& other) const { + // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts + const auto& r1 = m_r; + const auto& v1 = m_v; + const auto& r2 = other.m_r; + const auto& v2 = other.m_v; + + Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2), + v2(0) * v1(2) - v1(0) * v2(2), + v1(0) * v2(1) - v2(0) * v1(1)}; + + Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross; + return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)}; +} + +bool Quaternion::operator==(const Quaternion& other) const { + return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9; +} + +bool Quaternion::operator!=(const Quaternion& other) const { + return !operator==(other); +} + +Quaternion Quaternion::Inverse() const { + return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)}; +} + +Quaternion Quaternion::Normalize() const { + double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z()); + if (norm == 0.0) { + return Quaternion{}; + } else { + return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm}; + } +} + +double Quaternion::W() const { + return m_r; +} + +double Quaternion::X() const { + return m_v(0); +} + +double Quaternion::Y() const { + return m_v(1); +} + +double Quaternion::Z() const { + return m_v(2); +} + +Eigen::Vector3d Quaternion::ToRotationVector() const { + // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with + // Sound State Representation through Encapsulation of Manifolds" + // + // https://arxiv.org/pdf/1107.1119.pdf + double norm = m_v.norm(); + + if (norm < 1e-9) { + return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v; + } else { + if (W() < 0.0) { + return 2.0 * std::atan2(-norm, -W()) / norm * m_v; + } else { + return 2.0 * std::atan2(norm, W()) / norm * m_v; + } + } +} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp new file mode 100644 index 0000000000..f684b33dff --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -0,0 +1,138 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Rotation3d.h" + +#include + +#include + +#include "units/math.h" + +using namespace frc; + +Rotation3d::Rotation3d(const Quaternion& q) { + m_q = q.Normalize(); +} + +Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch, + units::radian_t yaw) { + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion + double cr = units::math::cos(roll * 0.5); + double sr = units::math::sin(roll * 0.5); + + double cp = units::math::cos(pitch * 0.5); + double sp = units::math::sin(pitch * 0.5); + + double cy = units::math::cos(yaw * 0.5); + double sy = units::math::sin(yaw * 0.5); + + m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy}; +} + +Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) { + double norm = axis.norm(); + if (norm == 0.0) { + return; + } + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition + Vectord<3> v = axis / norm * units::math::sin(angle / 2.0); + m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)}; +} + +Rotation3d Rotation3d::operator+(const Rotation3d& other) const { + return RotateBy(other); +} + +Rotation3d Rotation3d::operator-(const Rotation3d& other) const { + return *this + -other; +} + +Rotation3d Rotation3d::operator-() const { + return Rotation3d{m_q.Inverse()}; +} + +Rotation3d Rotation3d::operator*(double scalar) const { + // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp + if (m_q.W() >= 0.0) { + return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()}, + 2.0 * units::radian_t{scalar * std::acos(m_q.W())}}; + } else { + return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}, + 2.0 * units::radian_t{scalar * std::acos(-m_q.W())}}; + } +} + +bool Rotation3d::operator==(const Rotation3d& other) const { + return m_q == other.m_q; +} + +bool Rotation3d::operator!=(const Rotation3d& other) const { + return !operator==(other); +} + +Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const { + return Rotation3d{other.m_q * m_q}; +} + +const Quaternion& Rotation3d::GetQuaternion() const { + return m_q; +} + +units::radian_t Rotation3d::X() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion + return units::radian_t{ + std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))}; +} + +units::radian_t Rotation3d::Y() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion + double ratio = 2.0 * (w * y - z * x); + if (std::abs(ratio) >= 1.0) { + return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)}; + } else { + return units::radian_t{std::asin(ratio)}; + } +} + +units::radian_t Rotation3d::Z() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion + return units::radian_t{ + std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))}; +} + +Vectord<3> Rotation3d::Axis() const { + double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z()); + if (norm == 0.0) { + return {0.0, 0.0, 0.0}; + } else { + return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}; + } +} + +units::radian_t Rotation3d::Angle() const { + double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z()); + return units::radian_t{2.0 * std::atan2(norm, m_q.W())}; +} + +Rotation2d Rotation3d::ToRotation2d() const { + return Rotation2d{Z()}; +} diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp index 0808f351fa..43c3488175 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp @@ -19,7 +19,7 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) { } Transform2d::Transform2d(Translation2d translation, Rotation2d rotation) - : m_translation(translation), m_rotation(rotation) {} + : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} Transform2d Transform2d::Inverse() const { // We are rotating the difference between the translations diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp new file mode 100644 index 0000000000..f8a11857e9 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Transform3d.h" + +#include "frc/geometry/Pose3d.h" + +using namespace frc; + +Transform3d::Transform3d(Pose3d initial, Pose3d 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(); +} + +Transform3d::Transform3d(Translation3d translation, Rotation3d rotation) + : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} + +Transform3d Transform3d::Inverse() const { + // 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). + return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()}; +} + +Transform3d Transform3d::operator+(const Transform3d& other) const { + return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)}; +} + +bool Transform3d::operator==(const Transform3d& other) const { + return m_translation == other.m_translation && m_rotation == other.m_rotation; +} + +bool Transform3d::operator!=(const Transform3d& other) const { + return !operator==(other); +} diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp new file mode 100644 index 0000000000..9a25254652 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Translation3d.h" + +#include "units/math.h" + +using namespace frc; + +Translation3d::Translation3d(units::meter_t x, units::meter_t y, + units::meter_t z) + : m_x(x), m_y(y), m_z(z) {} + +Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) { + auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle); + m_x = rectangular.X(); + m_y = rectangular.Y(); + m_z = rectangular.Z(); +} + +units::meter_t Translation3d::Distance(const Translation3d& other) const { + return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) + + units::math::pow<2>(other.m_y - m_y) + + units::math::pow<2>(other.m_z - m_z)); +} + +units::meter_t Translation3d::Norm() const { + return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z); +} + +Translation3d Translation3d::RotateBy(const Rotation3d& other) const { + Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()}; + auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse(); + return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()}, + units::meter_t{qprime.Z()}}; +} + +Translation2d Translation3d::ToTranslation2d() const { + return Translation2d{m_x, m_y}; +} + +Translation3d Translation3d::operator+(const Translation3d& other) const { + return {X() + other.X(), Y() + other.Y(), Z() + other.Z()}; +} + +Translation3d Translation3d::operator-(const Translation3d& other) const { + return *this + -other; +} + +Translation3d Translation3d::operator-() const { + return {-m_x, -m_y, -m_z}; +} + +Translation3d Translation3d::operator*(double scalar) const { + return {scalar * m_x, scalar * m_y, scalar * m_z}; +} + +Translation3d Translation3d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + +bool Translation3d::operator==(const Translation3d& other) const { + return units::math::abs(m_x - other.m_x) < 1E-9_m && + units::math::abs(m_y - other.m_y) < 1E-9_m && + units::math::abs(m_z - other.m_z) < 1E-9_m; +} + +bool Translation3d::operator!=(const Translation3d& other) const { + return !operator==(other); +} diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index ebaa7c193e..34cda3e8b8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -17,13 +17,12 @@ class json; namespace frc { /** - * Represents a 2d pose containing translational and rotational elements. + * Represents a 2D pose containing translational and rotational elements. */ class WPILIB_DLLEXPORT Pose2d { public: /** * Constructs a pose at the origin facing toward the positive X axis. - * (Translation2d{0, 0} and Rotation{0}) */ constexpr Pose2d() = default; @@ -36,8 +35,8 @@ class WPILIB_DLLEXPORT Pose2d { Pose2d(Translation2d translation, Rotation2d rotation); /** - * Convenience constructors that takes in x and y values directly instead of - * having to construct a Translation2d. + * Constructs a pose with x and y translations instead of a separate + * 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. @@ -49,9 +48,11 @@ class WPILIB_DLLEXPORT Pose2d { * 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]
+   * [t_new]    [  0,    0, 1][transform.t]
+   *
* * @param other The transform to transform the pose by. * @@ -152,7 +153,7 @@ class WPILIB_DLLEXPORT Pose2d { * @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 0.5 degrees since the previous pose - * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)} + * update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}. * * @return The new pose of the robot. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h new file mode 100644 index 0000000000..23c75d38a8 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -0,0 +1,180 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "Pose2d.h" +#include "Transform3d.h" +#include "Translation3d.h" +#include "Twist3d.h" + +namespace frc { + +/** + * Represents a 3D pose containing translational and rotational elements. + */ +class WPILIB_DLLEXPORT Pose3d { + public: + /** + * Constructs a pose at the origin facing toward the positive X axis. + */ + constexpr Pose3d() = 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. + */ + Pose3d(Translation3d translation, Rotation3d rotation); + + /** + * Constructs a pose with x, y, and z translations instead of a separate + * Translation3d. + * + * @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 z The z component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, + Rotation3d rotation); + + /** + * Transforms the pose by the given transformation and returns the new + * transformed pose. + * + * @param other The transform to transform the pose by. + * + * @return The transformed pose. + */ + Pose3d operator+(const Transform3d& other) const; + + /** + * Returns the Transform3d that maps the one pose to another. + * + * @param other The initial pose of the transformation. + * @return The transform that maps the other pose to the current pose. + */ + Transform3d operator-(const Pose3d& other) const; + + /** + * Checks equality between this Pose3d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Pose3d& other) const; + + /** + * Checks inequality between this Pose3d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Pose3d& other) const; + + /** + * Returns the underlying translation. + * + * @return Reference to the translational component of the pose. + */ + const Translation3d& Translation() const { return m_translation; } + + /** + * Returns the X component of the pose's translation. + * + * @return The x component of the pose's translation. + */ + units::meter_t X() const { return m_translation.X(); } + + /** + * Returns the Y component of the pose's translation. + * + * @return The y component of the pose's translation. + */ + units::meter_t Y() const { return m_translation.Y(); } + + /** + * Returns the Z component of the pose's translation. + * + * @return The z component of the pose's translation. + */ + units::meter_t Z() const { return m_translation.Z(); } + + /** + * Returns the underlying rotation. + * + * @return Reference to the rotational component of the pose. + */ + const Rotation3d& 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. + */ + Pose3d TransformBy(const Transform3d& 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. + */ + Pose3d RelativeTo(const Pose3d& other) const; + + /** + * Obtain a new Pose3d from a (constant curvature) velocity. + * + * 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 0.5 degrees since the previous pose + * update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0, + * 0.5_deg}}. + * + * @return The new pose of the robot. + */ + Pose3d Exp(const Twist3d& twist) const; + + /** + * Returns a Twist3d that maps this pose to the end pose. If c is the output + * of a.Log(b), then a.Exp(c) would yield b. + * + * @param end The end pose for the transformation. + * + * @return The twist that maps this to end. + */ + Twist3d Log(const Pose3d& end) const; + + /** + * Returns a Pose2d representing this Pose3d projected into the X-Y plane. + */ + Pose2d ToPose2d() const; + + private: + Translation3d m_translation; + Rotation3d m_rotation; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h new file mode 100644 index 0000000000..c21db3c808 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -0,0 +1,95 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/EigenCore.h" + +namespace frc { + +class WPILIB_DLLEXPORT Quaternion { + public: + /** + * Constructs a quaternion with a default angle of 0 degrees. + */ + Quaternion() = default; + + /** + * Constructs a quaternion with the given components. + * + * @param w W component of the quaternion. + * @param x X component of the quaternion. + * @param y Y component of the quaternion. + * @param z Z component of the quaternion. + */ + Quaternion(double w, double x, double y, double z); + + /** + * Multiply with another quaternion. + * + * @param other The other quaternion. + */ + Quaternion operator*(const Quaternion& other) const; + + /** + * Checks equality between this Quaternion and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Quaternion& other) const; + + /** + * Checks inequality between this Quaternion and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Quaternion& other) const; + + /** + * Returns the inverse of the quaternion. + */ + Quaternion Inverse() const; + + /** + * Normalizes the quaternion. + */ + Quaternion Normalize() const; + + /** + * Returns W component of the quaternion. + */ + double W() const; + + /** + * Returns X component of the quaternion. + */ + double X() const; + + /** + * Returns Y component of the quaternion. + */ + double Y() const; + + /** + * Returns Z component of the quaternion. + */ + double Z() const; + + /** + * Returns the rotation vector representation of this quaternion. + * + * This is also the log operator of SO(3). + */ + Eigen::Vector3d ToRotationVector() const; + + private: + double m_r = 1.0; + Eigen::Vector3d m_v{0.0, 0.0, 0.0}; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 73f32d4d34..c206d5f892 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -15,7 +15,7 @@ class json; namespace frc { /** - * A rotation in a 2d coordinate frame represented by a point on the unit circle + * A rotation in a 2D coordinate frame represented by a point on the unit circle * (cosine and sine). */ class WPILIB_DLLEXPORT Rotation2d { diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h new file mode 100644 index 0000000000..ef19bf8d3c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -0,0 +1,153 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "Quaternion.h" +#include "Rotation2d.h" +#include "frc/EigenCore.h" +#include "units/angle.h" + +namespace frc { + +/** + * A rotation in a 3D coordinate frame. + */ +class WPILIB_DLLEXPORT Rotation3d { + public: + /** + * Constructs a Rotation3d with a default angle of 0 degrees. + */ + Rotation3d() = default; + + /** + * Constructs a Rotation3d from a quaternion. + * + * @param q The quaternion. + */ + explicit Rotation3d(const Quaternion& q); + + /** + * Constructs a Rotation3d from extrinsic roll, pitch, and yaw. + * + * Extrinsic rotations occur in that order around the axes in the fixed global + * frame rather than the body frame. + * + * @param roll The counterclockwise rotation angle around the X axis (roll). + * @param pitch The counterclockwise rotation angle around the Y axis (pitch). + * @param yaw The counterclockwise rotation angle around the Z axis (yaw). + */ + Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw); + + /** + * Constructs a Rotation3d with the given axis-angle representation. The axis + * doesn't have to be normalized. + * + * @param axis The rotation axis. + * @param angle The rotation around the axis. + */ + Rotation3d(const Vectord<3>& axis, units::radian_t angle); + + /** + * Adds two rotations together. + * + * @param other The rotation to add. + * + * @return The sum of the two rotations. + */ + Rotation3d operator+(const Rotation3d& other) const; + + /** + * Subtracts the new rotation from the current rotation and returns the new + * rotation. + * + * @param other The rotation to subtract. + * + * @return The difference between the two rotations. + */ + Rotation3d operator-(const Rotation3d& other) const; + + /** + * Takes the inverse of the current rotation. + * + * @return The inverse of the current rotation. + */ + Rotation3d operator-() const; + + /** + * Multiplies the current rotation by a scalar. + * @param scalar The scalar. + * + * @return The new scaled Rotation3d. + */ + Rotation3d operator*(double scalar) const; + + /** + * Checks equality between this Rotation3d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Rotation3d& other) const; + + /** + * Checks inequality between this Rotation3d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Rotation3d& other) const; + + /** + * Adds the new rotation to the current rotation. + * + * @param other The rotation to rotate by. + * + * @return The new rotated Rotation3d. + */ + Rotation3d RotateBy(const Rotation3d& other) const; + + /** + * Returns the quaternion representation of the Rotation3d. + */ + const Quaternion& GetQuaternion() const; + + /** + * Returns the counterclockwise rotation angle around the X axis (roll). + */ + units::radian_t X() const; + + /** + * Returns the counterclockwise rotation angle around the Y axis (pitch). + */ + units::radian_t Y() const; + + /** + * Returns the counterclockwise rotation angle around the Z axis (yaw). + */ + units::radian_t Z() const; + + /** + * Returns the axis in the axis-angle representation of this rotation. + */ + Vectord<3> Axis() const; + + /** + * Returns the angle in the axis-angle representation of this rotation. + */ + units::radian_t Angle() const; + + /** + * Returns a Rotation2d representing this Rotation3d projected into the X-Y + * plane. + */ + Rotation2d ToRotation2d() const; + + private: + Quaternion m_q; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h new file mode 100644 index 0000000000..cb89dee70c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "Translation3d.h" + +namespace frc { + +class WPILIB_DLLEXPORT Pose3d; + +/** + * Represents a transformation for a Pose3d. + */ +class WPILIB_DLLEXPORT Transform3d { + 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. + */ + Transform3d(Pose3d initial, Pose3d 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. + */ + Transform3d(Translation3d translation, Rotation3d rotation); + + /** + * Constructs the identity transform -- maps an initial pose to itself. + */ + constexpr Transform3d() = default; + + /** + * Returns the translation component of the transformation. + * + * @return Reference to the translational component of the transform. + */ + const Translation3d& Translation() const { return m_translation; } + + /** + * Returns the X component of the transformation's translation. + * + * @return The x component of the transformation's translation. + */ + units::meter_t X() const { return m_translation.X(); } + + /** + * Returns the Y component of the transformation's translation. + * + * @return The y component of the transformation's translation. + */ + units::meter_t Y() const { return m_translation.Y(); } + + /** + * Returns the Z component of the transformation's translation. + * + * @return The z component of the transformation's translation. + */ + units::meter_t Z() const { return m_translation.Z(); } + + /** + * Returns the rotational component of the transformation. + * + * @return Reference to the rotational component of the transform. + */ + const Rotation3d& Rotation() const { return m_rotation; } + + /** + * Invert the transformation. This is useful for undoing a transformation. + * + * @return The inverted transformation. + */ + Transform3d Inverse() const; + + /** + * Scales the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform3d. + */ + Transform3d operator*(double scalar) const { + return Transform3d(m_translation * scalar, m_rotation * scalar); + } + + /** + * Composes two transformations. + * + * @param other The transform to compose with this one. + * @return The composition of the two transformations. + */ + Transform3d operator+(const Transform3d& other) const; + + /** + * Checks equality between this Transform3d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Transform3d& other) const; + + /** + * Checks inequality between this Transform3d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Transform3d& other) const; + + private: + Translation3d m_translation; + Rotation3d m_rotation; +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 204da30b40..67180ddef0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -16,12 +16,12 @@ class json; namespace frc { /** - * Represents a translation in 2d space. + * 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. + * When the robot is at the origin facing in the positive X direction, forward + * is positive X and left is positive Y. */ class WPILIB_DLLEXPORT Translation2d { public: @@ -49,10 +49,9 @@ class WPILIB_DLLEXPORT Translation2d { Translation2d(units::meter_t distance, const Rotation2d& angle); /** - * Calculates the distance between two translations in 2d space. + * 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) + * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²). * * @param other The translation to compute the distance to. * @@ -63,14 +62,14 @@ class WPILIB_DLLEXPORT Translation2d { /** * Returns the X component of the translation. * - * @return The x component of the translation. + * @return The X component of the translation. */ units::meter_t X() const { return m_x; } /** * Returns the Y component of the translation. * - * @return The y component of the translation. + * @return The Y component of the translation. */ units::meter_t Y() const { return m_y; } @@ -82,16 +81,18 @@ class WPILIB_DLLEXPORT Translation2d { units::meter_t Norm() const; /** - * Applies a rotation to the translation in 2d space. + * 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}. + * 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. * @@ -100,11 +101,10 @@ class WPILIB_DLLEXPORT Translation2d { Translation2d RotateBy(const Rotation2d& other) const; /** - * Adds two translations in 2d space and returns the sum. This is similar to - * vector addition. + * Returns the sum of two translations in 2D space. * - * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = - * Translation2d{3.0, 8.0} + * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} = + * Translation3d{3.0, 8.0}. * * @param other The translation to add. * @@ -113,11 +113,10 @@ class WPILIB_DLLEXPORT Translation2d { Translation2d operator+(const Translation2d& other) const; /** - * Subtracts the other translation from the other translation and returns the - * difference. + * Returns the difference between two translations. * * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = - * Translation2d{4.0, 2.0} + * Translation2d{4.0, 2.0}. * * @param other The translation to subtract. * @@ -127,17 +126,17 @@ class WPILIB_DLLEXPORT Translation2d { /** * 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. + * rotating by 180 degrees, flipping the point over both axes, or negating all + * 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. + * Returns the translation multiplied by a scalar. * - * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0} + * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}. * * @param scalar The scalar to multiply by. * @@ -146,9 +145,9 @@ class WPILIB_DLLEXPORT Translation2d { Translation2d operator*(double scalar) const; /** - * Divides the translation by a scalar and returns the new translation. + * Returns the translation divided by a scalar. * - * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25} + * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}. * * @param scalar The scalar to divide by. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h new file mode 100644 index 0000000000..385a300c21 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -0,0 +1,185 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "Rotation3d.h" +#include "Translation2d.h" +#include "units/length.h" + +namespace frc { + +/** + * Represents a translation in 3D 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 at the origin facing in the positive X direction, forward is + * positive X, left is positive Y, and up is positive Z. + */ +class WPILIB_DLLEXPORT Translation3d { + public: + /** + * Constructs a Translation3d with X, Y, and Z components equal to zero. + */ + constexpr Translation3d() = default; + + /** + * Constructs a Translation3d with the X, Y, and Z components equal to the + * provided values. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + * @param z The z component of the translation. + */ + Translation3d(units::meter_t x, units::meter_t y, units::meter_t z); + + /** + * Constructs a Translation3d with the provided distance and angle. This is + * essentially converting from polar coordinates to Cartesian coordinates. + * + * @param distance The distance from the origin to the end of the translation. + * @param angle The angle between the x-axis and the translation vector. + */ + Translation3d(units::meter_t distance, const Rotation3d& angle); + + /** + * Calculates the distance between two translations in 3D space. + * + * The distance between translations is defined as + * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²). + * + * @param other The translation to compute the distance to. + * + * @return The distance between the two translations. + */ + units::meter_t Distance(const Translation3d& other) const; + + /** + * Returns the X component of the translation. + * + * @return The Z component of the translation. + */ + units::meter_t X() const { return m_x; } + + /** + * Returns the Y component of the translation. + * + * @return The Y component of the translation. + */ + units::meter_t Y() const { return m_y; } + + /** + * Returns the Z component of the translation. + * + * @return The Z component of the translation. + */ + units::meter_t Z() const { return m_z; } + + /** + * Returns the norm, or distance from the origin to the translation. + * + * @return The norm of the translation. + */ + units::meter_t Norm() const; + + /** + * Applies a rotation to the translation in 3D space. + * + * For example, rotating a Translation3d of <2, 0, 0> by 90 degrees + * around the Z axis will return a Translation3d of <0, 2, 0>. + * + * @param other The rotation to rotate the translation by. + * + * @return The new rotated translation. + */ + Translation3d RotateBy(const Rotation3d& other) const; + + /** + * Returns a Translation2d representing this Translation3d projected into the + * X-Y plane. + */ + Translation2d ToTranslation2d() const; + + /** + * Returns the sum of two translations in 3D space. + * + * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} = + * Translation3d{3.0, 8.0, 11.0}. + * + * @param other The translation to add. + * + * @return The sum of the translations. + */ + Translation3d operator+(const Translation3d& other) const; + + /** + * Returns the difference between two translations. + * + * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} = + * Translation3d{4.0, 2.0, 0.0}. + * + * @param other The translation to subtract. + * + * @return The difference between the two translations. + */ + Translation3d operator-(const Translation3d& other) const; + + /** + * Returns the inverse of the current translation. This is equivalent to + * negating all components of the translation. + * + * @return The inverse of the current translation. + */ + Translation3d operator-() const; + + /** + * Returns the translation multiplied by a scalar. + * + * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0, + * 9.0}. + * + * @param scalar The scalar to multiply by. + * + * @return The scaled translation. + */ + Translation3d operator*(double scalar) const; + + /** + * Returns the translation divided by a scalar. + * + * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25, + * 2.25}. + * + * @param scalar The scalar to divide by. + * + * @return The scaled translation. + */ + Translation3d operator/(double scalar) const; + + /** + * Checks equality between this Translation3d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Translation3d& other) const; + + /** + * Checks inequality between this Translation3d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Translation3d& other) const; + + private: + units::meter_t m_x = 0_m; + units::meter_t m_y = 0_m; + units::meter_t m_z = 0_m; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index 6ad2b38a98..a5fcbe76c4 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -12,9 +12,9 @@ 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 change in distance along a 2D 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. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h new file mode 100644 index 0000000000..f862d497ec --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Rotation3d.h" +#include "units/angle.h" +#include "units/length.h" +#include "units/math.h" + +namespace frc { +/** + * A change in distance along a 3D arc since the last pose update. We can use + * ideas from differential calculus to create new Pose3ds from a Twist3d and + * vise versa. + * + * A Twist can be used to represent a difference between two poses. + */ +struct WPILIB_DLLEXPORT Twist3d { + /** + * Linear "dx" component + */ + units::meter_t dx = 0_m; + + /** + * Linear "dy" component + */ + units::meter_t dy = 0_m; + + /** + * Linear "dz" component + */ + units::meter_t dz = 0_m; + + /** + * Rotation vector x component. + */ + units::radian_t rx = 0_rad; + + /** + * Rotation vector y component. + */ + units::radian_t ry = 0_rad; + + /** + * Rotation vector z component. + */ + units::radian_t rz = 0_rad; + + /** + * Checks equality between this Twist3d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Twist3d& other) const { + return units::math::abs(dx - other.dx) < 1E-9_m && + units::math::abs(dy - other.dy) < 1E-9_m && + units::math::abs(dz - other.dz) < 1E-9_m && + units::math::abs(rx - other.rx) < 1E-9_rad && + units::math::abs(ry - other.ry) < 1E-9_rad && + units::math::abs(rz - other.rz) < 1E-9_rad; + } + + /** + * Checks inequality between this Twist3d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Twist3d& other) const { return !operator==(other); } + + /** + * Scale this by a given factor. + * + * @param factor The factor by which to scale. + * @return The scaled Twist3d. + */ + Twist3d operator*(double factor) const { + return Twist3d{dx * factor, dy * factor, dz * factor, + rx * factor, ry * factor, rz * factor}; + } +}; +} // namespace frc 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 5a314b131c..780c816eaa 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 @@ -21,9 +21,9 @@ class Pose2dTest { var transformed = initial.plus(transformation); assertAll( - () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon), - () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon), - () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)); + () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon), + () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon), + () -> assertEquals(50.0, transformed.getRotation().getDegrees(), kEpsilon)); } @Test @@ -34,9 +34,9 @@ class Pose2dTest { var finalRelativeToInitial = last.relativeTo(initial); assertAll( - () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon), - () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon), - () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)); + () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon), + () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon), + () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon)); } @Test @@ -61,8 +61,8 @@ class Pose2dTest { final var transform = last.minus(initial); assertAll( - () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon), - () -> assertEquals(transform.getY(), 0.0, kEpsilon), - () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)); + () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon), + () -> assertEquals(0.0, transform.getY(), kEpsilon), + () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java new file mode 100644 index 0000000000..8cccb301b7 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -0,0 +1,106 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +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 edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class Pose3dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testTransformBy() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var initial = + new Pose3d( + new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + var transformation = + new Transform3d( + new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); + + var transformed = initial.plus(transformation); + + assertAll( + () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon), + () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon), + () -> + assertEquals(Units.degreesToRadians(50.0), transformed.getRotation().getZ(), kEpsilon)); + } + + @Test + void testRelativeTo() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + + var finalRelativeToInitial = last.relativeTo(initial); + + assertAll( + () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon), + () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon), + () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon)); + } + + @Test + void testEquality() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0))); + var two = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0))); + assertEquals(one, two); + } + + @Test + void testInequality() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0))); + var two = new Pose3d(0.0, 1.524, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0))); + assertNotEquals(one, two); + } + + @Test + void testMinus() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + + final var transform = last.minus(initial); + + assertAll( + () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon), + () -> assertEquals(0.0, transform.getY(), kEpsilon), + () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon)); + } + + @Test + void testToPose2d() { + var pose = + new Pose3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var expected = new Pose2d(1.0, 2.0, new Rotation2d(Units.degreesToRadians(40.0))); + + assertEquals(expected, pose.toPose2d()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java new file mode 100644 index 0000000000..e09e168a01 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class QuaternionTest { + @Test + void testInit() { + // Identity + var q1 = new Quaternion(); + assertEquals(1.0, q1.getW()); + assertEquals(0.0, q1.getX()); + assertEquals(0.0, q1.getY()); + assertEquals(0.0, q1.getZ()); + + // Normalized + var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5); + assertEquals(0.5, q2.getW()); + assertEquals(0.5, q2.getX()); + assertEquals(0.5, q2.getY()); + assertEquals(0.5, q2.getZ()); + + // Unnormalized + var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5); + assertEquals(0.75, q3.getW()); + assertEquals(0.3, q3.getX()); + assertEquals(0.4, q3.getY()); + assertEquals(0.5, q3.getZ()); + + q3 = q3.normalize(); + double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5); + assertEquals(0.75 / norm, q3.getW()); + assertEquals(0.3 / norm, q3.getX()); + assertEquals(0.4 / norm, q3.getY()); + assertEquals(0.5 / norm, q3.getZ()); + assertEquals( + 1.0, + q3.getW() * q3.getW() + + q3.getX() * q3.getX() + + q3.getY() * q3.getY() + + q3.getZ() * q3.getZ()); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testTimes() { + // 90° CCW rotations around each axis + double c = Math.cos(Units.degreesToRadians(90.0) / 2.0); + double s = Math.sin(Units.degreesToRadians(90.0) / 2.0); + var xRot = new Quaternion(c, s, 0.0, 0.0); + var yRot = new Quaternion(c, 0.0, s, 0.0); + var zRot = new Quaternion(c, 0.0, 0.0, s); + + // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should + // produce a 90° CCW Y rotation + var expected = yRot; + var actual = zRot.times(yRot).times(xRot); + assertEquals(expected.getW(), actual.getW(), 1e-9); + assertEquals(expected.getX(), actual.getX(), 1e-9); + assertEquals(expected.getY(), actual.getY(), 1e-9); + assertEquals(expected.getZ(), actual.getZ(), 1e-9); + + // Identity + var q = + new Quaternion( + 0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594); + actual = q.times(q.inverse()); + assertEquals(1.0, actual.getW()); + assertEquals(0.0, actual.getX()); + assertEquals(0.0, actual.getY()); + assertEquals(0.0, actual.getZ()); + } + + @Test + void testInverse() { + var q = new Quaternion(0.75, 0.3, 0.4, 0.5); + var inv = q.inverse(); + + assertEquals(q.getW(), inv.getW()); + assertEquals(-q.getX(), inv.getX()); + assertEquals(-q.getY(), inv.getY()); + assertEquals(-q.getZ(), inv.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index 2368ba3b3c..f960e493cb 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -19,8 +19,8 @@ class Rotation2dTest { var rot2 = Rotation2d.fromRadians(Math.PI / 4); assertAll( - () -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon), - () -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon)); + () -> assertEquals(60.0, rot1.getDegrees(), kEpsilon), + () -> assertEquals(45.0, rot2.getDegrees(), kEpsilon)); } @Test @@ -29,8 +29,8 @@ class Rotation2dTest { var rot2 = Rotation2d.fromDegrees(30.0); assertAll( - () -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon), - () -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon)); + () -> assertEquals(Math.PI / 4.0, rot1.getRadians(), kEpsilon), + () -> assertEquals(Math.PI / 6.0, rot2.getRadians(), kEpsilon)); } @Test @@ -39,8 +39,8 @@ class Rotation2dTest { var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0)); assertAll( - () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon), - () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)); + () -> assertEquals(Math.PI / 2.0, rotated.getRadians(), kEpsilon), + () -> assertEquals(90.0, rotated.getDegrees(), kEpsilon)); } @Test @@ -48,7 +48,7 @@ class Rotation2dTest { var rot = Rotation2d.fromDegrees(90.0); rot = rot.plus(Rotation2d.fromDegrees(30.0)); - assertEquals(rot.getDegrees(), 120.0, kEpsilon); + assertEquals(120.0, rot.getDegrees(), kEpsilon); } @Test @@ -56,7 +56,7 @@ class Rotation2dTest { var rot1 = Rotation2d.fromDegrees(70.0); var rot2 = Rotation2d.fromDegrees(30.0); - assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon); + assertEquals(40.0, rot1.minus(rot2).getDegrees(), kEpsilon); } @Test @@ -65,9 +65,9 @@ class Rotation2dTest { var rot2 = Rotation2d.fromDegrees(43.0); assertEquals(rot1, rot2); - var rot3 = Rotation2d.fromDegrees(-180.0); - var rot4 = Rotation2d.fromDegrees(180.0); - assertEquals(rot3, rot4); + rot1 = Rotation2d.fromDegrees(-180.0); + rot2 = Rotation2d.fromDegrees(180.0); + assertEquals(rot1, rot2); } @Test @@ -83,12 +83,12 @@ class Rotation2dTest { var rot1 = Rotation2d.fromDegrees(50); var rot2 = Rotation2d.fromDegrees(70); var interpolated = rot1.interpolate(rot2, 0.5); - assertEquals(60.0, interpolated.getDegrees(), 1e-2); + assertEquals(60.0, interpolated.getDegrees(), kEpsilon); // -160 minus half distance between 170 and -160 (15) = -175 - var rot3 = Rotation2d.fromDegrees(170); - var rot4 = Rotation2d.fromDegrees(-160); - interpolated = rot3.interpolate(rot4, 0.5); + rot1 = Rotation2d.fromDegrees(170); + rot2 = Rotation2d.fromDegrees(-160); + interpolated = rot1.interpolate(rot2, 0.5); assertEquals(-175.0, interpolated.getDegrees()); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java new file mode 100644 index 0000000000..b48150ed92 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -0,0 +1,293 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +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 edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class Rotation3dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testInit() { + @SuppressWarnings("LocalVariableName") + var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + final var rot1 = new Rotation3d(xAxis, Math.PI / 3); + final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0); + assertEquals(rot1, rot2); + + @SuppressWarnings("LocalVariableName") + var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + final var rot3 = new Rotation3d(yAxis, Math.PI / 3); + final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0); + assertEquals(rot3, rot4); + + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + final var rot5 = new Rotation3d(zAxis, Math.PI / 3); + final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3); + assertEquals(rot5, rot6); + } + + @Test + void testRadiansToDegrees() { + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot1 = new Rotation3d(zAxis, Math.PI / 3); + assertAll( + () -> assertEquals(Units.degreesToRadians(0.0), rot1.getX(), kEpsilon), + () -> assertEquals(Units.degreesToRadians(0.0), rot1.getY(), kEpsilon), + () -> assertEquals(Units.degreesToRadians(60.0), rot1.getZ(), kEpsilon)); + + var rot2 = new Rotation3d(zAxis, Math.PI / 4); + assertAll( + () -> assertEquals(Units.degreesToRadians(0.0), rot2.getX(), kEpsilon), + () -> assertEquals(Units.degreesToRadians(0.0), rot2.getY(), kEpsilon), + () -> assertEquals(Units.degreesToRadians(45.0), rot2.getZ(), kEpsilon)); + } + + @Test + void testRadiansAndDegrees() { + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(45.0)); + assertAll( + () -> assertEquals(0.0, rot1.getX(), kEpsilon), + () -> assertEquals(0.0, rot1.getY(), kEpsilon), + () -> assertEquals(Math.PI / 4.0, rot1.getZ(), kEpsilon)); + + var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0)); + assertAll( + () -> assertEquals(0.0, rot2.getX(), kEpsilon), + () -> assertEquals(0.0, rot2.getY(), kEpsilon), + () -> assertEquals(Math.PI / 6.0, rot2.getZ(), kEpsilon)); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testRotationLoop() { + var rot = new Rotation3d(); + + rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0)); + var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0); + assertEquals(expected, rot); + + rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0)); + expected = + new Rotation3d( + VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)), + Units.degreesToRadians(120.0)); + assertEquals(expected, rot); + + rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0))); + expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0); + assertEquals(expected, rot); + + rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0)); + assertEquals(new Rotation3d(), rot); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testRotateByFromZeroX() { + final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + + final var zero = new Rotation3d(); + var rotated = zero.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0))); + + var expected = new Rotation3d(xAxis, Units.degreesToRadians(90.0)); + assertEquals(expected, rotated); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testRotateByFromZeroY() { + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + + final var zero = new Rotation3d(); + var rotated = zero.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0))); + + var expected = new Rotation3d(yAxis, Units.degreesToRadians(90.0)); + assertEquals(expected, rotated); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testRotateByFromZeroZ() { + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + final var zero = new Rotation3d(); + var rotated = zero.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0))); + + var expected = new Rotation3d(zAxis, Units.degreesToRadians(90.0)); + assertEquals(expected, rotated); + } + + @Test + void testRotateByNonZeroX() { + @SuppressWarnings("LocalVariableName") + final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + + var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0)); + rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0))); + + var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0)); + assertEquals(expected, rot); + } + + @Test + void testRotateByNonZeroY() { + @SuppressWarnings("LocalVariableName") + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + + var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0)); + rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0))); + + var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0)); + assertEquals(expected, rot); + } + + @Test + void testRotateByNonZeroZ() { + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0)); + rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0))); + + var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0)); + assertEquals(expected, rot); + } + + @Test + void testMinus() { + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0)); + var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0)); + + assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon); + } + + @Test + void testEquality() { + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0)); + var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.0)); + assertEquals(rot1, rot2); + + rot1 = new Rotation3d(zAxis, Units.degreesToRadians(-180.0)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(180.0)); + assertEquals(rot1, rot2); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testAxisAngle() { + final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(90.0)); + assertEquals(xAxis, rot1.getAxis()); + assertEquals(Math.PI / 2.0, rot1.getAngle(), 1e-9); + + var rot2 = new Rotation3d(yAxis, Units.degreesToRadians(45.0)); + assertEquals(yAxis, rot2.getAxis()); + assertEquals(Math.PI / 4.0, rot2.getAngle(), 1e-9); + + var rot3 = new Rotation3d(zAxis, Units.degreesToRadians(60.0)); + assertEquals(zAxis, rot3.getAxis()); + assertEquals(Math.PI / 3.0, rot3.getAngle(), 1e-9); + } + + @Test + void testToRotation2d() { + var rotation = + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0)); + var expected = new Rotation2d(Units.degreesToRadians(40.0)); + + assertEquals(expected, rotation.toRotation2d()); + } + + @Test + void testInequality() { + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0)); + var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.5)); + assertNotEquals(rot1, rot2); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testInterpolate() { + final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + // 50 + (70 - 50) * 0.5 = 60 + var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(50)); + var rot2 = new Rotation3d(xAxis, Units.degreesToRadians(70)); + var interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(Units.degreesToRadians(60.0), interpolated.getX(), kEpsilon); + assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon); + assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon); + + // -160 minus half distance between 170 and -160 (15) = -175 + rot1 = new Rotation3d(xAxis, Units.degreesToRadians(170)); + rot2 = new Rotation3d(xAxis, Units.degreesToRadians(-160)); + interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(Units.degreesToRadians(-175.0), interpolated.getX()); + assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon); + assertEquals(Units.degreesToRadians(0.0), interpolated.getZ()); + + // 50 + (70 - 50) * 0.5 = 60 + rot1 = new Rotation3d(yAxis, Units.degreesToRadians(50)); + rot2 = new Rotation3d(yAxis, Units.degreesToRadians(70)); + interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon); + assertEquals(Units.degreesToRadians(60.0), interpolated.getY(), kEpsilon); + assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon); + + // -160 minus half distance between 170 and -160 (165) = 5 + rot1 = new Rotation3d(yAxis, Units.degreesToRadians(170)); + rot2 = new Rotation3d(yAxis, Units.degreesToRadians(-160)); + interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(Units.degreesToRadians(180.0), interpolated.getX(), kEpsilon); + assertEquals(Units.degreesToRadians(-5.0), interpolated.getY(), kEpsilon); + assertEquals(Units.degreesToRadians(180.0), interpolated.getZ(), kEpsilon); + + // 50 + (70 - 50) * 0.5 = 60 + rot1 = new Rotation3d(zAxis, Units.degreesToRadians(50)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(70)); + interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon); + assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon); + assertEquals(Units.degreesToRadians(60.0), interpolated.getZ(), kEpsilon); + + // -160 minus half distance between 170 and -160 (15) = -175 + rot1 = new Rotation3d(zAxis, Units.degreesToRadians(170)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(-160)); + interpolated = rot1.interpolate(rot2, 0.5); + assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon); + assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon); + assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java new file mode 100644 index 0000000000..5b59d5056e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class Transform3dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testInverse() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var initial = + new Pose3d( + new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + var transform = + new Transform3d( + new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); + + var transformed = initial.plus(transform); + var untransformed = transformed.plus(transform.inverse()); + + assertAll( + () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon), + () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon), + () -> assertEquals(initial.getZ(), untransformed.getZ(), kEpsilon), + () -> + assertEquals( + initial.getRotation().getZ(), untransformed.getRotation().getZ(), kEpsilon)); + } + + @Test + void testComposition() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var initial = + new Pose3d( + new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + var transform1 = + new Transform3d( + new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); + var transform2 = + new Transform3d( + new Translation3d(0.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0))); + + var transformedSeparate = initial.plus(transform1).plus(transform2); + var transformedCombined = initial.plus(transform1.plus(transform2)); + + assertAll( + () -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon), + () -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon), + () -> assertEquals(transformedSeparate.getZ(), transformedCombined.getZ(), kEpsilon), + () -> + assertEquals( + transformedSeparate.getRotation().getZ(), + transformedCombined.getRotation().getZ(), + kEpsilon)); + } +} 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 2d8eeaabe7..c8f5690506 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 @@ -21,8 +21,8 @@ class Translation2dTest { var sum = one.plus(two); assertAll( - () -> assertEquals(sum.getX(), 3.0, kEpsilon), - () -> assertEquals(sum.getY(), 8.0, kEpsilon)); + () -> assertEquals(3.0, sum.getX(), kEpsilon), + () -> assertEquals(8.0, sum.getY(), kEpsilon)); } @Test @@ -33,8 +33,8 @@ class Translation2dTest { var difference = one.minus(two); assertAll( - () -> assertEquals(difference.getX(), -1.0, kEpsilon), - () -> assertEquals(difference.getY(), -2.0, kEpsilon)); + () -> assertEquals(-1.0, difference.getX(), kEpsilon), + () -> assertEquals(-2.0, difference.getY(), kEpsilon)); } @Test @@ -43,8 +43,8 @@ class Translation2dTest { var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0)); assertAll( - () -> assertEquals(rotated.getX(), 0.0, kEpsilon), - () -> assertEquals(rotated.getY(), 3.0, kEpsilon)); + () -> assertEquals(0.0, rotated.getX(), kEpsilon), + () -> assertEquals(3.0, rotated.getY(), kEpsilon)); } @Test @@ -53,8 +53,8 @@ class Translation2dTest { var mult = original.times(3); assertAll( - () -> assertEquals(mult.getX(), 9.0, kEpsilon), - () -> assertEquals(mult.getY(), 15.0, kEpsilon)); + () -> assertEquals(9.0, mult.getX(), kEpsilon), + () -> assertEquals(15.0, mult.getY(), kEpsilon)); } @Test @@ -63,21 +63,21 @@ class Translation2dTest { var div = original.div(2); assertAll( - () -> assertEquals(div.getX(), 1.5, kEpsilon), - () -> assertEquals(div.getY(), 2.5, kEpsilon)); + () -> assertEquals(1.5, div.getX(), kEpsilon), + () -> assertEquals(2.5, div.getY(), kEpsilon)); } @Test void testNorm() { var one = new Translation2d(3.0, 5.0); - assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon); + assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), 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); + assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon); } @Test @@ -86,8 +86,8 @@ class Translation2dTest { var inverted = original.unaryMinus(); assertAll( - () -> assertEquals(inverted.getX(), 4.5, kEpsilon), - () -> assertEquals(inverted.getY(), -7, kEpsilon)); + () -> assertEquals(4.5, inverted.getX(), kEpsilon), + () -> assertEquals(-7.0, inverted.getY(), kEpsilon)); } @Test @@ -109,9 +109,9 @@ class Translation2dTest { var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0)); var two = new Translation2d(2, Rotation2d.fromDegrees(60.0)); assertAll( - () -> assertEquals(one.getX(), 1.0, kEpsilon), - () -> assertEquals(one.getY(), 1.0, kEpsilon), - () -> assertEquals(two.getX(), 1.0, kEpsilon), - () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon)); + () -> assertEquals(1.0, one.getX(), kEpsilon), + () -> assertEquals(1.0, one.getY(), kEpsilon), + () -> assertEquals(1.0, two.getX(), kEpsilon), + () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java new file mode 100644 index 0000000000..b8f16dd6b2 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java @@ -0,0 +1,155 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +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 edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class Translation3dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testSum() { + var one = new Translation3d(1.0, 3.0, 5.0); + var two = new Translation3d(2.0, 5.0, 8.0); + + var sum = one.plus(two); + + assertAll( + () -> assertEquals(3.0, sum.getX(), kEpsilon), + () -> assertEquals(8.0, sum.getY(), kEpsilon), + () -> assertEquals(13.0, sum.getZ(), kEpsilon)); + } + + @Test + void testDifference() { + var one = new Translation3d(1.0, 3.0, 5.0); + var two = new Translation3d(2.0, 5.0, 8.0); + + var difference = one.minus(two); + + assertAll( + () -> assertEquals(-1.0, difference.getX(), kEpsilon), + () -> assertEquals(-2.0, difference.getY(), kEpsilon), + () -> assertEquals(-3.0, difference.getZ(), kEpsilon)); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testRotateBy() { + var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var translation = new Translation3d(1.0, 2.0, 3.0); + + var rotated1 = translation.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0))); + assertAll( + () -> assertEquals(1.0, rotated1.getX(), kEpsilon), + () -> assertEquals(-3.0, rotated1.getY(), kEpsilon), + () -> assertEquals(2.0, rotated1.getZ(), kEpsilon)); + + var rotated2 = translation.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0))); + assertAll( + () -> assertEquals(3.0, rotated2.getX(), kEpsilon), + () -> assertEquals(2.0, rotated2.getY(), kEpsilon), + () -> assertEquals(-1.0, rotated2.getZ(), kEpsilon)); + + var rotated3 = translation.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0))); + assertAll( + () -> assertEquals(-2.0, rotated3.getX(), kEpsilon), + () -> assertEquals(1.0, rotated3.getY(), kEpsilon), + () -> assertEquals(3.0, rotated3.getZ(), kEpsilon)); + } + + @Test + void testToTranslation2d() { + var translation = new Translation3d(1.0, 2.0, 3.0); + var expected = new Translation2d(1.0, 2.0); + + assertEquals(expected, translation.toTranslation2d()); + } + + @Test + void testMultiplication() { + var original = new Translation3d(3.0, 5.0, 7.0); + var mult = original.times(3); + + assertAll( + () -> assertEquals(9.0, mult.getX(), kEpsilon), + () -> assertEquals(15.0, mult.getY(), kEpsilon), + () -> assertEquals(21.0, mult.getZ(), kEpsilon)); + } + + @Test + void testDivision() { + var original = new Translation3d(3.0, 5.0, 7.0); + var div = original.div(2); + + assertAll( + () -> assertEquals(1.5, div.getX(), kEpsilon), + () -> assertEquals(2.5, div.getY(), kEpsilon), + () -> assertEquals(3.5, div.getZ(), kEpsilon)); + } + + @Test + void testNorm() { + var one = new Translation3d(3.0, 5.0, 7.0); + assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon); + } + + @Test + void testDistance() { + var one = new Translation3d(1.0, 1.0, 1.0); + var two = new Translation3d(6.0, 6.0, 6.0); + assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon); + } + + @Test + void testUnaryMinus() { + var original = new Translation3d(-4.5, 7.0, 9.0); + var inverted = original.unaryMinus(); + + assertAll( + () -> assertEquals(4.5, inverted.getX(), kEpsilon), + () -> assertEquals(-7.0, inverted.getY(), kEpsilon), + () -> assertEquals(-9.0, inverted.getZ(), kEpsilon)); + } + + @Test + void testEquality() { + var one = new Translation3d(9, 5.5, 3.5); + var two = new Translation3d(9, 5.5, 3.5); + assertEquals(one, two); + } + + @Test + void testInequality() { + var one = new Translation3d(9, 5.5, 3.5); + var two = new Translation3d(9, 5.7, 3.5); + assertNotEquals(one, two); + } + + @Test + void testPolarConstructor() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var one = new Translation3d(Math.sqrt(2), new Rotation3d(zAxis, Units.degreesToRadians(45.0))); + var two = new Translation3d(2, new Rotation3d(zAxis, Units.degreesToRadians(60.0))); + assertAll( + () -> assertEquals(1.0, one.getX(), kEpsilon), + () -> assertEquals(1.0, one.getY(), kEpsilon), + () -> assertEquals(0.0, one.getZ(), kEpsilon), + () -> assertEquals(1.0, two.getX(), kEpsilon), + () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon), + () -> assertEquals(0.0, two.getZ(), kEpsilon)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java index 69a4c8645a..4108a628ed 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java @@ -4,35 +4,28 @@ package edu.wpi.first.math.geometry; -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 org.junit.jupiter.api.Test; class Twist2dTest { - private static final double kEpsilon = 1E-9; - @Test - void testStraightLineTwist() { + void testStraight() { var straight = new Twist2d(5.0, 0.0, 0.0); var straightPose = new Pose2d().exp(straight); - assertAll( - () -> assertEquals(straightPose.getX(), 5.0, kEpsilon), - () -> assertEquals(straightPose.getY(), 0.0, kEpsilon), - () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)); + var expected = new Pose2d(5.0, 0.0, new Rotation2d()); + assertEquals(expected, straightPose); } @Test - void testQuarterCirleTwist() { + void testQuarterCirle() { var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0); var quarterCirclePose = new Pose2d().exp(quarterCircle); - assertAll( - () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon), - () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon), - () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)); + var expected = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0)); + assertEquals(expected, quarterCirclePose); } @Test @@ -40,10 +33,8 @@ class Twist2dTest { var diagonal = new Twist2d(2.0, 2.0, 0.0); var diagonalPose = new Pose2d().exp(diagonal); - assertAll( - () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon), - () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon), - () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)); + var expected = new Pose2d(2.0, 2.0, new Rotation2d()); + assertEquals(expected, diagonalPose); } @Test @@ -67,9 +58,11 @@ class Twist2dTest { final var twist = start.log(end); - assertAll( - () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon), - () -> assertEquals(twist.dy, 0.0, kEpsilon), - () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)); + var expected = new Twist2d(5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0); + assertEquals(expected, twist); + + // Make sure computed twist gives back original end pose + final var reapplied = start.exp(twist); + assertEquals(end, reapplied); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java new file mode 100644 index 0000000000..f4b6b7c333 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java @@ -0,0 +1,125 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class Twist3dTest { + @Test + void testStraightX() { + var straight = new Twist3d(5.0, 0.0, 0.0, 0.0, 0.0, 0.0); + var straightPose = new Pose3d().exp(straight); + + var expected = new Pose3d(5.0, 0.0, 0.0, new Rotation3d()); + assertEquals(expected, straightPose); + } + + @Test + void testStraightY() { + var straight = new Twist3d(0.0, 5.0, 0.0, 0.0, 0.0, 0.0); + var straightPose = new Pose3d().exp(straight); + + var expected = new Pose3d(0.0, 5.0, 0.0, new Rotation3d()); + assertEquals(expected, straightPose); + } + + @Test + void testStraightZ() { + var straight = new Twist3d(0.0, 0.0, 5.0, 0.0, 0.0, 0.0); + var straightPose = new Pose3d().exp(straight); + + var expected = new Pose3d(0.0, 0.0, 5.0, new Rotation3d()); + assertEquals(expected, straightPose); + } + + @Test + void testQuarterCirle() { + @SuppressWarnings("LocalVariableName") + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var quarterCircle = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0); + var quarterCirclePose = new Pose3d().exp(quarterCircle); + + var expected = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(90.0))); + assertEquals(expected, quarterCirclePose); + } + + @Test + void testDiagonalNoDtheta() { + var diagonal = new Twist3d(2.0, 2.0, 0.0, 0.0, 0.0, 0.0); + var diagonalPose = new Pose3d().exp(diagonal); + + var expected = new Pose3d(2.0, 2.0, 0.0, new Rotation3d()); + assertEquals(expected, diagonalPose); + } + + @Test + void testEquality() { + var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0); + var two = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0); + assertEquals(one, two); + } + + @Test + void testInequality() { + var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0); + var two = new Twist3d(5, 1.2, 0, 0.0, 0.0, 3.0); + assertNotEquals(one, two); + } + + @Test + void testPose3dLogX() { + final var start = new Pose3d(); + final var end = + new Pose3d(0.0, 5.0, 5.0, new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0)); + + final var twist = start.log(end); + + var expected = + new Twist3d(0.0, 5.0 / 2.0 * Math.PI, 0.0, Units.degreesToRadians(90.0), 0.0, 0.0); + assertEquals(expected, twist); + + // Make sure computed twist gives back original end pose + final var reapplied = start.exp(twist); + assertEquals(end, reapplied); + } + + @Test + void testPose3dLogY() { + final var start = new Pose3d(); + final var end = + new Pose3d(5.0, 0.0, 5.0, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0)); + + final var twist = start.log(end); + + var expected = new Twist3d(0.0, 0.0, 5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0, 0.0); + assertEquals(expected, twist); + + // Make sure computed twist gives back original end pose + final var reapplied = start.exp(twist); + assertEquals(end, reapplied); + } + + @Test + void testPose3dLogZ() { + final var start = new Pose3d(); + final var end = + new Pose3d(5.0, 5.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0))); + + final var twist = start.log(end); + + var expected = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0); + assertEquals(expected, twist); + + // Make sure computed twist gives back original end pose + final var reapplied = start.exp(twist); + assertEquals(end, reapplied); + } +} diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index cd5b127c32..3e4023562d 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -9,51 +9,47 @@ using namespace frc; -static constexpr double kEpsilon = 1E-9; - TEST(Pose2dTest, TransformBy) { - const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)}; - const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)}; + const Pose2d initial{1_m, 2_m, Rotation2d{45_deg}}; + const Transform2d transform{Translation2d{5_m, 0_m}, Rotation2d{5_deg}}; const auto transformed = initial + transform; - EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon); + EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value()); + EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value()); + EXPECT_DOUBLE_EQ(50.0, transformed.Rotation().Degrees().value()); } TEST(Pose2dTest, RelativeTo) { - const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)}; - const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)}; + const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}}; + const Pose2d final{5_m, 5_m, Rotation2d{45.0_deg}}; const auto finalRelativeToInitial = final.RelativeTo(initial); - EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0), - kEpsilon); - EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon); - EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0, - kEpsilon); + EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value()); + EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9); + EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value()); } TEST(Pose2dTest, Equality) { - const Pose2d a{0_m, 5_m, Rotation2d(43_deg)}; - const Pose2d b{0_m, 5_m, Rotation2d(43_deg)}; + const Pose2d a{0_m, 5_m, Rotation2d{43_deg}}; + const Pose2d b{0_m, 5_m, Rotation2d{43_deg}}; EXPECT_TRUE(a == b); } TEST(Pose2dTest, Inequality) { - const Pose2d a{0_m, 5_m, Rotation2d(43_deg)}; - const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)}; + const Pose2d a{0_m, 5_m, Rotation2d{43_deg}}; + const Pose2d b{0_m, 5_ft, Rotation2d{43_deg}}; EXPECT_TRUE(a != b); } TEST(Pose2dTest, Minus) { - const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)}; - const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)}; + const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}}; + const Pose2d final{5_m, 5_m, Rotation2d{45_deg}}; const auto transform = final - initial; - EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon); - EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon); + EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value()); + EXPECT_NEAR(0.0, transform.Y().value(), 1e-9); + EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value()); } diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp new file mode 100644 index 0000000000..46e4a28ae1 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/geometry/Pose3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(Pose3dTest, TransformBy) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45.0_deg}}; + const Transform3d transform{Translation3d{5_m, 0_m, 0_m}, + Rotation3d{zAxis, 5_deg}}; + + const auto transformed = initial + transform; + + EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value()); + EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value()); + EXPECT_DOUBLE_EQ(transformed.Rotation().Z().value(), + units::radian_t{50_deg}.value()); +} + +TEST(Pose3dTest, RelativeTo) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}}; + const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}}; + + const auto finalRelativeToInitial = final.RelativeTo(initial); + + EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value()); + EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Y().value()); + EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Z().value()); +} + +TEST(Pose3dTest, Equality) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}}; + const Pose3d b{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}}; + EXPECT_TRUE(a == b); +} + +TEST(Pose3dTest, Inequality) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}}; + const Pose3d b{0_m, 5_ft, 0_m, Rotation3d{zAxis, 43_deg}}; + EXPECT_TRUE(a != b); +} + +TEST(Pose3dTest, Minus) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}}; + const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}}; + + const auto transform = final - initial; + + EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value()); + EXPECT_DOUBLE_EQ(0.0, transform.Y().value()); + EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Z().value()); +} + +TEST(Pose3dTest, ToPose2d) { + Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}}; + Pose2d expected{1_m, 2_m, 40_deg}; + + EXPECT_EQ(expected, pose.ToPose2d()); +} diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp new file mode 100644 index 0000000000..3f235b78bd --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp @@ -0,0 +1,82 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/geometry/Quaternion.h" +#include "gtest/gtest.h" +#include "units/angle.h" +#include "units/math.h" + +using namespace frc; + +TEST(QuaternionTest, Init) { + // Identity + Quaternion q1; + EXPECT_DOUBLE_EQ(1.0, q1.W()); + EXPECT_DOUBLE_EQ(0.0, q1.X()); + EXPECT_DOUBLE_EQ(0.0, q1.Y()); + EXPECT_DOUBLE_EQ(0.0, q1.Z()); + + // Normalized + Quaternion q2{0.5, 0.5, 0.5, 0.5}; + EXPECT_DOUBLE_EQ(0.5, q2.W()); + EXPECT_DOUBLE_EQ(0.5, q2.X()); + EXPECT_DOUBLE_EQ(0.5, q2.Y()); + EXPECT_DOUBLE_EQ(0.5, q2.Z()); + + // Unnormalized + Quaternion q3{0.75, 0.3, 0.4, 0.5}; + EXPECT_DOUBLE_EQ(0.75, q3.W()); + EXPECT_DOUBLE_EQ(0.3, q3.X()); + EXPECT_DOUBLE_EQ(0.4, q3.Y()); + EXPECT_DOUBLE_EQ(0.5, q3.Z()); + + q3 = q3.Normalize(); + double norm = std::sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5); + EXPECT_DOUBLE_EQ(0.75 / norm, q3.W()); + EXPECT_DOUBLE_EQ(0.3 / norm, q3.X()); + EXPECT_DOUBLE_EQ(0.4 / norm, q3.Y()); + EXPECT_DOUBLE_EQ(0.5 / norm, q3.Z()); + EXPECT_DOUBLE_EQ(1.0, q3.W() * q3.W() + q3.X() * q3.X() + q3.Y() * q3.Y() + + q3.Z() * q3.Z()); +} + +TEST(QuaternionTest, Multiply) { + // 90° CCW rotations around each axis + double c = units::math::cos(90_deg / 2.0); + double s = units::math::sin(90_deg / 2.0); + Quaternion xRot{c, s, 0.0, 0.0}; + Quaternion yRot{c, 0.0, s, 0.0}; + Quaternion zRot{c, 0.0, 0.0, s}; + + // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should + // produce a 90° CCW Y rotation + auto expected = yRot; + auto actual = zRot * yRot * xRot; + EXPECT_NEAR(expected.W(), actual.W(), 1e-9); + EXPECT_NEAR(expected.X(), actual.X(), 1e-9); + EXPECT_NEAR(expected.Y(), actual.Y(), 1e-9); + EXPECT_NEAR(expected.Z(), actual.Z(), 1e-9); + + // Identity + Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276, + 0.48507125007266594}; + actual = q * q.Inverse(); + EXPECT_DOUBLE_EQ(1.0, actual.W()); + EXPECT_DOUBLE_EQ(0.0, actual.X()); + EXPECT_DOUBLE_EQ(0.0, actual.Y()); + EXPECT_DOUBLE_EQ(0.0, actual.Z()); +} + +TEST(QuaternionTest, Inverse) { + Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276, + 0.48507125007266594}; + auto inv = q.Inverse(); + + EXPECT_DOUBLE_EQ(q.W(), inv.W()); + EXPECT_DOUBLE_EQ(-q.X(), inv.X()); + EXPECT_DOUBLE_EQ(-q.Y(), inv.Y()); + EXPECT_DOUBLE_EQ(-q.Z(), inv.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index ed3b6b59a2..792dabba3a 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -11,58 +11,56 @@ using namespace frc; -static constexpr double kEpsilon = 1E-9; - TEST(Rotation2dTest, RadiansToDegrees) { - const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)}; - const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)}; + const Rotation2d rot1{units::radian_t{wpi::numbers::pi / 3.0}}; + const Rotation2d rot2{units::radian_t{wpi::numbers::pi / 4.0}}; - EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon); - EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon); + EXPECT_DOUBLE_EQ(60.0, rot1.Degrees().value()); + EXPECT_DOUBLE_EQ(45.0, rot2.Degrees().value()); } TEST(Rotation2dTest, DegreesToRadians) { - const auto rot1 = Rotation2d(45.0_deg); - const auto rot2 = Rotation2d(30.0_deg); + const auto rot1 = Rotation2d{45_deg}; + const auto rot2 = Rotation2d{30_deg}; - EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon); - EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot1.Radians().value()); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 6.0, rot2.Radians().value()); } TEST(Rotation2dTest, RotateByFromZero) { const Rotation2d zero; - auto sum = zero + Rotation2d(90.0_deg); + auto rotated = zero + Rotation2d(90_deg); - EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon); - EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rotated.Radians().value()); + EXPECT_DOUBLE_EQ(90.0, rotated.Degrees().value()); } TEST(Rotation2dTest, RotateByNonZero) { - auto rot = Rotation2d(90.0_deg); - rot = rot + Rotation2d(30.0_deg); + auto rot = Rotation2d{90_deg}; + rot = rot + Rotation2d{30_deg}; - EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon); + EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value()); } TEST(Rotation2dTest, Minus) { - const auto rot1 = Rotation2d(70.0_deg); - const auto rot2 = Rotation2d(30.0_deg); + const auto rot1 = Rotation2d{70_deg}; + const auto rot2 = Rotation2d{30_deg}; - EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon); + EXPECT_DOUBLE_EQ(40.0, (rot1 - rot2).Degrees().value()); } TEST(Rotation2dTest, Equality) { - const auto rot1 = Rotation2d(43_deg); - const auto rot2 = Rotation2d(43_deg); + auto rot1 = Rotation2d{43_deg}; + auto rot2 = Rotation2d{43_deg}; EXPECT_EQ(rot1, rot2); - const auto rot3 = Rotation2d(-180_deg); - const auto rot4 = Rotation2d(180_deg); - EXPECT_EQ(rot3, rot4); + rot1 = Rotation2d{-180_deg}; + rot2 = Rotation2d{180_deg}; + EXPECT_EQ(rot1, rot2); } TEST(Rotation2dTest, Inequality) { - const auto rot1 = Rotation2d(43_deg); - const auto rot2 = Rotation2d(43.5_deg); + const auto rot1 = Rotation2d{43_deg}; + const auto rot2 = Rotation2d{43.5_deg}; EXPECT_NE(rot1, rot2); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp new file mode 100644 index 0000000000..5fa8f9b7e9 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -0,0 +1,246 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include +#include + +#include "frc/geometry/Rotation3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(Rotation3dTest, Init) { + const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + const Rotation3d rot1{xAxis, units::radian_t{wpi::numbers::pi / 3}}; + const Rotation3d rot2{units::radian_t{wpi::numbers::pi / 3}, 0_rad, 0_rad}; + EXPECT_EQ(rot1, rot2); + + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + const Rotation3d rot3{yAxis, units::radian_t{wpi::numbers::pi / 3}}; + const Rotation3d rot4{0_rad, units::radian_t{wpi::numbers::pi / 3}, 0_rad}; + EXPECT_EQ(rot3, rot4); + + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + const Rotation3d rot5{zAxis, units::radian_t{wpi::numbers::pi / 3}}; + const Rotation3d rot6{0_rad, 0_rad, units::radian_t{wpi::numbers::pi / 3}}; + EXPECT_EQ(rot5, rot6); +} + +TEST(Rotation3dTest, RadiansToDegrees) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Rotation3d rot1{zAxis, units::radian_t{wpi::numbers::pi / 3}}; + EXPECT_DOUBLE_EQ(0.0, rot1.X().value()); + EXPECT_DOUBLE_EQ(0.0, rot1.Y().value()); + EXPECT_DOUBLE_EQ(units::radian_t{60_deg}.value(), rot1.Z().value()); + + const Rotation3d rot2{zAxis, units::radian_t{wpi::numbers::pi / 4}}; + EXPECT_DOUBLE_EQ(0.0, rot2.X().value()); + EXPECT_DOUBLE_EQ(0.0, rot2.Y().value()); + EXPECT_DOUBLE_EQ(units::radian_t{45_deg}.value(), rot2.Z().value()); +} + +TEST(Rotation3dTest, DegreesToRadians) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const auto rot1 = Rotation3d{zAxis, 45_deg}; + EXPECT_DOUBLE_EQ(0.0, rot1.X().value()); + EXPECT_DOUBLE_EQ(0.0, rot1.Y().value()); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot1.Z().value()); + + const auto rot2 = Rotation3d{zAxis, 30_deg}; + EXPECT_DOUBLE_EQ(0.0, rot2.X().value()); + EXPECT_DOUBLE_EQ(0.0, rot2.Y().value()); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 6.0, rot2.Z().value()); +} + +TEST(Rotation3dTest, RotationLoop) { + Rotation3d rot; + + rot = rot + Rotation3d{90_deg, 0_deg, 0_deg}; + Rotation3d expected{90_deg, 0_deg, 0_deg}; + EXPECT_EQ(expected, rot); + + rot = rot + Rotation3d{0_deg, 90_deg, 0_deg}; + expected = Rotation3d{ + {1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg}; + EXPECT_EQ(expected, rot); + + rot = rot + Rotation3d{0_deg, 0_deg, 90_deg}; + expected = Rotation3d{0_deg, 90_deg, 0_deg}; + EXPECT_EQ(expected, rot); + + rot = rot + Rotation3d{0_deg, -90_deg, 0_deg}; + EXPECT_EQ(Rotation3d{}, rot); +} + +TEST(Rotation3dTest, RotateByFromZeroX) { + const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + + const Rotation3d zero; + auto rotated = zero + Rotation3d{xAxis, 90_deg}; + + Rotation3d expected{xAxis, 90_deg}; + EXPECT_EQ(expected, rotated); +} + +TEST(Rotation3dTest, RotateByFromZeroY) { + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + + const Rotation3d zero; + auto rotated = zero + Rotation3d{yAxis, 90_deg}; + + Rotation3d expected{yAxis, 90_deg}; + EXPECT_EQ(expected, rotated); +} + +TEST(Rotation3dTest, RotateByFromZeroZ) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Rotation3d zero; + auto rotated = zero + Rotation3d{zAxis, 90_deg}; + + Rotation3d expected{zAxis, 90_deg}; + EXPECT_EQ(expected, rotated); +} + +TEST(Rotation3dTest, RotateByNonZeroX) { + const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + + auto rot = Rotation3d{xAxis, 90_deg}; + rot = rot + Rotation3d{xAxis, 30_deg}; + + Rotation3d expected{xAxis, 120_deg}; + EXPECT_EQ(expected, rot); +} + +TEST(Rotation3dTest, RotateByNonZeroY) { + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + + auto rot = Rotation3d{yAxis, 90_deg}; + rot = rot + Rotation3d{yAxis, 30_deg}; + + Rotation3d expected{yAxis, 120_deg}; + EXPECT_EQ(expected, rot); +} + +TEST(Rotation3dTest, RotateByNonZeroZ) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + auto rot = Rotation3d{zAxis, 90_deg}; + rot = rot + Rotation3d{zAxis, 30_deg}; + + Rotation3d expected{zAxis, 120_deg}; + EXPECT_EQ(expected, rot); +} + +TEST(Rotation3dTest, Minus) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const auto rot1 = Rotation3d{zAxis, 70_deg}; + const auto rot2 = Rotation3d{zAxis, 30_deg}; + + EXPECT_DOUBLE_EQ(40.0, units::degree_t{(rot1 - rot2).Z()}.value()); +} + +TEST(Rotation3dTest, AxisAngle) { + const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + Rotation3d rot1{xAxis, 90_deg}; + EXPECT_EQ(xAxis, rot1.Axis()); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rot1.Angle().value()); + + Rotation3d rot2{yAxis, 45_deg}; + EXPECT_EQ(yAxis, rot2.Axis()); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 4.0, rot2.Angle().value()); + + Rotation3d rot3{zAxis, 60_deg}; + EXPECT_EQ(zAxis, rot3.Axis()); + EXPECT_DOUBLE_EQ(wpi::numbers::pi / 3.0, rot3.Angle().value()); +} + +TEST(Rotation3dTest, ToRotation2d) { + Rotation3d rotation{20_deg, 30_deg, 40_deg}; + Rotation2d expected{40_deg}; + + EXPECT_EQ(expected, rotation.ToRotation2d()); +} + +TEST(Rotation3dTest, Equality) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const auto rot1 = Rotation3d{zAxis, 43_deg}; + const auto rot2 = Rotation3d{zAxis, 43_deg}; + EXPECT_EQ(rot1, rot2); + + const auto rot3 = Rotation3d{zAxis, -180_deg}; + const auto rot4 = Rotation3d{zAxis, 180_deg}; + EXPECT_EQ(rot3, rot4); +} + +TEST(Rotation3dTest, Inequality) { + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const auto rot1 = Rotation3d{zAxis, 43_deg}; + const auto rot2 = Rotation3d{zAxis, 43.5_deg}; + EXPECT_NE(rot1, rot2); +} + +TEST(Rotation3dTest, Interpolate) { + const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + // 50 + (70 - 50) * 0.5 = 60 + auto rot1 = Rotation3d{xAxis, 50_deg}; + auto rot2 = Rotation3d{xAxis, 70_deg}; + auto interpolated = wpi::Lerp(rot1, rot2, 0.5); + EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.X()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value()); + + // -160 minus half distance between 170 and -160 (15) = -175 + rot1 = Rotation3d{xAxis, 170_deg}; + rot2 = Rotation3d{xAxis, -160_deg}; + interpolated = wpi::Lerp(rot1, rot2, 0.5); + EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.X()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value()); + + // 50 + (70 - 50) * 0.5 = 60 + rot1 = Rotation3d{yAxis, 50_deg}; + rot2 = Rotation3d{yAxis, 70_deg}; + interpolated = wpi::Lerp(rot1, rot2, 0.5); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value()); + EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Y()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value()); + + // -160 plus half distance between 170 and -160 (165) = 5 + rot1 = Rotation3d{yAxis, 170_deg}; + rot2 = Rotation3d{yAxis, -160_deg}; + interpolated = wpi::Lerp(rot1, rot2, 0.5); + EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.X()}.value()); + EXPECT_DOUBLE_EQ(-5.0, units::degree_t{interpolated.Y()}.value()); + EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.Z()}.value()); + + // 50 + (70 - 50) * 0.5 = 60 + rot1 = Rotation3d{zAxis, 50_deg}; + rot2 = Rotation3d{zAxis, 70_deg}; + interpolated = wpi::Lerp(rot1, rot2, 0.5); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value()); + EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Z()}.value()); + + // -160 minus half distance between 170 and -160 (15) = -175 + rot1 = Rotation3d{zAxis, 170_deg}; + rot2 = Rotation3d{zAxis, -160_deg}; + interpolated = wpi::Lerp(rot1, rot2, 0.5); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value()); + EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value()); + EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 968ab29edf..161378c713 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -12,8 +12,6 @@ using namespace frc; -static constexpr double kEpsilon = 1E-9; - TEST(Transform2dTest, Inverse) { const Pose2d initial{1_m, 2_m, 45_deg}; const Transform2d transform{{5_m, 0_m}, 5_deg}; @@ -21,10 +19,10 @@ TEST(Transform2dTest, Inverse) { auto transformed = initial + transform; auto untransformed = transformed + transform.Inverse(); - EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon); - EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon); - EXPECT_NEAR(initial.Rotation().Degrees().value(), - untransformed.Rotation().Degrees().value(), kEpsilon); + EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9); + EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value()); + EXPECT_DOUBLE_EQ(initial.Rotation().Degrees().value(), + untransformed.Rotation().Degrees().value()); } TEST(Transform2dTest, Composition) { @@ -35,10 +33,10 @@ TEST(Transform2dTest, Composition) { auto transformedSeparate = initial + transform1 + transform2; auto transformedCombined = initial + (transform1 + transform2); - EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(), - kEpsilon); - EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(), - kEpsilon); - EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(), - transformedCombined.Rotation().Degrees().value(), kEpsilon); + EXPECT_DOUBLE_EQ(transformedSeparate.X().value(), + transformedCombined.X().value()); + EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(), + transformedCombined.Y().value()); + EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Degrees().value(), + transformedCombined.Rotation().Degrees().value()); } diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp new file mode 100644 index 0000000000..7361096d7f --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/geometry/Transform3d.h" +#include "frc/geometry/Translation3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(Transform3dTest, Inverse) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}}; + const Transform3d transform{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}}; + + auto transformed = initial + transform; + auto untransformed = transformed + transform.Inverse(); + + EXPECT_NEAR(initial.X().value(), untransformed.X().value(), 1e-9); + EXPECT_DOUBLE_EQ(initial.Y().value(), untransformed.Y().value()); + EXPECT_DOUBLE_EQ(initial.Z().value(), untransformed.Z().value()); + EXPECT_DOUBLE_EQ(initial.Rotation().Z().value(), + untransformed.Rotation().Z().value()); +} + +TEST(Transform3dTest, Composition) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45_deg}}; + const Transform3d transform1{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}}; + const Transform3d transform2{{0_m, 2_m, 0_m}, Rotation3d{zAxis, 5_deg}}; + + auto transformedSeparate = initial + transform1 + transform2; + auto transformedCombined = initial + (transform1 + transform2); + + EXPECT_DOUBLE_EQ(transformedSeparate.X().value(), + transformedCombined.X().value()); + EXPECT_DOUBLE_EQ(transformedSeparate.Y().value(), + transformedCombined.Y().value()); + EXPECT_DOUBLE_EQ(transformedSeparate.Z().value(), + transformedCombined.Z().value()); + EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Z().value(), + transformedCombined.Rotation().Z().value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index efdcace525..148c528559 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -9,69 +9,67 @@ using namespace frc; -static constexpr double kEpsilon = 1E-9; - TEST(Translation2dTest, Sum) { - const Translation2d one{1.0_m, 3.0_m}; - const Translation2d two{2.0_m, 5.0_m}; + const Translation2d one{1_m, 3_m}; + const Translation2d two{2_m, 5_m}; const auto sum = one + two; - EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon); - EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon); + EXPECT_DOUBLE_EQ(3.0, sum.X().value()); + EXPECT_DOUBLE_EQ(8.0, sum.Y().value()); } TEST(Translation2dTest, Difference) { - const Translation2d one{1.0_m, 3.0_m}; - const Translation2d two{2.0_m, 5.0_m}; + const Translation2d one{1_m, 3_m}; + const Translation2d two{2_m, 5_m}; const auto difference = one - two; - EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon); - EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon); + EXPECT_DOUBLE_EQ(-1.0, difference.X().value()); + EXPECT_DOUBLE_EQ(-2.0, difference.Y().value()); } TEST(Translation2dTest, RotateBy) { - const Translation2d another{3.0_m, 0.0_m}; - const auto rotated = another.RotateBy(Rotation2d(90.0_deg)); + const Translation2d another{3_m, 0_m}; + const auto rotated = another.RotateBy(Rotation2d(90_deg)); - EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon); - EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon); + EXPECT_NEAR(0.0, rotated.X().value(), 1e-9); + EXPECT_DOUBLE_EQ(3.0, rotated.Y().value()); } TEST(Translation2dTest, Multiplication) { - const Translation2d original{3.0_m, 5.0_m}; + const Translation2d original{3_m, 5_m}; const auto mult = original * 3; - EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon); - EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon); + EXPECT_DOUBLE_EQ(9.0, mult.X().value()); + EXPECT_DOUBLE_EQ(15.0, mult.Y().value()); } TEST(Translation2dTest, Division) { - const Translation2d original{3.0_m, 5.0_m}; + const Translation2d original{3_m, 5_m}; const auto div = original / 2; - EXPECT_NEAR(div.X().value(), 1.5, kEpsilon); - EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon); + EXPECT_DOUBLE_EQ(1.5, div.X().value()); + EXPECT_DOUBLE_EQ(2.5, div.Y().value()); } TEST(Translation2dTest, Norm) { - const Translation2d one{3.0_m, 5.0_m}; - EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon); + const Translation2d one{3_m, 5_m}; + EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value()); } TEST(Translation2dTest, Distance) { const Translation2d one{1_m, 1_m}; const Translation2d two{6_m, 6_m}; - EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon); + EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value()); } TEST(Translation2dTest, UnaryMinus) { const Translation2d original{-4.5_m, 7_m}; const auto inverted = -original; - EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon); - EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon); + EXPECT_DOUBLE_EQ(4.5, inverted.X().value()); + EXPECT_DOUBLE_EQ(-7.0, inverted.Y().value()); } TEST(Translation2dTest, Equality) { @@ -87,11 +85,11 @@ TEST(Translation2dTest, Inequality) { } TEST(Translation2dTest, PolarConstructor) { - Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)}; - EXPECT_NEAR(one.X().value(), 1.0, kEpsilon); - EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon); + Translation2d one{std::sqrt(2) * 1_m, Rotation2d{45_deg}}; + EXPECT_DOUBLE_EQ(1.0, one.X().value()); + EXPECT_DOUBLE_EQ(1.0, one.Y().value()); - Translation2d two{2_m, Rotation2d(60_deg)}; - EXPECT_NEAR(two.X().value(), 1.0, kEpsilon); - EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon); + Translation2d two{2_m, Rotation2d{60_deg}}; + EXPECT_DOUBLE_EQ(1.0, two.X().value()); + EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value()); } diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp new file mode 100644 index 0000000000..32c5d68b9d --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp @@ -0,0 +1,128 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/geometry/Translation3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(Translation3dTest, Sum) { + const Translation3d one{1_m, 3_m, 5_m}; + const Translation3d two{2_m, 5_m, 8_m}; + + const auto sum = one + two; + + EXPECT_NEAR(3.0, sum.X().value(), kEpsilon); + EXPECT_NEAR(8.0, sum.Y().value(), kEpsilon); + EXPECT_NEAR(13.0, sum.Z().value(), kEpsilon); +} + +TEST(Translation3dTest, Difference) { + const Translation3d one{1_m, 3_m, 5_m}; + const Translation3d two{2_m, 5_m, 8_m}; + + const auto difference = one - two; + + EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon); + EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon); + EXPECT_NEAR(difference.Z().value(), -3.0, kEpsilon); +} + +TEST(Translation3dTest, RotateBy) { + Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Translation3d translation{1_m, 2_m, 3_m}; + + const auto rotated1 = translation.RotateBy(Rotation3d{xAxis, 90_deg}); + EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon); + EXPECT_NEAR(rotated1.Y().value(), -3.0, kEpsilon); + EXPECT_NEAR(rotated1.Z().value(), 2.0, kEpsilon); + + const auto rotated2 = translation.RotateBy(Rotation3d{yAxis, 90_deg}); + EXPECT_NEAR(rotated2.X().value(), 3.0, kEpsilon); + EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon); + EXPECT_NEAR(rotated2.Z().value(), -1.0, kEpsilon); + + const auto rotated3 = translation.RotateBy(Rotation3d{zAxis, 90_deg}); + EXPECT_NEAR(rotated3.X().value(), -2.0, kEpsilon); + EXPECT_NEAR(rotated3.Y().value(), 1.0, kEpsilon); + EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon); +} + +TEST(Translation3dTest, ToTranslation2d) { + Translation3d translation{1_m, 2_m, 3_m}; + Translation2d expected{1_m, 2_m}; + + EXPECT_EQ(expected, translation.ToTranslation2d()); +} + +TEST(Translation3dTest, Multiplication) { + const Translation3d original{3_m, 5_m, 7_m}; + const auto mult = original * 3; + + EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon); + EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon); + EXPECT_NEAR(mult.Z().value(), 21.0, kEpsilon); +} + +TEST(Translation3dTest, Division) { + const Translation3d original{3_m, 5_m, 7_m}; + const auto div = original / 2; + + EXPECT_NEAR(div.X().value(), 1.5, kEpsilon); + EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon); + EXPECT_NEAR(div.Z().value(), 3.5, kEpsilon); +} + +TEST(Translation3dTest, Norm) { + const Translation3d one{3_m, 5_m, 7_m}; + EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon); +} + +TEST(Translation3dTest, Distance) { + const Translation3d one{1_m, 1_m, 1_m}; + const Translation3d two{6_m, 6_m, 6_m}; + EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon); +} + +TEST(Translation3dTest, UnaryMinus) { + const Translation3d original{-4.5_m, 7_m, 9_m}; + const auto inverted = -original; + + EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon); + EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon); + EXPECT_NEAR(inverted.Z().value(), -9, kEpsilon); +} + +TEST(Translation3dTest, Equality) { + const Translation3d one{9_m, 5.5_m, 3.5_m}; + const Translation3d two{9_m, 5.5_m, 3.5_m}; + EXPECT_TRUE(one == two); +} + +TEST(Translation3dTest, Inequality) { + const Translation3d one{9_m, 5.5_m, 3.5_m}; + const Translation3d two{9_m, 5.7_m, 3.5_m}; + EXPECT_TRUE(one != two); +} + +TEST(Translation3dTest, PolarConstructor) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + Translation3d one{std::sqrt(2) * 1_m, Rotation3d(zAxis, 45_deg)}; + EXPECT_NEAR(one.X().value(), 1.0, kEpsilon); + EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon); + EXPECT_NEAR(one.Z().value(), 0.0, kEpsilon); + + Translation3d two{2_m, Rotation3d(zAxis, 60_deg)}; + EXPECT_NEAR(two.X().value(), 1.0, kEpsilon); + EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon); + EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon); +} diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp index fa9eecce9d..d728dbfbc2 100644 --- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -11,55 +11,57 @@ using namespace frc; -static constexpr double kEpsilon = 1E-9; - TEST(Twist2dTest, Straight) { - const Twist2d straight{5.0_m, 0.0_m, 0.0_rad}; + const Twist2d straight{5_m, 0_m, 0_rad}; const auto straightPose = Pose2d().Exp(straight); - EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon); - EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon); - EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon); + EXPECT_DOUBLE_EQ(5.0, straightPose.X().value()); + EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value()); + EXPECT_DOUBLE_EQ(0.0, straightPose.Rotation().Radians().value()); } TEST(Twist2dTest, QuarterCircle) { - const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m, - units::radian_t(wpi::numbers::pi / 2.0)}; - const auto quarterCirclePose = Pose2d().Exp(quarterCircle); + const Twist2d quarterCircle{5_m / 2.0 * wpi::numbers::pi, 0_m, + units::radian_t{wpi::numbers::pi / 2.0}}; + const auto quarterCirclePose = Pose2d{}.Exp(quarterCircle); - EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon); - EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon); - EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon); + EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value()); + EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.Y().value()); + EXPECT_DOUBLE_EQ(90.0, quarterCirclePose.Rotation().Degrees().value()); } TEST(Twist2dTest, DiagonalNoDtheta) { - const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg}; - const auto diagonalPose = Pose2d().Exp(diagonal); + const Twist2d diagonal{2_m, 2_m, 0_deg}; + const auto diagonalPose = Pose2d{}.Exp(diagonal); - EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon); - EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon); - EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon); + EXPECT_DOUBLE_EQ(2.0, diagonalPose.X().value()); + EXPECT_DOUBLE_EQ(2.0, diagonalPose.Y().value()); + EXPECT_DOUBLE_EQ(0.0, diagonalPose.Rotation().Degrees().value()); } TEST(Twist2dTest, Equality) { - const Twist2d one{5.0_m, 1.0_m, 3.0_rad}; - const Twist2d two{5.0_m, 1.0_m, 3.0_rad}; + const Twist2d one{5_m, 1_m, 3_rad}; + const Twist2d two{5_m, 1_m, 3_rad}; EXPECT_TRUE(one == two); } TEST(Twist2dTest, Inequality) { - const Twist2d one{5.0_m, 1.0_m, 3.0_rad}; - const Twist2d two{5.0_m, 1.2_m, 3.0_rad}; + const Twist2d one{5_m, 1_m, 3_rad}; + const Twist2d two{5_m, 1.2_m, 3_rad}; EXPECT_TRUE(one != two); } TEST(Twist2dTest, Pose2dLog) { - const Pose2d end{5_m, 5_m, Rotation2d(90_deg)}; - const Pose2d start{}; + const Pose2d end{5_m, 5_m, Rotation2d{90_deg}}; + const Pose2d start; const auto twist = start.Log(end); - EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon); - EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon); - EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon); + Twist2d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, 0_m, + units::radian_t{wpi::numbers::pi / 2.0}}; + EXPECT_EQ(expected, twist); + + // Make sure computed twist gives back original end pose + const auto reapplied = start.Exp(twist); + EXPECT_EQ(end, reapplied); } diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp new file mode 100644 index 0000000000..ba22ebfb79 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp @@ -0,0 +1,118 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "frc/geometry/Pose3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(Twist3dTest, StraightX) { + const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad}; + const auto straightPose = Pose3d().Exp(straight); + + Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}}; + EXPECT_EQ(expected, straightPose); +} + +TEST(Twist3dTest, StraightY) { + const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad}; + const auto straightPose = Pose3d().Exp(straight); + + Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}}; + EXPECT_EQ(expected, straightPose); +} + +TEST(Twist3dTest, StraightZ) { + const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad}; + const auto straightPose = Pose3d().Exp(straight); + + Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}}; + EXPECT_EQ(expected, straightPose); +} + +TEST(Twist3dTest, QuarterCircle) { + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Twist3d quarterCircle{ + 5_m / 2.0 * wpi::numbers::pi, 0_m, 0_m, 0_rad, 0_rad, + units::radian_t(wpi::numbers::pi / 2.0)}; + const auto quarterCirclePose = Pose3d().Exp(quarterCircle); + + Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}}; + EXPECT_EQ(expected, quarterCirclePose); +} + +TEST(Twist3dTest, DiagonalNoDtheta) { + const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad}; + const auto diagonalPose = Pose3d().Exp(diagonal); + + Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}}; + EXPECT_EQ(expected, diagonalPose); +} + +TEST(Twist3dTest, Equality) { + const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad}; + const Twist3d two{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad}; + EXPECT_TRUE(one == two); +} + +TEST(Twist3dTest, Inequality) { + const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad}; + const Twist3d two{5_m, 1.2_m, 0_m, 0_rad, 0_rad, 3_rad}; + EXPECT_TRUE(one != two); +} + +TEST(Twist3dTest, Pose3dLogX) { + const Pose3d end{0_m, 5_m, 5_m, Rotation3d{90_deg, 0_deg, 0_deg}}; + const Pose3d start; + + const auto twist = start.Log(end); + + Twist3d expected{0_m, units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, + 0_m, 90_deg, + 0_deg, 0_deg}; + EXPECT_EQ(expected, twist); + + // Make sure computed twist gives back original end pose + const auto reapplied = start.Exp(twist); + EXPECT_EQ(end, reapplied); +} + +TEST(Twist3dTest, Pose3dLogY) { + const Pose3d end{5_m, 0_m, 5_m, Rotation3d{0_deg, 90_deg, 0_deg}}; + const Pose3d start; + + const auto twist = start.Log(end); + + Twist3d expected{0_m, 0_m, units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, + 0_deg, 90_deg, 0_deg}; + EXPECT_EQ(expected, twist); + + // Make sure computed twist gives back original end pose + const auto reapplied = start.Exp(twist); + EXPECT_EQ(end, reapplied); +} + +TEST(Twist3dTest, Pose3dLogZ) { + const Pose3d end{5_m, 5_m, 0_m, Rotation3d{0_deg, 0_deg, 90_deg}}; + const Pose3d start; + + const auto twist = start.Log(end); + + Twist3d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, + 0_m, + 0_m, + 0_deg, + 0_deg, + 90_deg}; + EXPECT_EQ(expected, twist); + + // Make sure computed twist gives back original end pose + const auto reapplied = start.Exp(twist); + EXPECT_EQ(end, reapplied); +}