Hi I was working on a project and think I may have found a bug in Rotation2d, where values are incorrectly being wrapped into a 0-360 degree range. It also seems to apply for Rotation3d.

The Rotation2d class says “The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will return 361 degrees. This allows algorithms that wouldn’t want to see a discontinuity in the rotations as it sweeps past from 360 to 0 on the second time around.”

This works when constructing a Rotation2d but fails for some of the math methods. The times method works as expected but the plus method wraps the sum value back to 0 - 360.

Is this the intended result? I feel like it is not and the plus method is not supposed to wrap the value but I may be missing something.

Rotation3d also has 0 - 360 wrapping but it happens in all methods. Both the plus and times methods do the wrap, so it seems more intentional here.

Below is some code in which you can test this. I would expect that all of these print true but only the first one does.

The docs for Rotation2d.plus() say it will wrap. Rotation2d only preserves the input angle in the sense that the constructor won’t wrap it.

Fundamentally, Rotation2d is not an Euler angle; it’s a 2D unit vector. Thus, no assumptions should be made about the Euler angle it returns later. Rotation3d already works like this, but Rotation2d special-cases it because 254’s version did it that way first. I’ve wanted to get rid of that behavior, but was told we couldn’t because it would break too much user code. In my opinion, if a user wants Euler angle behavior, they should just use an Euler angle (double or Angle) instead of Rotation2d.

Why do you want the angle to “not wrap” in the first place? I mean, back then when encoders don’t have getVelocity functions, some teams may want to find the derivatives, but they can do it this way:

And the code above demonstrates why you want a wrap-around. Because if does not, 359 deg - 1 deg = 358 deg. But what is the change in rotation when you travel from 1deg to 359deg? It’s -2 deg.

Our team has had a lot of problems with angle units conversions in the past and it has caused a lot of bugs, so when we tried Rotation2d this year it was really helpful for us. One of the main problems we encountered was when we tried to describe our drivetrains rotational velocity with Rotation2d. The drivetrain rotates at 640 deg/s and so if we try to add a rotational velocity to it in any way it effectively becomes rotating at 220 deg/s which is not what we want.

This becomes more apparent in Rotation3d because for look at the following code. In this case the moment you do rotationalVelocity.times(dt) your rotationalVelocity effectively becomes 220 degs/s.

Rotation3d rotationalVelocity = new Rotation3d(640, 0, 0);
swerveRotation = swerveRotation.plus(rotationalVelocity.times(dt));

This is what we did in previous years, but it created a lot of bugs where functions would expect inputs in one unit and people would input a different unit. If Rotation2d is implemented without any wrapping then it can also represent derivatives of an angle without any problems which is super helpful.

My problem with using Angle for example is that it means using multiple types to represent angles. We use Translation2d for example and it has a getAngle method which returns a Rotation2d. If we wanted to use Angle then we would have to convert that Rotation2d into angle each time.

Unit types are the correct fix for this, not Rotation2d. Unfortunately, Java is worse at this than C++ due to language and target platform limitations.

Rotation2d doesn’t implicitly convert to an Euler angle. You have to use Degrees() or Radians().

It’s correct in the sense that the 2D orientation aliases on the SO(2) manifold. SO(2) just isn’t the orientation representation they care about.

We initially based Rotation2d and all the other manifold geometry methods on Sophus (Sophus/sophus/so2.hpp at main · strasdat/Sophus · GitHub), so the idea is that Rotation2d is a member of SO(2). There are an infinite number of Euler angles that can map to an individual SO(2) rotation, so the convention we adopted was to always* wrap the Euler angle we provide back to the user to remove this ambiguity.

But conceptually, we want it to be as if “getRadians()” was always computed anew from the SO(2) rotation and so adopted a canonical wrapping scheme. If you want to do math on Euler angles without adopting SO(2) semantics, then you should use doubles or a first class Angle type.

* as a (very small and likely premature) performance optimization we did allow construction without enforcing wrapping as a deliberate breach of this contract only because in many cases the caller trivially knows they aren’t wrapped, or knows that whatever math they are doing later won’t care.

Just to add a data point… the NavX2 AHRS class inherits from Gyro (a WPILib class which has been deprecated).

That Gyro class has a getRotation2d method which returns a Rotation2d object and is continuous (i.e. it doesn’t wrap). This is actually the method we were using to get continuous rotation signal from our gyro.

The AHRS class also has a getYaw method which will return a double from -180 to 180 (in degrees obviously).

In this example you appear to be assuming the input angles are already wrapped to (0, 360), which makes this moot.

As Tyler said:

Here’s an example:

What is the average angular velocity of the robot in this scenario?
What happens when you wrap the difference to (-pi, pi) when calculating average angular velocity?

Ok, so if I wanted to define 3d angular velocities what is the best non-wrapping, unit safe way to do it?

Using a Rotation3d won’t work because it is wrapping.

I can try using the Units library but this is really verbose and I have to define the roll, pitch, and yaw as seperate variables as opposed to having one variable which stores them all. What I put below is also technically not unit safe because it is possible to change the Units.Radians.of to Units.Degrees.of. I think I may be missing something with how multiplication works for units, but this is the only way I could get it to compile.

You could create your own AngularVelocity2d (or 3d) class to wrap the multiple dimensions into a single object (similar to how Rotation2d/3d works, but with velocity in mind rather than (angular) position)

Your approach of integrating the body rates separately is not correct.

Something like the following is more accurate, but I haven’t finished writing tests for it.

/**
* Projects the rotation forward by integrating the given body rates over time.
*
* @param rollRate The body roll rate in radians per second.
* @param pitchRate The body pitch rate in radians per second.
* @param yawRate The body yaw rate in radians per second.
* @param dt The time over which to integrate in seconds.
* @return The rotation in the world frame projected forward.
*/
Rotation3d integrate(double rollRate, double pitchRate, double yawRate, double dt) {
// qₖ₊₁ = qₖ exp(Ω dt) where Ω = 0 + ω_x/2 i + ω_y/2 j + ω_z/2 k
//
// https://math.stackexchange.com/a/2099673
var Ω = new Quaternion(0.0, rollRate / 2, pitchRate / 2, yawRate / 2);
return new Rotation3d(m_q.times(Ω.times(dt).exp()));
}

/**
* Projects the rotation forward by integrating the given body rates over
* time.
*
* @param rollRate The body roll rate.
* @param pitchRate The body pitch rate.
* @param yawRate The body yaw rate.
* @param dt The time over which to integrate.
* @return The rotation in the world frame projected forward.
*/
constexpr Rotation3d Integrate(units::radians_per_second_t rollRate,
units::radians_per_second_t pitchRate,
units::radians_per_second_t yawRate,
units::second_t dt) const {
// qₖ₊₁ = qₖ exp(Ω dt) where Ω = 0 + ω_x/2 i + ω_y/2 j + ω_z/2 k
//
// https://math.stackexchange.com/a/2099673
Quaternion Ω{0.0, rollRate.value() / 2, pitchRate.value() / 2,
yawRate.value() / 2};
return Rotation3d{m_q * (Ω * dt.value()).Exp()};
}

The Stack Overflow link in the code says it’s from the quaternion version of Euler’s formula: \mathbf{q} = e^{\frac{\theta}{2}(u_x \mathbf{i} + u_y \mathbf{j} + u_z \mathbf{k})}. The link chain leads to Wikipedia.