Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Implement Rotation3d interpolation as slerp instead of lerp (#8529)

Browse Source 
Also replace arithmetic operators since they're not commutative, which
is confusing for users.
This commit is contained in:
Tyler Veness
2026-03-06 15:15:00 -08:00
committed by GitHub
parent 28176f1062
commit b29bde123f
22 changed files with 142 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java
View File

@@ -86,7 +86,7 @@ public class CoordinateSystem {
public static Translation3d convert(
Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
// Convert to NWU, then convert to the new coordinate system
return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus());
return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.inverse());
}
/**
@@ -100,7 +100,7 @@ public class CoordinateSystem {
public static Rotation3d convert(
Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
// Convert to NWU, then convert to the new coordinate system
return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus());
return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.inverse());
}
/**
@@ -128,7 +128,7 @@ public class CoordinateSystem {
Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
// 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());
var coordRot = from.m_rotation.rotateBy(to.m_rotation.inverse());
// 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
@@ -140,9 +140,9 @@ public class CoordinateSystem {
// = (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().
// change in order).
return new Transform3d(
convert(transform.getTranslation(), from, to),
coordRot.unaryMinus().rotateBy(transform.getRotation().rotateBy(coordRot)));
coordRot.inverse().rotateBy(transform.getRotation().rotateBy(coordRot)));
}
}

2
wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java
View File

@@ -338,7 +338,7 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
Comparator.comparing(
(Pose3d other) -> this.getTranslation().getDistance(other.getTranslation()))
.thenComparing(
(Pose3d other) -> this.getRotation().minus(other.getRotation()).getAngle()));
(Pose3d other) -> this.getRotation().relativeTo(other.getRotation()).getAngle()));
}
@Override

20
wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java
View File

@@ -216,16 +216,6 @@ public class Quaternion implements ProtobufSerializable, StructSerializable {
return this.log().times(t).exp();
}
/**
* Matrix exponential of a quaternion.
*
* @param adjustment the "Twist" that will be applied to this quaternion.
* @return The quaternion product of exp(adjustment) * this
*/
public Quaternion exp(Quaternion adjustment) {
return adjustment.exp().times(this);
}
/**
* Matrix exponential of a quaternion.
*
@@ -260,16 +250,6 @@ public class Quaternion implements ProtobufSerializable, StructSerializable {
getZ() * axial_scalar * scalar);
}
/**
* Log operator of a quaternion.
*
* @param end The quaternion to map this quaternion onto.
* @return The "Twist" that maps this quaternion to the argument.
*/
public Quaternion log(Quaternion end) {
return end.times(this.inverse()).log();
}
/**
* The Log operator of a general quaternion.
*

2
wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java
View File

@@ -203,7 +203,7 @@ public class Rotation2d
}
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
* Returns this rotation relative to another rotation.
*
* <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code>
* equals <code>Rotation2d(-Math.PI/2.0)</code>

63
wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java
View File

@@ -326,49 +326,12 @@ public class Rotation3d
this(0.0, 0.0, rotation.getRadians());
}
/**
* 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.
*
* <p>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) {
return rotateBy(other);
}
/**
* 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.
*
* <p>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, from the perspective of the other rotation.
*/
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() {
public Rotation3d inverse() {
return new Rotation3d(m_q.inverse());
}
@@ -379,16 +342,7 @@ public class Rotation3d
* @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()));
}
return Rotation3d.kZero.interpolate(this, scalar);
}
/**
@@ -638,7 +592,18 @@ public class Rotation3d
@Override
public Rotation3d interpolate(Rotation3d endValue, double t) {
return endValue.minus(this).times(Math.clamp(t, 0, 1)).plus(this);
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
//
// slerp(q₀, q₁, t) = (q₁q₀⁻¹)ᵗq₀
//
// We negate the delta quaternion if necessary to take the shortest path
var q0 = m_q;
var q1 = endValue.m_q;
var delta = q1.times(q0.inverse());
if (delta.getW() < 0.0) {
delta = new Quaternion(-delta.getW(), -delta.getX(), -delta.getY(), -delta.getZ());
}
return new Rotation3d(delta.pow(t).times(q0));
}
/** Rotation3d protobuf for serialization. */

5
wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java
View File

@@ -45,7 +45,7 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
.rotateBy(initial.getRotation().inverse());
m_rotation = last.getRotation().relativeTo(initial.getRotation());
}
@@ -290,8 +290,7 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
// 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());
getTranslation().unaryMinus().rotateBy(getRotation().inverse()), getRotation().inverse());
}
@Override

10
wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java
View File

@@ -57,7 +57,7 @@ public class Odometry3d<T> {
m_pose = initialPose;
// When applied extrinsically, m_gyroOffset cancels the current gyroAngle and
// then rotates to m_poseMeters.getRotation()
m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
m_gyroOffset = gyroAngle.inverse().rotateBy(m_pose.getRotation());
m_previousAngle = m_pose.getRotation();
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}
@@ -76,7 +76,7 @@ public class Odometry3d<T> {
m_pose = pose;
// When applied extrinsically, m_gyroOffset cancels the current gyroAngle and
// then rotates to m_poseMeters.getRotation()
m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
m_gyroOffset = gyroAngle.inverse().rotateBy(m_pose.getRotation());
m_previousAngle = m_pose.getRotation();
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
}
@@ -89,7 +89,7 @@ public class Odometry3d<T> {
public void resetPose(Pose3d pose) {
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
m_gyroOffset =
m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation());
m_gyroOffset.rotateBy(m_pose.getRotation().inverse()).rotateBy(pose.getRotation());
m_pose = pose;
m_previousAngle = m_pose.getRotation();
}
@@ -110,7 +110,7 @@ public class Odometry3d<T> {
*/
public void resetRotation(Rotation3d rotation) {
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation);
m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().inverse()).rotateBy(rotation);
m_pose = new Pose3d(m_pose.getTranslation(), rotation);
m_previousAngle = m_pose.getRotation();
}
@@ -136,7 +136,7 @@ public class Odometry3d<T> {
*/
public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
var angle = gyroAngle.rotateBy(m_gyroOffset);
var angle_difference = angle.minus(m_previousAngle).toVector();
var angle_difference = angle.relativeTo(m_previousAngle).toVector();
var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
var twist =

11
wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp
View File

@@ -86,7 +86,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
const CoordinateSystem& from,
const CoordinateSystem& to) {
// Convert to NWU, then convert to the new coordinate system
return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
return translation.RotateBy(from.m_rotation)
.RotateBy(to.m_rotation.Inverse());
}
/**
@@ -101,7 +102,7 @@ class WPILIB_DLLEXPORT CoordinateSystem {
const CoordinateSystem& from,
const CoordinateSystem& to) {
// Convert to NWU, then convert to the new coordinate system
return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
return rotation.RotateBy(from.m_rotation).RotateBy(to.m_rotation.Inverse());
}
/**
@@ -133,7 +134,7 @@ class WPILIB_DLLEXPORT CoordinateSystem {
// 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);
const auto coordRot = from.m_rotation.RotateBy(to.m_rotation.Inverse());
// 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
@@ -145,10 +146,10 @@ class WPILIB_DLLEXPORT CoordinateSystem {
// = (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.
// change in order).
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
coordRot.Inverse().RotateBy(transform.Rotation().RotateBy(coordRot))};
}
private:

22
wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp
View File

@@ -256,9 +256,14 @@ class WPILIB_DLLEXPORT Pose3d {
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
return gcem::abs(this->Rotation()
.RelativeTo(a.Rotation())
.Angle()
.value()) <
gcem::abs(this->Rotation()
.RelativeTo(b.Rotation())
.Angle()
.value());
}
return aDistance < bDistance;
});
@@ -281,9 +286,14 @@ class WPILIB_DLLEXPORT Pose3d {
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Angle().value()) <
gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
return gcem::abs(this->Rotation()
.RelativeTo(a.Rotation())
.Angle()
.value()) <
gcem::abs(this->Rotation()
.RelativeTo(b.Rotation())
.Angle()
.value());
}
return aDistance < bDistance;
});

18
wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp
View File

@@ -157,15 +157,6 @@ class WPILIB_DLLEXPORT Quaternion {
*/
constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); }
/**
* Matrix exponential of a quaternion.
*
* @param other the "Twist" that will be applied to this quaternion.
*/
constexpr Quaternion Exp(const Quaternion& other) const {
return other.Exp() * *this;
}
/**
* Matrix exponential of a quaternion.
*
@@ -196,15 +187,6 @@ class WPILIB_DLLEXPORT Quaternion {
Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
}
/**
* Log operator of a quaternion.
*
* @param other The quaternion to map this quaternion onto
*/
constexpr Quaternion Log(const Quaternion& other) const {
return (other * Inverse()).Log();
}
/**
* Log operator of a quaternion.
*

3
wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp
View File

@@ -118,8 +118,7 @@ class WPILIB_DLLEXPORT Rotation2d {
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
* Returns this rotation relative to another rotation.
*
* For example, <code>Rotation2d{10_deg} - Rotation2d{100_deg}</code> equals
* <code>Rotation2d{wpi::units::radian_t{-std::numbers::pi/2.0}}</code>

79
wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp
View File

@@ -282,53 +282,12 @@ class WPILIB_DLLEXPORT Rotation3d {
constexpr explicit Rotation3d(const Rotation2d& rotation)
: Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
/**
* 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.
*
* 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.
*/
constexpr Rotation3d operator+(const Rotation3d& other) const {
return RotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* 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, from the perspective of
* the other rotation.
*/
constexpr Rotation3d operator-(const Rotation3d& other) const {
return *this + -other;
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; }
constexpr Rotation3d Inverse() const { return Rotation3d{m_q.Inverse()}; }
/**
* Multiplies the current rotation by a scalar.
@@ -338,16 +297,7 @@ class WPILIB_DLLEXPORT Rotation3d {
* @return The new scaled Rotation3d.
*/
constexpr Rotation3d operator*(double scalar) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{
Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(m_q.W())}};
} else {
return Rotation3d{
Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(-m_q.W())}};
}
return Rotation3d{}.Interpolate(*this, scalar);
}
/**
@@ -406,6 +356,29 @@ class WPILIB_DLLEXPORT Rotation3d {
return Rotation3d{other.m_q.Inverse() * m_q};
}
/**
* Interpolates between this rotation and another.
*
* @param endValue The other rotation.
* @param t How far between the two rotations we are (0 means this rotation, 1
* means other rotation).
* @return The interpolated value.
*/
constexpr Rotation3d Interpolate(Rotation3d endValue, double t) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
//
// slerp(q₀, q₁, t) = (q₁q₀⁻¹)ᵗq₀
//
// We negate the delta quaternion if necessary to take the shortest path
const auto& q0 = m_q;
const auto& q1 = endValue.m_q;
auto delta = q1 * q0.Inverse();
if (delta.W() < 0.0) {
delta = Quaternion{-delta.W(), -delta.X(), -delta.Y(), -delta.Z()};
}
return Rotation3d{delta.Pow(t) * q0};
}
/**
* Returns the quaternion representation of the Rotation3d.
*/
@@ -537,7 +510,7 @@ template <>
constexpr wpi::math::Rotation3d wpi::util::Lerp(
const wpi::math::Rotation3d& startValue,
const wpi::math::Rotation3d& endValue, double t) {
return (endValue - startValue) * t + startValue;
return startValue.Interpolate(endValue, t);
}
#include "wpi/math/geometry/proto/Rotation3dProto.hpp"

5
wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp
View File

@@ -153,7 +153,8 @@ class WPILIB_DLLEXPORT Transform3d {
// 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()};
return Transform3d{(-Translation()).RotateBy(Rotation().Inverse()),
Rotation().Inverse()};
}
/**
@@ -207,7 +208,7 @@ constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
// 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());
.RotateBy(initial.Rotation().Inverse());
m_rotation = final.Rotation().RelativeTo(initial.Rotation());
}

15
wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp
View File

@@ -36,7 +36,7 @@ class TimeInterpolatableBuffer {
/**
* Create a new TimeInterpolatableBuffer.
*
* @param historySize The history size of the buffer.
* @param historySize The history size of the buffer.
* @param func The function used to interpolate between values.
*/
TimeInterpolatableBuffer(wpi::units::second_t historySize,
@@ -45,15 +45,20 @@ class TimeInterpolatableBuffer {
/**
* Create a new TimeInterpolatableBuffer. By default, the interpolation
* function is wpi::util::Lerp except for Pose2d, which uses the pose
* exponential.
* function is wpi::util::Lerp. If the arithmetic operators aren't supported
* (usually because they wouldn't be commutative), Interpolate() is used
* instead.
*
* @param historySize The history size of the buffer.
* @param historySize The history size of the buffer.
*/
explicit TimeInterpolatableBuffer(wpi::units::second_t historySize)
: m_historySize(historySize),
m_interpolatingFunc([](const T& start, const T& end, double t) {
return wpi::util::Lerp(start, end, t);
if constexpr (requires(T a, T b, double t) { a + (b - a) * t; }) {
return wpi::util::Lerp(start, end, t);
} else {
return start.Interpolate(end, t);
}
}) {}
/**

13
wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp
View File

@@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT Odometry3d {
m_previousAngle = m_pose.Rotation();
// When applied extrinsically, m_gyroOffset cancels the
// current gyroAngle and then rotates to m_pose.Rotation()
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
m_gyroOffset = gyroAngle.Inverse().RotateBy(m_pose.Rotation());
}
/**
@@ -68,7 +68,7 @@ class WPILIB_DLLEXPORT Odometry3d {
m_previousAngle = pose.Rotation();
// When applied extrinsically, m_gyroOffset cancels the
// current gyroAngle and then rotates to m_pose.Rotation()
m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
m_gyroOffset = gyroAngle.Inverse().RotateBy(m_pose.Rotation());
m_previousWheelPositions = wheelPositions;
}
@@ -79,8 +79,8 @@ class WPILIB_DLLEXPORT Odometry3d {
*/
void ResetPose(const Pose3d& pose) {
// 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_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation().Inverse())
.RotateBy(pose.Rotation());
m_pose = pose;
m_previousAngle = pose.Rotation();
}
@@ -101,7 +101,8 @@ class WPILIB_DLLEXPORT Odometry3d {
*/
void ResetRotation(const Rotation3d& 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_gyroOffset =
m_gyroOffset.RotateBy(m_pose.Rotation().Inverse()).RotateBy(rotation);
m_pose = Pose3d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}
@@ -126,7 +127,7 @@ class WPILIB_DLLEXPORT Odometry3d {
const Pose3d& Update(const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions) {
auto angle = gyroAngle.RotateBy(m_gyroOffset);
auto angle_difference = (angle - m_previousAngle).ToVector();
auto angle_difference = angle.RelativeTo(m_previousAngle).ToVector();
auto twist2d =
m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);

7
wpimath/src/main/python/semiwrap/Rotation3d.yml
View File

@@ -26,15 +26,12 @@ classes:
keepalive: []
const Rotation2d&:
keepalive: []
operator+:
operator-:
overloads:
const Rotation3d& [const]:
'[const]':
Inverse:
operator*:
operator/:
operator==:
RotateBy:
Interpolate:
GetQuaternion:
X:
Y:

4
wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java
View File

@@ -219,8 +219,8 @@ class QuaternionTest {
var start = new Quaternion(1, 2, 3, 4);
var expect = new Quaternion(5, 6, 7, 8);
var twist = start.log(expect);
var actual = start.exp(twist);
var twist = expect.times(start.inverse()).log();
var actual = twist.exp().times(start);
assertEquals(expect, actual);
}

30
wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java
View File

@@ -24,7 +24,7 @@ class Rotation3dTest {
var rot1 = new Rotation3d(0, 0, Math.PI / 2);
var rot2 = new Rotation3d(Math.PI, 0, 0);
var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
final var result1 = rot1.plus(rot2).plus(rot3);
final var result1 = rot1.rotateBy(rot2).rotateBy(rot3);
final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
assertAll(
() -> assertEquals(expected1, result1),
@@ -34,7 +34,7 @@ class Rotation3dTest {
rot1 = new Rotation3d(0, 0, Math.PI / 2);
rot2 = new Rotation3d(-Math.PI, 0, 0);
rot3 = new Rotation3d(Math.PI / 2, 0, 0);
final var result2 = rot1.plus(rot2).plus(rot3);
final var result2 = rot1.rotateBy(rot2).rotateBy(rot3);
final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
assertAll(
() -> assertEquals(expected2, result2),
@@ -44,7 +44,7 @@ class Rotation3dTest {
rot1 = new Rotation3d(0, 0, Math.PI / 2);
rot2 = new Rotation3d(0, Math.PI / 3, 0);
rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
final var result3 = rot1.plus(rot2).plus(rot3);
final var result3 = rot1.rotateBy(rot2).rotateBy(rot3);
final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
assertAll(
() -> assertEquals(expected3, result3),
@@ -196,22 +196,22 @@ class Rotation3dTest {
void testRotationLoop() {
var rot = Rotation3d.kZero;
rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
rot = rot.rotateBy(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));
rot = rot.rotateBy(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)));
rot = rot.rotateBy(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));
rot = rot.rotateBy(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
assertEquals(Rotation3d.kZero, rot);
}
@@ -253,7 +253,7 @@ class Rotation3dTest {
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)));
rot = rot.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0));
assertEquals(expected, rot);
@@ -264,7 +264,7 @@ class Rotation3dTest {
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)));
rot = rot.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0));
assertEquals(expected, rot);
@@ -275,7 +275,7 @@ class Rotation3dTest {
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)));
rot = rot.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0));
assertEquals(expected, rot);
@@ -297,16 +297,6 @@ class Rotation3dTest {
assertEquals(expected, result);
}
@Test
void testMinus() {
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() {
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);

4
wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
View File

@@ -200,8 +200,8 @@ TEST(QuaternionTest, LogarithmAndExponentialInverse) {
Quaternion start{1, 2, 3, 4};
Quaternion expect{5, 6, 7, 8};
auto twist = start.Log(expect);
auto actual = start.Exp(twist);
auto twist = (expect * start.Inverse()).Log();
auto actual = twist.Exp() * start;
EXPECT_EQ(expect, actual);
}

37
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -20,7 +20,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
auto rot2 = Rotation3d{wpi::units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
auto rot3 =
Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result1 = rot1 + rot2 + rot3;
const auto result1 = rot1.RotateBy(rot2).RotateBy(rot3);
const auto expected1 =
Rotation3d{0_rad, -wpi::units::radian_t{std::numbers::pi / 2},
wpi::units::radian_t{std::numbers::pi / 2}};
@@ -31,7 +31,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{wpi::units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
rot3 = Rotation3d{wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result2 = rot1 + rot2 + rot3;
const auto result2 = rot1.RotateBy(rot2).RotateBy(rot3);
const auto expected2 =
Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2},
wpi::units::radian_t{std::numbers::pi / 2}};
@@ -42,7 +42,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 3}, 0_rad};
rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result3 = rot1 + rot2 + rot3;
const auto result3 = rot1.RotateBy(rot2).RotateBy(rot3);
const auto expected3 =
Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2},
wpi::units::radian_t{std::numbers::pi / 6}};
@@ -184,20 +184,20 @@ TEST(Rotation3dTest, DegreesToRadians) {
TEST(Rotation3dTest, RotationLoop) {
Rotation3d rot;
rot = rot + Rotation3d{90_deg, 0_deg, 0_deg};
rot = rot.RotateBy(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};
rot = rot.RotateBy(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};
rot = rot.RotateBy(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};
rot = rot.RotateBy(Rotation3d{0_deg, -90_deg, 0_deg});
EXPECT_EQ(Rotation3d{}, rot);
}
@@ -205,7 +205,7 @@ TEST(Rotation3dTest, RotateByFromZeroX) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Rotation3d zero;
auto rotated = zero + Rotation3d{xAxis, 90_deg};
auto rotated = zero.RotateBy(Rotation3d{xAxis, 90_deg});
Rotation3d expected{xAxis, 90_deg};
EXPECT_EQ(expected, rotated);
@@ -215,7 +215,7 @@ TEST(Rotation3dTest, RotateByFromZeroY) {
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Rotation3d zero;
auto rotated = zero + Rotation3d{yAxis, 90_deg};
auto rotated = zero.RotateBy(Rotation3d{yAxis, 90_deg});
Rotation3d expected{yAxis, 90_deg};
EXPECT_EQ(expected, rotated);
@@ -225,7 +225,7 @@ TEST(Rotation3dTest, RotateByFromZeroZ) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d zero;
auto rotated = zero + Rotation3d{zAxis, 90_deg};
auto rotated = zero.RotateBy(Rotation3d{zAxis, 90_deg});
Rotation3d expected{zAxis, 90_deg};
EXPECT_EQ(expected, rotated);
@@ -235,7 +235,7 @@ 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};
rot = rot.RotateBy(Rotation3d{xAxis, 30_deg});
Rotation3d expected{xAxis, 120_deg};
EXPECT_EQ(expected, rot);
@@ -245,7 +245,7 @@ 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};
rot = rot.RotateBy(Rotation3d{yAxis, 30_deg});
Rotation3d expected{yAxis, 120_deg};
EXPECT_EQ(expected, rot);
@@ -255,7 +255,7 @@ 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};
rot = rot.RotateBy(Rotation3d{zAxis, 30_deg});
Rotation3d expected{zAxis, 120_deg};
EXPECT_EQ(expected, rot);
@@ -276,15 +276,6 @@ TEST(Rotation3dTest, RelativeTo) {
EXPECT_EQ(expected, result);
}
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, wpi::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};
@@ -376,7 +367,7 @@ TEST(Rotation3dTest, Interpolate) {
rot2 = Rotation3d{yAxis, -160_deg};
interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(-5.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_NEAR(-5.0, wpi::units::degree_t{interpolated.Y()}.value(), 1e-9);
EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.Z()}.value());
// 50 + (70 - 50) * 0.5 = 60

4
wpimath/src/test/python/geometry/test_quaternion.py
View File

@@ -204,8 +204,8 @@ def test_logarithm_and_exponential_inverse():
start = Quaternion(1, 2, 3, 4)
expect = Quaternion(5, 6, 7, 8)
twist = start.log(expect)
actual = start.exp(twist)
twist = (expect * start.inverse()).log()
actual = twist.exp() * start
assert actual == expect

35
wpimath/src/test/python/geometry/test_rotation3d.py
View File

@@ -9,7 +9,7 @@ def test_gimbal_lock_accuracy():
rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2)
rot2 = Rotation3d(roll=math.pi, pitch=0, yaw=0)
rot3 = Rotation3d(roll=-math.pi / 2, pitch=0, yaw=0)
result1 = rot1 + rot2 + rot3
result1 = rot1.rotateBy(rot2).rotateBy(rot3)
expected1 = Rotation3d(roll=0, pitch=-math.pi / 2, yaw=math.pi / 2)
assert expected1 == result1
assert (result1.x + result1.z) == pytest.approx(math.pi / 2)
@@ -18,7 +18,7 @@ def test_gimbal_lock_accuracy():
rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2)
rot2 = Rotation3d(roll=-math.pi, pitch=0, yaw=0)
rot3 = Rotation3d(roll=math.pi / 2, pitch=0, yaw=0)
result2 = rot1 + rot2 + rot3
result2 = rot1.rotateBy(rot2).rotateBy(rot3)
expected2 = Rotation3d(roll=0, pitch=math.pi / 2, yaw=math.pi / 2)
assert expected2 == result2
assert (result2.z - result2.x) == pytest.approx(math.pi / 2)
@@ -27,7 +27,7 @@ def test_gimbal_lock_accuracy():
rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2)
rot2 = Rotation3d(roll=0, pitch=math.pi / 3, yaw=0)
rot3 = Rotation3d(roll=-math.pi / 2, pitch=0, yaw=0)
result3 = rot1 + rot2 + rot3
result3 = rot1.rotateBy(rot2).rotateBy(rot3)
expected3 = Rotation3d(roll=0, pitch=math.pi / 2, yaw=math.pi / 6)
assert expected3 == result3
assert (result3.z - result3.x) == pytest.approx(math.pi / 6)
@@ -170,22 +170,22 @@ def test_degrees_to_radians():
def test_rotation_loop():
rot = Rotation3d()
rot = rot + Rotation3d(math.radians(90), 0, 0)
rot = rot.rotateBy(Rotation3d(math.radians(90), 0, 0))
expected = Rotation3d(math.radians(90), 0, 0)
assert expected == rot
rot = rot + Rotation3d(0, math.radians(90), 0)
rot = rot.rotateBy(Rotation3d(0, math.radians(90), 0))
expected = Rotation3d(
np.array([1.0 / math.sqrt(3), 1.0 / math.sqrt(3), -1.0 / math.sqrt(3)]),
math.radians(120),
)
assert expected == rot
rot = rot + Rotation3d(0, 0, math.radians(90))
rot = rot.rotateBy(Rotation3d(0, 0, math.radians(90)))
expected = Rotation3d(0, math.radians(90), 0)
assert expected == rot
rot = rot + Rotation3d(0, math.radians(-90), 0)
rot = rot.rotateBy(Rotation3d(0, math.radians(-90), 0))
assert Rotation3d() == rot
@@ -193,7 +193,7 @@ def test_rotate_by_from_zero_x():
x_axis = np.array([1.0, 0.0, 0.0])
zero = Rotation3d()
rotated = zero + Rotation3d(x_axis, math.radians(90))
rotated = zero.rotateBy(Rotation3d(x_axis, math.radians(90)))
expected = Rotation3d(x_axis, math.radians(90))
assert expected == rotated
@@ -203,7 +203,7 @@ def test_rotate_by_from_zero_y():
y_axis = np.array([0.0, 1.0, 0.0])
zero = Rotation3d()
rotated = zero + Rotation3d(y_axis, math.radians(90))
rotated = zero.rotateBy(Rotation3d(y_axis, math.radians(90)))
expected = Rotation3d(y_axis, math.radians(90))
assert expected == rotated
@@ -213,7 +213,7 @@ def test_rotate_by_from_zero_z():
z_axis = np.array([0.0, 0.0, 1.0])
zero = Rotation3d()
rotated = zero + Rotation3d(z_axis, math.radians(90))
rotated = zero.rotateBy(Rotation3d(z_axis, math.radians(90)))
expected = Rotation3d(z_axis, math.radians(90))
assert expected == rotated
@@ -223,7 +223,7 @@ def test_rotate_by_non_zero_x():
x_axis = np.array([1.0, 0.0, 0.0])
rot = Rotation3d(x_axis, math.radians(90))
rot = rot + Rotation3d(x_axis, math.radians(30))
rot = rot.rotateBy(Rotation3d(x_axis, math.radians(30)))
expected = Rotation3d(x_axis, math.radians(120))
assert expected == rot
@@ -233,7 +233,7 @@ def test_rotate_by_non_zero_y():
y_axis = np.array([0.0, 1.0, 0.0])
rot = Rotation3d(y_axis, math.radians(90))
rot = rot + Rotation3d(y_axis, math.radians(30))
rot = rot.rotateBy(Rotation3d(y_axis, math.radians(30)))
expected = Rotation3d(y_axis, math.radians(120))
assert expected == rot
@@ -243,21 +243,12 @@ def test_rotate_by_non_zero_z():
z_axis = np.array([0.0, 0.0, 1.0])
rot = Rotation3d(z_axis, math.radians(90))
rot = rot + Rotation3d(z_axis, math.radians(30))
rot = rot.rotateBy(Rotation3d(z_axis, math.radians(30)))
expected = Rotation3d(z_axis, math.radians(120))
assert expected == rot
def test_minus():
z_axis = np.array([0.0, 0.0, 1.0])
rot1 = Rotation3d(z_axis, math.radians(70))
rot2 = Rotation3d(z_axis, math.radians(30))
assert math.degrees((rot1 - rot2).z) == pytest.approx(40.0)
def test_axis_angle():
x_axis = np.array([1.0, 0.0, 0.0])
y_axis = np.array([0.0, 1.0, 0.0])