diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java index 9e827c3e48..95788c7798 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java @@ -16,7 +16,7 @@ public class CoordinateSystem { private static final CoordinateSystem m_ned = new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D()); - // Rotation from this coordinate system to NWU coordinate system + // Rotation from this coordinate system to NWU coordinate system when applied extrinsically private final Rotation3d m_rotation; /** @@ -85,7 +85,8 @@ public class CoordinateSystem { */ public static Translation3d convert( Translation3d translation, CoordinateSystem from, CoordinateSystem to) { - return translation.rotateBy(from.m_rotation.minus(to.m_rotation)); + // Convert to NWU, then convert to the new coordinate system + return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus()); } /** @@ -98,7 +99,8 @@ public class CoordinateSystem { */ public static Rotation3d convert( Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) { - return rotation.rotateBy(from.m_rotation.minus(to.m_rotation)); + // Convert to NWU, then convert to the new coordinate system + return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus()); } /** @@ -124,9 +126,23 @@ public class CoordinateSystem { */ public static Transform3d convert( Transform3d transform, CoordinateSystem from, CoordinateSystem to) { - var coordRot = from.m_rotation.minus(to.m_rotation); + // coordRot is the rotation that converts between the coordinate systems when applied + // extrinsically. It first converts to NWU, then converts to the new coordinate system. + var coordRot = from.m_rotation.rotateBy(to.m_rotation.unaryMinus()); + // The new rotation is the extrinsic rotation from convert(zero) to + // convert(transformRot). That is, applying convertedRot extrinsically to + // convert(zero) produces convert(transformRot). In the below snippet, we + // use matrix notation, so rotA rotB applies rotA extrinsically to rotB. + // + // convertedRot convert(zero) = convert(transformRot) + // convertedRot = convert(transformRot) convert(zero)⁻¹ + // = (coordRot transformRot) (coordRot zero)⁻¹ + // = (coordRot transformRot) coordRot⁻¹ + // + // In code, the equivalent for rotA rotB is rotB.rotateBy(rotA) (note the + // change in order), and the equivalent for rot⁻¹ is rot.unaryMinus(). return new Transform3d( convert(transform.getTranslation(), from, to), - coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot))); + coordRot.unaryMinus().rotateBy(transform.getRotation().rotateBy(coordRot))); } } 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 4a84ef6543..4b80cc4801 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 @@ -220,7 +220,7 @@ public class Pose2d implements Interpolatable, ProtobufSerializable, Str public Pose2d transformBy(Transform2d other) { return new Pose2d( m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - other.getRotation().plus(m_rotation)); + other.getRotation().rotateBy(m_rotation)); } /** 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 index 20204235bc..ca3a8ebab5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -254,9 +254,12 @@ public class Pose3d implements Interpolatable, ProtobufSerializable, Str * @return The transformed pose. */ public Pose3d transformBy(Transform3d other) { + // Rotating the transform's rotation by the pose's rotation extrinsically is equivalent to + // rotating the pose's rotation by the transform's rotation intrinsically. (We define transforms + // as being applied intrinsically.) return new Pose3d( m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - other.getRotation().plus(m_rotation)); + other.getRotation().rotateBy(m_rotation)); } /** 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 2d4206b8c6..01ad00cb1b 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 @@ -275,6 +275,17 @@ public class Rotation2d m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); } + /** + * Returns the current rotation relative to the given rotation. + * + * @param other The rotation describing the orientation of the new coordinate frame that the + * current rotation will be converted into. + * @return The current rotation relative to the new orientation of the coordinate frame. + */ + public Rotation2d relativeTo(Rotation2d other) { + return rotateBy(other.unaryMinus()); + } + /** * Returns matrix representation of this rotation. * 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 index 5d3f479a97..46a258e117 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -26,7 +26,49 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; -/** A rotation in a 3D coordinate frame represented by a quaternion. */ +/** + * A rotation in a 3D coordinate frame, represented by a quaternion. Note that unlike 2D rotations, + * 3D rotations are not always commutative, so changing the order of rotations changes the result. + * + *

As an example of the order of rotations mattering, suppose we have a card that looks like + * this: + * + * 

+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ *
+ * + *

If we rotate 90º clockwise around the axis into the page, then flip around the left/right + * axis, we get one result: + * + * 

+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ *
+ * + *

If we flip around the left/right axis, then rotate 90º clockwise around the axis into the + * page, we get a different result: + * + * 

+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ *
+ * + *

Because order matters for 3D rotations, we need to distinguish between extrinsic + * rotations and intrinsic rotations. Rotating extrinsically means rotating around the + * global axes, while rotating intrinsically means rotating from the perspective of the other + * rotation. A neat property is that applying a series of rotations extrinsically is the same as + * applying the same series in the opposite order intrinsically. + */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation3d @@ -286,9 +328,17 @@ public class Rotation3d } /** - * Adds two rotations together. + * Adds two rotations together. The other rotation is applied extrinsically to this rotation, + * which is equivalent to this rotation being applied intrinsically to the other rotation. See the + * class comment for definitions of extrinsic and intrinsic rotations. * - * @param other The rotation to add. + *

Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))} + * sometimes doesn't. To apply a rotation offset, use either {@code offset = + * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset = + * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected + * angle needs to change as the input angle changes. + * + * @param other The rotation to add (applied extrinsically). * @return The sum of the two rotations. */ public Rotation3d plus(Rotation3d other) { @@ -296,10 +346,19 @@ public class Rotation3d } /** - * Subtracts the new rotation from the current rotation and returns the new rotation. + * Subtracts the other rotation from the current rotation and returns the new rotation. The new + * rotation is from the perspective of the other rotation (like {@link Pose3d#minus}), so it needs + * to be applied intrinsically. See the class comment for definitions of extrinsic and intrinsic + * rotations. + * + *

Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))} + * sometimes doesn't. To apply a rotation offset, use either {@code offset = + * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset = + * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected + * angle needs to change as the input angle changes. * * @param other The rotation to subtract. - * @return The difference between the two rotations. + * @return The difference between the two rotations, from the perspective of the other rotation. */ public Rotation3d minus(Rotation3d other) { return rotateBy(other.unaryMinus()); @@ -358,6 +417,24 @@ public class Rotation3d return new Rotation3d(other.m_q.times(m_q)); } + /** + * Returns the current rotation relative to the given rotation. Because the result is in the + * perspective of the given rotation, it must be applied intrinsically. See the class comment for + * definitions of extrinsic and intrinsic rotations. + * + * @param other The rotation describing the orientation of the new coordinate frame that the + * current rotation will be converted into. + * @return The current rotation relative to the new orientation of the coordinate frame. + */ + public Rotation3d relativeTo(Rotation3d other) { + // To apply a quaternion intrinsically, we must right-multiply by that quaternion. + // Therefore, "this_q relative to other_q" is the q such that other_q q = this_q: + // + // other_q q = this_q + // q = other_q⁻¹ this_q + return new Rotation3d(other.m_q.inverse().times(m_q)); + } + /** * Returns the quaternion representation of the Rotation3d. * @@ -562,7 +639,7 @@ public class Rotation3d @Override public Rotation3d interpolate(Rotation3d endValue, double t) { - return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); + return endValue.minus(this).times(MathUtil.clamp(t, 0, 1)).plus(this); } /** Rotation3d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 7a4cf8f4f9..79e8955220 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -36,15 +36,14 @@ public class Transform2d implements ProtobufSerializable, StructSerializable { * @param last The final pose for the transformation. */ public Transform2d(Pose2d initial, Pose2d last) { - // We are rotating the difference between the translations - // using a clockwise rotation matrix. This transforms the global - // delta into a local delta (relative to the initial pose). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = last.getTranslation() .minus(initial.getTranslation()) .rotateBy(initial.getRotation().unaryMinus()); - m_rotation = last.getRotation().minus(initial.getRotation()); + m_rotation = last.getRotation().relativeTo(initial.getRotation()); } /** 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 index d993d86100..67a4ba6173 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -17,7 +17,10 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; -/** Represents a transformation for a Pose3d in the pose's frame. */ +/** + * Represents a transformation for a Pose3d in the pose's frame. Translation is applied before + * rotation. (The translation is applied in the pose's original frame, not the transformed frame.) + */ public class Transform3d implements ProtobufSerializable, StructSerializable { /** * A preallocated Transform3d representing no transformation. @@ -36,15 +39,14 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { * @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). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = last.getTranslation() .minus(initial.getTranslation()) .rotateBy(initial.getRotation().unaryMinus()); - m_rotation = last.getRotation().minus(initial.getRotation()); + m_rotation = last.getRotation().relativeTo(initial.getRotation()); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java index 6a09f92bff..acc72ea55f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -24,7 +24,10 @@ public class Odometry { private Pose2d m_poseMeters; private Rotation2d m_gyroOffset; + + // Always equal to m_poseMeters.getRotation() private Rotation2d m_previousAngle; + private final T m_previousWheelPositions; /** @@ -42,7 +45,7 @@ public class Odometry { Pose2d initialPoseMeters) { m_kinematics = kinematics; m_poseMeters = initialPoseMeters; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation()); m_previousAngle = m_poseMeters.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -60,7 +63,7 @@ public class Odometry { public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { m_poseMeters = poseMeters; m_previousAngle = m_poseMeters.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation()); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } @@ -70,7 +73,10 @@ public class Odometry { * @param poseMeters The pose to reset to. */ public void resetPose(Pose2d poseMeters) { - m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); + m_gyroOffset = + m_gyroOffset + .rotateBy(m_poseMeters.getRotation().unaryMinus()) + .rotateBy(poseMeters.getRotation()); m_poseMeters = poseMeters; m_previousAngle = m_poseMeters.getRotation(); } @@ -90,7 +96,8 @@ public class Odometry { * @param rotation The rotation to reset to. */ public void resetRotation(Rotation2d rotation) { - m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); + m_gyroOffset = + m_gyroOffset.rotateBy(m_poseMeters.getRotation().unaryMinus()).rotateBy(rotation); m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation); m_previousAngle = m_poseMeters.getRotation(); } @@ -115,7 +122,7 @@ public class Odometry { * @return The new pose of the robot. */ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { - var angle = gyroAngle.plus(m_gyroOffset); + var angle = gyroAngle.rotateBy(m_gyroOffset); var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); twist.dtheta = angle.minus(m_previousAngle).getRadians(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java index f79403ef92..9e5122731b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -32,8 +32,15 @@ public class Odometry3d { private final Kinematics m_kinematics; private Pose3d m_poseMeters; + // Applying a rotation intrinsically to the measured gyro angle should cause the corrected angle + // to be rotated intrinsically in the same way, so the measured gyro angle must be applied + // intrinsically. This is equivalent to applying the offset extrinsically to the measured gyro + // angle. private Rotation3d m_gyroOffset; + + // Always equal to m_poseMeters.getRotation() private Rotation3d m_previousAngle; + private final T m_previousWheelPositions; /** @@ -51,7 +58,9 @@ public class Odometry3d { Pose3d initialPoseMeters) { m_kinematics = kinematics; m_poseMeters = initialPoseMeters; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and + // then rotates to m_poseMeters.getRotation() + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation()); m_previousAngle = m_poseMeters.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -69,7 +78,9 @@ public class Odometry3d { public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { m_poseMeters = poseMeters; m_previousAngle = m_poseMeters.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and + // then rotates to m_poseMeters.getRotation() + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation()); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } @@ -79,7 +90,11 @@ public class Odometry3d { * @param poseMeters The pose to reset to. */ public void resetPose(Pose3d poseMeters) { - m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = + m_gyroOffset + .rotateBy(m_poseMeters.getRotation().unaryMinus()) + .rotateBy(poseMeters.getRotation()); m_poseMeters = poseMeters; m_previousAngle = m_poseMeters.getRotation(); } @@ -99,7 +114,9 @@ public class Odometry3d { * @param rotation The rotation to reset to. */ public void resetRotation(Rotation3d rotation) { - m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = + m_gyroOffset.rotateBy(m_poseMeters.getRotation().unaryMinus()).rotateBy(rotation); m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation); m_previousAngle = m_poseMeters.getRotation(); } @@ -124,7 +141,7 @@ public class Odometry3d { * @return The new pose of the robot. */ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { - var angle = gyroAngle.plus(m_gyroOffset); + var angle = gyroAngle.rotateBy(m_gyroOffset); var angle_difference = angle.minus(m_previousAngle).toVector(); var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h index 0d4ba8f6c8..eff347f10d 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h +++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h @@ -86,7 +86,8 @@ class WPILIB_DLLEXPORT CoordinateSystem { constexpr static Translation3d Convert(const Translation3d& translation, const CoordinateSystem& from, const CoordinateSystem& to) { - return translation.RotateBy(from.m_rotation - to.m_rotation); + // Convert to NWU, then convert to the new coordinate system + return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation); } /** @@ -100,7 +101,8 @@ class WPILIB_DLLEXPORT CoordinateSystem { constexpr static Rotation3d Convert(const Rotation3d& rotation, const CoordinateSystem& from, const CoordinateSystem& to) { - return rotation.RotateBy(from.m_rotation - to.m_rotation); + // Convert to NWU, then convert to the new coordinate system + return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation); } /** @@ -129,14 +131,30 @@ class WPILIB_DLLEXPORT CoordinateSystem { constexpr static Transform3d Convert(const Transform3d& transform, const CoordinateSystem& from, const CoordinateSystem& to) { - const auto coordRot = from.m_rotation - to.m_rotation; + // coordRot is the rotation that converts between the coordinate systems + // when applied extrinsically. It first converts to NWU, then converts to + // the new coordinate system. + const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation); + // The new rotation is the extrinsic rotation from convert(zero) to + // convert(transformRot). That is, applying convertedRot extrinsically to + // convert(zero) produces convert(transformRot). In the below snippet, we + // use matrix notation, so rotA rotB applies rotA extrinsically to rotB. + // + // convertedRot convert(zero) = convert(transformRot) + // convertedRot = convert(transformRot) convert(zero)⁻¹ + // = (coordRot transformRot) (coordRot zero)⁻¹ + // = (coordRot transformRot) coordRot⁻¹ + // + // In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the + // change in order), and the equivalent for rot⁻¹ is -rot. return Transform3d{ Convert(transform.Translation(), from, to), (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))}; } private: - // Rotation from this coordinate system to NWU coordinate system + // Rotation from this coordinate system to NWU coordinate system when applied + // extrinsically Rotation3d m_rotation; }; diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index d488e312a7..ea0a1e6767 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -320,7 +320,7 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const { constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const { return {m_translation + (other.Translation().RotateBy(m_rotation)), - other.Rotation() + m_rotation}; + other.Rotation().RotateBy(m_rotation)}; } constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const { diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 17aac42648..fb1eb96a76 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -392,8 +392,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const { } constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const { + // Rotating the transform's rotation by the pose's rotation extrinsically is + // equivalent to rotating the pose's rotation by the transform's rotation + // intrinsically. (We define transforms as being applied intrinsically.) return {m_translation + (other.Translation().RotateBy(m_rotation)), - other.Rotation() + m_rotation}; + other.Rotation().RotateBy(m_rotation)}; } constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const { diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 82d9e39d78..61e39b148f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -198,6 +198,19 @@ class WPILIB_DLLEXPORT Rotation2d { Cos() * other.Sin() + Sin() * other.Cos()}; } + /** + * Returns the current rotation relative to the given rotation. + * + * @param other The rotation describing the orientation of the new coordinate + * frame that the current rotation will be converted into. + * + * @return The current rotation relative to the new orientation of the + * coordinate frame. + */ + constexpr Rotation2d RelativeTo(const Rotation2d& other) const { + return RotateBy(-other); + } + /** * Returns matrix representation of this rotation. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 8d1e975299..d972e85636 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -24,7 +25,49 @@ namespace frc { /** - * A rotation in a 3D coordinate frame represented by a quaternion. + * A rotation in a 3D coordinate frame, represented by a quaternion. Note that + * unlike 2D rotations, 3D rotations are not always commutative, so changing the + * order of rotations changes the result. + * + * As an example of the order of rotations mattering, suppose we have a card + * that looks like this: + * + * 

+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ *
+ * + * If we rotate 90º clockwise around the axis into the page, then flip around + * the left/right axis, we get one result: + * + * 
+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ *
+ * + * If we flip around the left/right axis, then rotate 90º clockwise around the + * axis into the page, we get a different result: + * + * 
+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ *
+ * + * Because order matters for 3D rotations, we need to distinguish between + * extrinsic rotations and intrinsic rotations. Rotating + * extrinsically means rotating around the global axes, while rotating + * intrinsically means rotating from the perspective of the other rotation. A + * neat property is that applying a series of rotations extrinsically is the + * same as applying the same series in the opposite order intrinsically. */ class WPILIB_DLLEXPORT Rotation3d { public: @@ -242,9 +285,18 @@ class WPILIB_DLLEXPORT Rotation3d { : Rotation3d{0_rad, 0_rad, rotation.Radians()} {} /** - * Adds two rotations together. + * Adds two rotations together. The other rotation is applied extrinsically to + * this rotation, which is equivalent to this rotation being applied + * intrinsically to the other rotation. See the class comment for definitions + * of extrinsic and intrinsic rotations. * - * @param other The rotation to add. + * Note that `a - b + b` always equals `a`, but `b + (a - b)` + * sometimes doesn't. To apply a rotation offset, use either `offset = + * -measurement + actual; newAngle = angle + offset;` or `offset = actual - + * measurement; newAngle = offset + angle;`, depending on how the corrected + * angle needs to change as the input angle changes. + * + * @param other The rotation to add (applied extrinsically). * * @return The sum of the two rotations. */ @@ -254,11 +306,20 @@ class WPILIB_DLLEXPORT Rotation3d { /** * Subtracts the new rotation from the current rotation and returns the new - * rotation. + * rotation. The new rotation is from the perspective of the other rotation + * (like Pose3d::operator-), so it needs to be applied intrinsically. See the + * class comment for definitions of extrinsic and intrinsic rotations. + * + * Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes + * doesn't. To apply a rotation offset, use either `offset = -measurement + + * actual; newAngle = angle + offset;` or `offset = actual - measurement; + * newAngle = offset + angle;`, depending on how the corrected angle needs to + * change as the input angle changes. * * @param other The rotation to subtract. * - * @return The difference between the two rotations. + * @return The difference between the two rotations, from the perspective of + * the other rotation. */ constexpr Rotation3d operator-(const Rotation3d& other) const { return *this + -other; @@ -323,6 +384,28 @@ class WPILIB_DLLEXPORT Rotation3d { return Rotation3d{other.m_q * m_q}; } + /** + * Returns the current rotation relative to the given rotation. Because the + * result is in the perspective of the given rotation, it must be applied + * intrinsically. See the class comment for definitions of extrinsic and + * intrinsic rotations. + * + * @param other The rotation describing the orientation of the new coordinate + * frame that the current rotation will be converted into. + * + * @return The current rotation relative to the new orientation of the + * coordinate frame. + */ + constexpr Rotation3d RelativeTo(const Rotation3d& other) const { + // To apply a quaternion intrinsically, we must right-multiply by that + // quaternion. Therefore, "this_q relative to other_q" is the q such that + // other_q q = this_q: + // + // other_q q = this_q + // q = other_q⁻¹ this_q + return Rotation3d{other.m_q.Inverse() * m_q}; + } + /** * Returns the quaternion representation of the Rotation3d. */ @@ -449,5 +532,11 @@ void from_json(const wpi::json& json, Rotation3d& rotation); } // namespace frc +template <> +constexpr frc::Rotation3d wpi::Lerp(const frc::Rotation3d& startValue, + const frc::Rotation3d& endValue, double t) { + return (endValue - startValue) * t + startValue; +} + #include "frc/geometry/proto/Rotation3dProto.h" #include "frc/geometry/struct/Rotation3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 88c00550d0..2055ee489f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -167,13 +167,12 @@ class WPILIB_DLLEXPORT Transform2d { namespace frc { constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) { - // We are rotating the difference between the translations - // using a clockwise rotation matrix. This transforms the global - // delta into a local delta (relative to the initial pose). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = (final.Translation() - initial.Translation()) .RotateBy(-initial.Rotation()); - m_rotation = final.Rotation() - initial.Rotation(); + m_rotation = final.Rotation().RelativeTo(initial.Rotation()); } constexpr Transform2d Transform2d::operator+(const Transform2d& other) const { diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index bd6ca9c41e..38cf39d69f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -16,7 +16,9 @@ namespace frc { class Pose3d; /** - * Represents a transformation for a Pose3d in the pose's frame. + * Represents a transformation for a Pose3d in the pose's frame. Translation is + * applied before rotation. (The translation is applied in the pose's original + * frame, not the transformed frame.) */ class WPILIB_DLLEXPORT Transform3d { public: @@ -192,13 +194,12 @@ class WPILIB_DLLEXPORT Transform3d { namespace frc { constexpr Transform3d::Transform3d(const Pose3d& initial, const 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). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = (final.Translation() - initial.Translation()) .RotateBy(-initial.Rotation()); - m_rotation = final.Rotation() - initial.Rotation(); + m_rotation = final.Rotation().RelativeTo(initial.Rotation()); } constexpr Transform3d Transform3d::operator+(const Transform3d& other) const { diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 47bcc46b14..f07df67cf0 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -14,6 +14,8 @@ #include #include "frc/geometry/Pose2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" #include "units/time.h" namespace frc { @@ -155,7 +157,8 @@ class TimeInterpolatableBuffer { std::function m_interpolatingFunc; }; -// Template specialization to ensure that Pose2d uses pose exponential +// Template specializations to ensure that Pose2d and Pose3d use pose +// exponential template <> inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( units::second_t historySize) @@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( } }) {} +template <> +inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( + units::second_t historySize) + : m_historySize(historySize), + m_interpolatingFunc([](const Pose3d& start, const Pose3d& end, double t) { + if (t < 0) { + return start; + } else if (t >= 1) { + return end; + } else { + Twist3d twist = start.Log(end); + Twist3d scaledTwist = twist * t; + return start.Exp(scaledTwist); + } + }) {} + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h index c73eed2248..991bd1a34d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h @@ -45,7 +45,7 @@ class WPILIB_DLLEXPORT Odometry { m_pose(initialPose), m_previousWheelPositions(wheelPositions) { m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); } /** @@ -62,7 +62,7 @@ class WPILIB_DLLEXPORT Odometry { const WheelPositions& wheelPositions, const Pose2d& pose) { m_pose = pose; m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); m_previousWheelPositions = wheelPositions; } @@ -72,7 +72,8 @@ class WPILIB_DLLEXPORT Odometry { * @param pose The pose to reset to. */ void ResetPose(const Pose2d& pose) { - m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + m_gyroOffset = + m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation()); m_pose = pose; m_previousAngle = pose.Rotation(); } @@ -92,7 +93,7 @@ class WPILIB_DLLEXPORT Odometry { * @param rotation The rotation to reset to. */ void ResetRotation(const Rotation2d& rotation) { - m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation); m_pose = Pose2d{m_pose.Translation(), rotation}; m_previousAngle = rotation; } @@ -116,7 +117,7 @@ class WPILIB_DLLEXPORT Odometry { */ const Pose2d& Update(const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { - auto angle = gyroAngle + m_gyroOffset; + auto angle = gyroAngle.RotateBy(m_gyroOffset); auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions); @@ -136,7 +137,10 @@ class WPILIB_DLLEXPORT Odometry { Pose2d m_pose; WheelPositions m_previousWheelPositions; + + // Always equal to m_pose.Rotation() Rotation2d m_previousAngle; + Rotation2d m_gyroOffset; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h index f24d6f86f6..9b67a75176 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h @@ -48,7 +48,9 @@ class WPILIB_DLLEXPORT Odometry3d { m_pose(initialPose), m_previousWheelPositions(wheelPositions) { m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + // When applied extrinsically, m_gyroOffset cancels the + // current gyroAngle and then rotates to m_pose.Rotation() + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); } /** @@ -65,7 +67,9 @@ class WPILIB_DLLEXPORT Odometry3d { const WheelPositions& wheelPositions, const Pose3d& pose) { m_pose = pose; m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + // When applied extrinsically, m_gyroOffset cancels the + // current gyroAngle and then rotates to m_pose.Rotation() + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); m_previousWheelPositions = wheelPositions; } @@ -75,7 +79,9 @@ class WPILIB_DLLEXPORT Odometry3d { * @param pose The pose to reset to. */ void ResetPose(const Pose3d& pose) { - m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = + m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation()); m_pose = pose; m_previousAngle = pose.Rotation(); } @@ -95,7 +101,8 @@ class WPILIB_DLLEXPORT Odometry3d { * @param rotation The rotation to reset to. */ void ResetRotation(const Rotation3d& rotation) { - m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation); m_pose = Pose3d{m_pose.Translation(), rotation}; m_previousAngle = rotation; } @@ -119,7 +126,7 @@ class WPILIB_DLLEXPORT Odometry3d { */ const Pose3d& Update(const Rotation3d& gyroAngle, const WheelPositions& wheelPositions) { - auto angle = gyroAngle + m_gyroOffset; + auto angle = gyroAngle.RotateBy(m_gyroOffset); auto angle_difference = (angle - m_previousAngle).ToVector(); auto twist2d = @@ -145,7 +152,14 @@ class WPILIB_DLLEXPORT Odometry3d { Pose3d m_pose; WheelPositions m_previousWheelPositions; + + // Always equal to m_pose.Rotation() Rotation3d m_previousAngle; + + // Applying a rotation intrinsically to the measured gyro angle should cause + // the corrected angle to be rotated intrinsically in the same way, so the + // measured gyro angle must be applied intrinsically. This is equivalent to + // applying the offset extrinsically to the measured gyro angle. Rotation3d m_gyroOffset; }; 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 6eaa2a3f98..223256bb6f 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 @@ -59,6 +59,16 @@ class Rotation2dTest { assertEquals(120.0, rot.getDegrees(), kEpsilon); } + @Test + void testRelativeTo() { + var start = Rotation2d.fromDegrees(30.0); + var end = Rotation2d.kCCW_Pi_2; + + var result = end.relativeTo(start); + + assertEquals(60.0, result.getDegrees(), kEpsilon); + } + @Test void testMinus() { var rot1 = Rotation2d.fromDegrees(70.0); 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 index 3e96a1cb3d..c87c7d0bad 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -281,6 +281,22 @@ class Rotation3dTest { assertEquals(expected, rot); } + @Test + void testRelativeTo() { + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var start = new Rotation3d(yAxis, Units.degreesToRadians(-90.0)); + var end = new Rotation3d(zAxis, Units.degreesToRadians(90.0)); + + final var intrinsicAxis = VecBuilder.fill(1.0, 1.0, 1.0); + var expected = new Rotation3d(intrinsicAxis, Units.degreesToRadians(120.0)); + + var result = end.relativeTo(start); + + assertEquals(expected, result); + } + @Test void testMinus() { final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); @@ -409,5 +425,32 @@ class Rotation3dTest { assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon); assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon); assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon); + + // t value of 0 should always produce the start + rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90)); + interpolated = rot1.interpolate(rot2, 0.0); + assertEquals(rot1.getX(), interpolated.getX(), kEpsilon); + assertEquals(rot1.getY(), interpolated.getY(), kEpsilon); + assertEquals(rot1.getZ(), interpolated.getZ(), kEpsilon); + + // The full rotation from rot1 to rot2 to 120 degrees around extrinsic <-1.0, 1.0, 1.0> + var extrinsicAxis = VecBuilder.fill(-1.0, 1.0, 1.0); + rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90)); + assertEquals(rot2, rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(120)))); + interpolated = rot1.interpolate(rot2, 0.5); + var expected = rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(60))); + assertEquals(expected.getX(), interpolated.getX(), kEpsilon); + assertEquals(expected.getY(), interpolated.getY(), kEpsilon); + assertEquals(expected.getZ(), interpolated.getZ(), kEpsilon); + + // t value of 1 should always produce the end + rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90)); + interpolated = rot1.interpolate(rot2, 1.0); + assertEquals(rot2.getX(), interpolated.getX(), kEpsilon); + assertEquals(rot2.getY(), interpolated.getY(), kEpsilon); + assertEquals(rot2.getZ(), interpolated.getZ(), kEpsilon); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java index 166a615ca9..b72b4ce8f5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java @@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; @@ -45,4 +46,22 @@ class DifferentialDriveOdometry3dTest { () -> assertEquals(pose.getZ(), 0.0, kEpsilon), () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 90.0, kEpsilon)); } + + @Test + void testGyroOffset() { + m_odometry.resetPosition( + new Rotation3d(0, Units.degreesToRadians(5), 0), + 0, + 0, + new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90)))); + var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), 0, 0); + + assertAll( + () -> assertEquals(pose.getX(), 0.0, kEpsilon), + () -> assertEquals(pose.getY(), 0.0, kEpsilon), + () -> assertEquals(pose.getZ(), 0.0, kEpsilon), + () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), kEpsilon), + () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), kEpsilon), + () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java index d417e2a120..4e1857d251 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java @@ -295,4 +295,22 @@ class MecanumDriveOdometry3dTest { 0.05, "Incorrect distance travelled"); } + + @Test + void testGyroOffset() { + var wheelPositions = new MecanumDriveWheelPositions(); + m_odometry.resetPosition( + new Rotation3d(0, Units.degreesToRadians(5), 0), + wheelPositions, + new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90)))); + var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), wheelPositions); + + assertAll( + () -> assertEquals(pose.getX(), 0.0, 1e-9), + () -> assertEquals(pose.getY(), 0.0, 1e-9), + () -> assertEquals(pose.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9), + () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9), + () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java index dfc446fcee..43058defe3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java @@ -305,4 +305,22 @@ class SwerveDriveOdometry3dTest { 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error"); assertEquals(0.0, maxError, 0.125, "Incorrect max error"); } + + @Test + void testGyroOffset() { + SwerveModulePosition[] modulePositions = {zero, zero, zero, zero}; + m_odometry.resetPosition( + new Rotation3d(0, Units.degreesToRadians(5), 0), + modulePositions, + new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90)))); + var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), modulePositions); + + assertAll( + () -> assertEquals(pose.getX(), 0.0, 1e-9), + () -> assertEquals(pose.getY(), 0.0, 1e-9), + () -> assertEquals(pose.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9), + () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9), + () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9)); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index ffbbd20c4a..66e000a8e2 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -42,6 +42,15 @@ TEST(Rotation2dTest, RotateByNonZero) { EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value()); } +TEST(Rotation2dTest, RelativeTo) { + auto start = Rotation2d{30_deg}; + auto end = Rotation2d{90_deg}; + + auto result = end.RelativeTo(start); + + EXPECT_DOUBLE_EQ(60.0, result.Degrees().value()); +} + TEST(Rotation2dTest, Minus) { const auto rot1 = Rotation2d{70_deg}; const auto rot2 = Rotation2d{30_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 903d4144bc..6c0d4f6e14 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -253,6 +253,21 @@ TEST(Rotation3dTest, RotateByNonZeroZ) { EXPECT_EQ(expected, rot); } +TEST(Rotation3dTest, RelativeTo) { + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + auto start = Rotation3d{yAxis, -90_deg}; + auto end = Rotation3d{zAxis, 90_deg}; + + const Eigen::Vector3d intrinsicAxis{1.0, 1.0, 1.0}; + auto expected = Rotation3d{intrinsicAxis, 120_deg}; + + auto result = end.RelativeTo(start); + + EXPECT_EQ(expected, result); +} + TEST(Rotation3dTest, Minus) { const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; @@ -371,4 +386,32 @@ TEST(Rotation3dTest, Interpolate) { 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()); + + // t value of 0 should always produce the start + rot1 = Rotation3d{yAxis, -90_deg}; + rot2 = Rotation3d{zAxis, 90_deg}; + interpolated = wpi::Lerp(rot1, rot2, 0.0); + EXPECT_DOUBLE_EQ(rot1.X().value(), interpolated.X().value()); + EXPECT_DOUBLE_EQ(rot1.Y().value(), interpolated.Y().value()); + EXPECT_DOUBLE_EQ(rot1.Z().value(), interpolated.Z().value()); + + // The full rotation from rot1 to rot2 is 120 degrees around extrinsic + // <-1.0, 1.0, 1.0> + const Eigen::Vector3d extrinsicAxis{-1.0, 1.0, 1.0}; + rot1 = Rotation3d{yAxis, -90_deg}; + rot2 = Rotation3d{zAxis, 90_deg}; + EXPECT_EQ(rot2, rot1.RotateBy(Rotation3d{extrinsicAxis, 120_deg})); + interpolated = wpi::Lerp(rot1, rot2, 0.5); + auto expected = rot1.RotateBy(Rotation3d{extrinsicAxis, 60_deg}); + EXPECT_DOUBLE_EQ(expected.X().value(), interpolated.X().value()); + EXPECT_DOUBLE_EQ(expected.Y().value(), interpolated.Y().value()); + EXPECT_DOUBLE_EQ(expected.Z().value(), interpolated.Z().value()); + + // t value of 1 should always produce the end + rot1 = Rotation3d{yAxis, -90_deg}; + rot2 = Rotation3d{zAxis, 90_deg}; + interpolated = wpi::Lerp(rot1, rot2, 1.0); + EXPECT_NEAR(rot2.X().value(), interpolated.X().value(), 1e-9); + EXPECT_NEAR(rot2.Y().value(), interpolated.Y().value(), 1e-9); + EXPECT_NEAR(rot2.Z().value(), interpolated.Z().value(), 1e-9); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp index 76f5f3292c..df7099368f 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp @@ -38,3 +38,18 @@ TEST(DifferentialDriveOdometry3dTest, EncoderDistances) { EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon); EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon); } + +TEST(DifferentialDriveOdometry3dTest, GyroOffset) { + DifferentialDriveOdometry3d odometry{ + frc::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m, + frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}}; + const auto& pose = + odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m); + + EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon); + EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon); + EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon); + EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, kEpsilon); +} diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp index b195e8507b..e957ae31c2 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp @@ -221,3 +221,19 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) { EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); EXPECT_LT(maxError, 0.125); } + +TEST_F(MecanumDriveOdometry3dTest, GyroOffset) { + frc::MecanumDriveWheelPositions wheelPositions; + odometry.ResetPosition( + frc::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions, + frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}); + auto pose = + odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, wheelPositions); + + EXPECT_NEAR(pose.X().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9); + EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9); + EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9); + EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9); +} diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp index d9fd52a559..01a2cea03a 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp @@ -222,3 +222,18 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) { EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); EXPECT_LT(maxError, 0.125); } + +TEST_F(SwerveDriveOdometry3dTest, GyroOffset) { + m_odometry.ResetPosition( + frc::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero}, + frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}); + auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, + {zero, zero, zero, zero}); + + EXPECT_NEAR(pose.X().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9); + EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9); + EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9); + EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9); +}