ResetYaw in MecanumDrive constructor?

[Vector5233]

RR FTC Version

1.1x

Observed Behavior

I posted this in the wrong version earlier.

“ So we have a Control Hub (different from Mr "Invalid Quaternion" Control Hub that we are grumpy with) that starts life with an offset most of the time. That makes it unusable. When running LocalizationTest, the initial heading is a random angle. BUT, if we turn the bot, the headings are correct relative to the initial offset.

Then today when we installed an Adafruit IMU, it also starts with an offset. In particular, it starts the LocalizationTest OpMode showing -5 deg. If we stop the OpMode, turn the bot 90 deg, and restart the OpMode, it shows an initial heading of 85 deg. If we restart the robot, the offset returns to -5 deg regardless of physical orientation. Similar results occur with different turns of the bot. Once the OpMode begins, the measured angles are correct relative to the offset.

I went poking around in the IMU API and behold, there is a resetYaw() method. I added that method call at line 217 of MecanumDrive, right after imu.initialize(), and it makes the offset go away for both devices. Initial heading is 0.0 deg regardless of physical orientation or robot reset history.

(1) Is there any reason not to do that? (2) Should that line become a permanent part of the MecanumDrive class?”

Tuning Files

No response

Robot Logs

No response

[rbrott]

(1) Is there any reason not to do that?

I don't think so. Calling resetYaw() immediately after imu.initialize() is fine from a RR perspective.

(2) Should that line become a permanent part of the MecanumDrive class?

I'm not sure about this. The provided localizers all operate on heading deltas, so they shouldn't care about the initial yaw. Are you using one of those localizers or a custom one?

[Vector5233]

A provided one.

[Vector5233]

Update: using the resetYaw() fix, all auto code works correctly where it had before not run well at all.

[rbrott]

Can you attach your code / MecanumDrive.java file?

[Vector5233]

Here it is. We believe we've made no changes except to the PARAMS class and the one resetYaw() call. Let us know if something seems not in order.

[rbrott]

Can you attach the robot log of a spline test without the resetYaw() call when you get a chance?

[Vector5233]

Ok. That’ll have to be after our qual tomorrow

[Vector5233]

Haven't forgotten about this, we've just been out of school for a week because of snow.

[Vector5233]

OK, here we go. We ran SplineTest with the .resetYaw() "Match-0-SplineTest" and followed it up with SplineTest without the .resetYaw() "Match-0-SplineTest2". The behavior was strikingly different. The spline was followed accurately with .resetYaw() and went to pieces without. Hopefully the log files will tell you what you need to know.

Control Hub 5233-RC - FIRST_matchlogs_Match-0-SplineTest2.txt Control Hub 5233-RC - FIRST_matchlogs_Match-0-SplineTest.txt

[rbrott]

Thanks for running the tests. Unfortunately, I can't use match logs -- I need RoadRunner-specific robot logs instead (see the link from my previous comment). Hopefully you don't need to run the test again and can just download the robot logs from the webpage.

[Vector5233]

Got your logs. First was a successful run with .resetYaw() - we've had zero problems since adding that line. Second is a successfulish run without .resetYaw() - there was a heading glitch 3/4 through turn that fixed itself. Third was an unsuccessful run without .resetYaw() about 1 min later with no code changes or bot changes. Had to change the file extension to .txt.

SplineTestLog w resetYaw.txt SplineTestLog wo resetYaw 1.txt SplineTest wo resetYaw 2.txt

[j5155]

I also noticed issues where the yaw would be tracked between runs when not using resetYaw. This is with an older control hub that worked fine with 0.5.

[Vector5233]

I suspect it's a bug in imu.initialize() and imu.resetYaw() is simply kludging it to good.

[rbrott]

I think I sort of have a handle on what's going on. The match logs were actually more helpful here! The crucial line from the run without resetYaw() is

12-31 19:21:49.079  3503  3961 W BNO055IMU (new): getRobotOrientationAsQuaternion(): Failed to retrieve valid quaternion from IMU. Returning the identity quaternion.

Apparently the IMU can just return garbage data soon after it's initialized? There are some hints sprinkled around that this is possible:

    // If at all possible, we want to get an up-to-date quaternion when resetting the yaw, so we
    // keep trying until we reach a timeout, at which time we give up and just use the most
    // up-to-date data we have (which may be arbitrarily stale).

and

During object construction the timeout is 500ms, and during normal operation the timeout is 50ms for the BHI260AP, and 100ms for the BNO055 (which returns invalid data for about 50-70ms after initialization)

Perhaps 50-70ms was too optimistic in some cases. I believe initialization occurs on each op mode run, so you might hit it even if another op mode has run since the last robot restart (though that's in tension with yaw being "tracked"/persisted between runs).

So why does resetYaw() help? Turns out it keeps reading until it gets something that's valid.

        do {
            try {
                robotOrientationWithoutYawOffset = getRobotOrientationAsQuaternionOrThrow(imuCentricOrientationSupplier, false);
                break;
            } catch (FailedToRetrieveQuaternionException ignored) { }
        } while (timer.milliseconds() < timeoutMs);

Seems like the first IMU read should do a similar dance. Just calling resetYaw() seems reasonable. Continuing with the identity quaternion is about as reasonable as anything else if we somehow actually time out. Pretty sad that this doesn't display a warning to the user -- the Road Runner version could do that at least to leave some trace.

Bonus round: why is RR 0.5 seemingly immune to this nonsense? Well the (barely functional) localizer waits to query the IMU until the first update which is usually when a trajectory is actually being followed. Presumably enough time has passed since IMU init for the readings to be stable.

Should RR 1.0 just mimic that behavior? It's a close call, but I like the flexibility of (1) being able to run odometry in init and (2) seeing a warning message in init when the IMU reads time out (and having a chance to init again before the robot does something strange).

[Vector5233]

Ahh… some teams apparently want to carry over pose from auto into teleop

https://discord.com/channels/225450307654647808/1200034062061019146/1200434359753261106

[j5155]

Ahh… some teams apparently want to carry over pose from auto into teleop

https://discord.com/channels/225450307654647808/1200034062061019146/1200434359753261106

see https://learnroadrunner.com/advanced.html#transferring-pose-between-opmodes for the proper way to do this. Easily converted to 1.0

[NoahAndrews]

Ahh… some teams apparently want to carry over pose from auto into teleop discord.com/channels/225450307654647808/1200034062061019146/1200434359753261106

see learnroadrunner.com/advanced.html#transferring-pose-between-opmodes for the proper way to do this. Easily converted to 1.0

@j5155 I actually explicitly designed the new IMU interface to maintain the heading's physical zero point until it's explicitly reset (even if you re-initialize it), so you don't need to do this anymore unless you're using the old BNO055IMU interface (which was probably the only interface available when that was written).

[j5155]

Ahh… some teams apparently want to carry over pose from auto into teleop discord.com/channels/225450307654647808/1200034062061019146/1200434359753261106

see learnroadrunner.com/advanced.html#transferring-pose-between-opmodes for the proper way to do this. Easily converted to 1.0

I actually explicitly designed the new IMU interface to maintain the heading zero point until it's explicitly reset, so you don't need to do this anymore unless you're using the old BNO055IMU interface (which was probably the only interface available when that was written).

Unfortunately if you don't explicitly reset the IMU it seems like you get garbage data and strange headings, as was the primary focus of this issue. Even without that garbage data, the default behavior of the robot seeming to remember it's location between opmodes led to some very confusing testing behavior before the resetyaw addition was implemented in RR; for example, you couldn't pick up the robot and move it to the other side of the field and try to run the same auto, as it would try to turn back around and slam into the wall. This would also lead to strange behavior when moving between practice fields that weren't facing the same way. The solution I linked has equivalent behavior for the auto to teleop transition, but does not track rotation while an opmode is not running so you can actually pick up the bot and move it between opmodes.

[NoahAndrews]

Your auton should absolutely be calling resetYaw() to prevent unpredictable autonomous runs, and the latest version of Roadrunner FTC has a workaround for the invalid data issue, so you don't need to worry about that.

The point is that when you do want to share the position between runs, you can do that by just not calling resetYaw() in your TeleOp program, rather than having to store the current heading externally (which among other potential issues, will break if your robot somehow gets moved between OpModes).

[j5155]

Your auton should absolutely be calling resetYaw() to prevent that issue, and the latest version of Roadrunner FTC has a workaround for the invalid data issue, so you don't need to worry about that either.

From the sounds of this issue, Roadrunner's workaround was just to call resetYaw()? In my opinion, the localizer control stuff should stay abstracted in the drive classes of Roadrunner rather then making it part of every opmode.

The point is that when you do want to share the position between runs, you can do that by just not calling resetYaw() in your TeleOp program, rather than having to store the current heading externally (which among other issues, will break if your robot somehow gets moved between OpModes).

"Breaking" if the robot moves between opmodes is almost the entire point. A robot is highly unlikely to get moved between opmodes in a real match, and in practice when I intentionally move the robot between opmodes I generally do not want it to remember the yaw between them. Additionally, this solution also stores the position data, which can be useful for automatic aiming and alignment in teleop.

I was under the impression that this option had few drawbacks; I would be curious to learn about the "other issues" that you mention.

[NoahAndrews]

Caveat: The method listed on learnroadrunner.com is more versatile, as it stores the whole Roadrunner pose, which includes positional data (and maybe incorporates heading data from the wheels?), so that's actually probably still what you want.

[NoahAndrews]

Heh, ninja'd.

As for the "other potential issues" (I had edited it from just "other issues"), I'm not familiar with the inner workings of Roadrunner, but I think naively adding a heading value could theoretically introduce some error, at least under certain circumstances, given the weirdnesses of Euler angles. It's probably fine/close enough as long as you aren't doing anything weird and are using an Euler angles variant that starts with Z (if you're using the getRobotYawPitchRollAngles() method you don't have to worry about that).

[NoahAndrews]

From the sounds of this issue, Roadrunner's workaround was just to call resetYaw()?

No, the workaround was to use a similar technique to that used in resetYaw() to keep getting the heading until you get a valid value. Here's the code:

https://github.com/acmerobotics/road-runner-ftc/blob/fa9abf03cc2a7b67ac04b4ee01a3ff9c5c1b8af7/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/LazyImu.kt#L33-L44

[NoahAndrews]

As for the "other potential issues" (I had edited it from just "other issues"), I'm not familiar with the inner workings of Roadrunner, but I think naively adding a heading value could theoretically introduce some error, at least under certain circumstances, given the weirdnesses of Euler angles. It's probably fine/close enough as long as you aren't doing anything weird and are using an Euler angles variant that starts with Z (if you're using the getRobotYawPitchRollAngles() method you don't have to worry about that).

To expand a little bit, you ideally don't want to do any manipulations on Euler angles. The new IMU interface avoids this by getting the orientation as a quaternion, doing all transformations while the data is in the quaternion format, and only converting to Euler angles once the final orientation has been computed.