Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
calcmogul wants to merge 1 commit into
base: 2027
Choose a base branch
from
Draft

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ public static CoordinateSystem NED() {
*/
public static Translation3d convert(
Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
return translation.rotateBy(to.m_rotation.relativeTo(from.m_rotation));
}

/**
Expand All @@ -98,7 +98,7 @@ public static Translation3d convert(
*/
public static Rotation3d convert(
Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
return rotation.rotateBy(to.m_rotation.relativeTo(from.m_rotation));
}

/**
Expand All @@ -124,9 +124,9 @@ public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSyste
*/
public static Transform3d convert(
Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
var coordRot = from.m_rotation.minus(to.m_rotation);
var coordRot = to.m_rotation.relativeTo(from.m_rotation);
return new Transform3d(
convert(transform.getTranslation(), from, to),
coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
coordRot.inverse().rotateBy(transform.getRotation().rotateBy(coordRot)));
}
}
4 changes: 2 additions & 2 deletions wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -255,7 +255,7 @@ public Pose3d rotateBy(Rotation3d other) {
public Pose3d transformBy(Transform3d other) {
return new Pose3d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
other.getRotation().plus(m_rotation));
other.getRotation().rotateBy(m_rotation));
}

/**
Expand Down Expand Up @@ -335,7 +335,7 @@ public Pose3d nearest(Collection<Pose3d> poses) {
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
Expand Down
20 changes: 0 additions & 20 deletions wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,16 +216,6 @@ public Quaternion pow(double t) {
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.
*
Expand Down Expand Up @@ -260,16 +250,6 @@ public Quaternion exp() {
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.
*
Expand Down
2 changes: 1 addition & 1 deletion wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,7 @@ public Rotation2d plus(Rotation2d other) {
}

/**
* 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>
Expand Down
44 changes: 19 additions & 25 deletions wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,31 +285,23 @@ public Rotation3d(Rotation2d rotation) {
}

/**
* 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.
* Returns this rotation relative to another rotation.
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation3d minus(Rotation3d other) {
return rotateBy(other.unaryMinus());
public Rotation3d relativeTo(Rotation3d other) {
// q_f = q_transform q_0
// q_transform = q_f q_0⁻¹
return new Rotation3d(m_q.times(other.m_q.inverse()));
}

/**
* 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());
}

Expand All @@ -320,16 +312,7 @@ public Rotation3d unaryMinus() {
* @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);
}

/**
Expand Down Expand Up @@ -561,7 +544,18 @@ public int hashCode() {

@Override
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(Math.clamp(t, 0, 1)));
// 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. */
Expand Down
7 changes: 3 additions & 4 deletions wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ public Transform3d(Pose3d initial, Pose3d last) {
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
.rotateBy(initial.getRotation().inverse());

m_rotation = last.getRotation().minus(initial.getRotation());
m_rotation = last.getRotation().relativeTo(initial.getRotation());
}

/**
Expand Down Expand Up @@ -288,8 +288,7 @@ public Transform3d inverse() {
// 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
Expand Down
12 changes: 6 additions & 6 deletions wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ public Odometry3d(
Kinematics<T, ?, ?> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
m_kinematics = kinematics;
m_pose = initialPose;
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
m_gyroOffset = m_pose.getRotation().relativeTo(gyroAngle);
m_previousAngle = m_pose.getRotation();
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}
Expand All @@ -66,7 +66,7 @@ public Odometry3d(
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) {
m_pose = pose;
m_previousAngle = m_pose.getRotation();
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
m_gyroOffset = m_pose.getRotation().relativeTo(gyroAngle);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
}

Expand All @@ -76,7 +76,7 @@ public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) {
* @param pose The pose to reset to.
*/
public void resetPose(Pose3d pose) {
m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation()));
m_gyroOffset = m_gyroOffset.rotateBy(pose.getRotation().relativeTo(m_pose.getRotation()));
m_pose = pose;
m_previousAngle = m_pose.getRotation();
}
Expand All @@ -96,7 +96,7 @@ public void resetTranslation(Translation3d translation) {
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation3d rotation) {
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation()));
m_gyroOffset = m_gyroOffset.rotateBy(rotation.relativeTo(m_pose.getRotation()));
m_pose = new Pose3d(m_pose.getTranslation(), rotation);
m_previousAngle = m_pose.getRotation();
}
Expand All @@ -121,8 +121,8 @@ public Pose3d getPose() {
* @return The new pose of the robot.
*/
public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
var angle_difference = angle.minus(m_previousAngle).toVector();
var angle = gyroAngle.rotateBy(m_gyroOffset);
var angle_difference = angle.relativeTo(m_previousAngle).toVector();

var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
var twist =
Expand Down
8 changes: 4 additions & 4 deletions wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ 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);
return translation.RotateBy(to.m_rotation.RelativeTo(from.m_rotation));
}

/**
Expand All @@ -99,7 +99,7 @@ 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);
return rotation.RotateBy(to.m_rotation.RelativeTo(from.m_rotation));
}

/**
Expand Down Expand Up @@ -128,10 +128,10 @@ 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;
const auto coordRot = to.m_rotation.RelativeTo(from.m_rotation);
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
coordRot.Inverse().RotateBy(transform.Rotation().RotateBy(coordRot))};
}

private:
Expand Down
24 changes: 17 additions & 7 deletions wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
});
Expand All @@ -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;
});
Expand Down Expand Up @@ -313,7 +323,7 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {

constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
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 {
Expand Down
18 changes: 0 additions & 18 deletions wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand Down Expand Up @@ -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.
*
Expand Down
3 changes: 1 addition & 2 deletions wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Loading
Loading