Make MecanumDrive not final or alternative method for custom absolute localization?

[j5155]

RR FTC Version

0.1.13, quickstart 03d95d7

Observed Behavior

Currently, MecanumDrive is final, preventing classes from extending it. This stops me from easily distributing my vision relocalizer system. The only reason this has to extend MecanumDrive is because there is no better way I could see to do custom absolute localization; localizers only return relative data.

Also, as an off-topic question; is RR planning to implement official AprilTag support, or would a PR that adds it to the quickstart be accepted?

Tuning Files

No response

Robot Logs

No response

[rbrott]

Currently, MecanumDrive is final, preventing classes from extending it.

True, though end users can easily remove the final.

This stops me from easily distributing my vision relocalizer system.

I don't recommend distributing "out-of-tree" quickstart modules. I don't give any guarantees about the stability of the quickstart, and I want teams to modify it to fit their purposes.

Also, as an off-topic question; is RR planning to implement official AprilTag support

I do not plan on implementing AprilTag support myself.

or would a PR that adds it to the quickstart be accepted?

I could be convinced to add it to the quickstart and adapt the interfaces accordingly. I do have pretty high standards for the code and would want something that's both theoretically sound and somewhat practically proven. A full AprilTag localizer in particular would require dealing with a bunch of stuff (filtering/latency compensation, camera calibration -- intrinsics & extrinsics, tag ambiguity to name some). I worry that it wouldn't be totally foolproof, and that's a pretty important requirement for all code going in the quickstart as I've learned.

On the other hand, my standards for the docs aren't quite so high. I'm happy to take contributions styled as official sections or, more casually, as "community guides" (see https://rr.brott.dev/docs/v1-0/guides/centerstage-auto/). Of course I will still retain some editorial control.

And you're free to disregard all of my gatekeeping and market your additions separately 🙂

[j5155]

As another update here, the SparkFun vision-based odometry sensor also uses absolute localization (it has both an IMU and an optical tracking sensor and automatically sums them to return an absolute pose) It would be great if there was a better way of incorporating absolute localization then making modifications to MecanumDrive. I could do that for this SparkFun sensor like I did for the AprilTag relocalization, but then they would be incompatible with each other. SparkFun sensor product page is https://www.sparkfun.com/products/24904 and the FTC code for it is https://github.com/sparkfun/SparkFun_Qwiic_OTOS_FTC_Java_Library/

[EddieDL]

@j5155 Do you have an example of what you did to incorporate an absolute localization system?

From what I can tell it should be as easy as creating our own localizer. The issue I am having is that the localizer now returns a Twist2dDual, and it isn't clear what that is exactly. Maybe @rbrott could provide some guidance?

Edit: I guess the simpler solution is to just modify the UpdatePoseEstimate. Which is where you were hoping to just "extend" what the Quickstart provides but it isn't easy to do it. @rbrott I do think that providing better extension options would be helpful for teams. Right now merging updates to the Quickstart isn't easy.

[j5155]

@j5155 Do you have an example of what you did to incorporate an absolute localization system?

From what I can tell it should be as easy as creating our own localizer. The issue I am having is that the localizer now returns a Twist2dDual, and it isn't clear what that is exactly. Maybe @rbrott could provide some guidance?

Edit: I guess the simpler solution is to just modify the UpdatePoseEstimate. Which is where you were hoping to just "extend" what the Quickstart provides but it isn't easy to do it. @rbrott I do think that providing better extension options would be helpful for teams. Right now merging updates to the Quickstart isn't easy.

My modifications are available at https://github.com/JDHS-ftc/apriltag-quickstart and https://github.com/jdhs-ftc/Sparkfun-OTOS-quickstart I am also working on Pinpoint support in a branch in the Sparkfun QuickStart; the code is done, I'll just need to fix the logic bugs once my Pinpoint gets in.

[rbrott]

As another update here, the SparkFun vision-based odometry sensor also uses absolute localization (it has both an IMU and an optical tracking sensor and automatically sums them to return an absolute pose) It would be great if there was a better way of incorporating absolute localization then making modifications to MecanumDrive.

The SparkFun/goBILDA odometry sensors seem pretty popular and provide pose sources similar to normal odometry systems that first-party support makes sense. (Field AprilTags are very different in their update frequency, availability/visibility, and pose-dependent accuracy.)

The issue I am having is that the localizer now returns a Twist2dDual, and it isn't clear what that is exactly.

A twist is the difference/delta between two poses, and the dual part just means it also has derivatives/velocities. Odometry systems can be thought of producing a twist in each update loop from encoder deltas and then applying that twist to the previous pose to produce the next estimate.

I do think that providing better extension options would be helpful for teams. Right now merging updates to the Quickstart isn't easy.

What would these extension options look like? I suppose maybe you're annoyed that I somewhat carefully restrict the feature set of the first-party quickstart?

[EddieDL]

@rbrott First-party support would be great! we are trying to figure it out right now.

Thanks for the Twist2dDual explanation. That makes sense.

The main issue I have is that you have to modify the Quickstart class, which makes it harder to update when bugs are fixed in the QuickStart. It would be really nice if you could extend the drive classes. This is because in most cases you really don't have to do a ton to the standard classes except "configuring" them.

[EddieDL]

@rbrott I looked at this more and I think the appropriate way to do what we are trying to do with absolute encoders is to implement a custom localizer. That would work like all of the other localizers.

However, I believe that will break all of the tuning logic because they are all dependent on the specific features of each localizer. I am not sure if the correct thing to do is to reimagine each tuning program in the context of this new localizer.

If you have any thoughts that would be appreciated.

[j5155]

@rbrott I looked at this more and I think the appropriate way to do what we are trying to do with absolute encoders is to implement a custom localizer. That would work like all of the other localizers.

However, I believe that will break all of the tuning logic because they are all dependent on the specific features of each localizer. I am not sure if the correct thing to do is to reimagine each tuning program in the context of this new localizer.

If you have any thoughts that would be appreciated.

Localizers in Roadrunner 1.0 are robot centric and return individual twists per each loop. There’s no great way to do absolute field centric position input with the way localizers are right now.

[rbrott]

I am not sure if the correct thing to do is to reimagine each tuning program in the context of this new localizer.

First-class tuning support is part of what I imagine with first-party support.

Localizers in Roadrunner 1.0 are robot centric and return individual twists per each loop. There’s no great way to do absolute field centric position input with the way localizers are right now.

True, though this isn't set in stone, and I imagine this would change with first-party support.

[EddieDL]

@rbrott is the PoseVelocity2d returned from UpdatePoseEstimate field relative or robot relative? Based on what @j5155 did I think it is but would like to confirm.

All of the Odometry Tools that I have seen include velocity values. If you were to store off the original pose that was set, wouldn't that allow you to convert back to robot centric in the localizer? Then you can pass back the appropriate twists and you could better fit with the current structure.

I am not sure exactly where to start thinking/looking at how to update all of the tuning logic so that it would work with absolute odometry. I think you would only want to do the following tuning programs:

• ForwardRampLogger - to get kS and kV
• AngularRampLogger - But it would only help you empirically determine the location of the odometry module.
• ManualFeedforwardTuner
• ManualFeedbackTunder

What @j5155 is just leaving all of this alone. Meaning that all of the tuning will be done as if the robot is using wheel encoders, even though that isn't what it will actually be doing. I think what you really want is to stop using the dead wheel encoders and just use the Pose and PoseVelocity. But I am not exactly sure how we would want to do that in a way that makes it work well with the existing approach.

If this is a reasonable approach I will think more on this. In general, I think the problem is that the current logic is calling things "enc" etc. It is really just the measured position and velocity.

[rbrott]

is the PoseVelocity2d returned from UpdatePoseEstimate field relative or robot relative?

Robot relative

If you were to store off the original pose that was set, wouldn't that allow you to convert back to robot centric in the localizer? Then you can pass back the appropriate twists and you could better fit with the current structure.

Not sure what this means

I am not sure exactly where to start thinking/looking at how to update all of the tuning logic so that it would work with absolute odometry.

I put some thoughts in https://github.com/acmerobotics/road-runner-ftc/issues/8

[EddieDL]

@rbrott I looked over things and have done some more testing with the SparkFun solution. The ultimate decision we have made is to switch away from that absolute localization solution to the GoBuilda Pinpoint solution.

When looking at that solution it looks like it would be really easy to integrate it into the all of the tuning logic and fairly simple to add it into pose estimation logic.

However, we have run into more issues were things are closed off from our customizations and the current logic makes some assumptions that aren't really required. Specifically, the problems are in the Encoder abstraction.

1. The Encoder abstraction is sealed so we can't create a version that works with the GoBuilda Pinpoint system. The Pinpoint system gives you raw access to their x and y encoders so it should be as simple as making a GoBuilda compatible Encoder and then creating a customized TwoWheelLocalizer. After that everything should just "work".
2. The current Encoder abstraction requires a DcMotorController. When looking at the usage of this object, it doesn't really need the DCMotorController. What it really needs is a "serial number" that it the tuning logic is using to cache some data. I would propose the Encoder interface require getSerialNumber() or getKey() method instead of requiring a DCMotorController.

Would you be open to changing these? I would also be willing to work on a PR for this, but I am not 100% sure where that code exists, perhaps it is not public.

[EddieDL]

I found where the code is for the Encoder library. If we can't update this in the main branch, how would I incorporate a custom version of that into our code?

[pxdal]

@rbrott I looked over things and have done some more testing with the SparkFun solution. The ultimate decision we have made is to switch away from that absolute localization solution to the GoBuilda Pinpoint solution.

When looking at that solution it looks like it would be really easy to integrate it into the all of the tuning logic and fairly simple to add it into pose estimation logic.

However, we have run into more issues were things are closed off from our customizations and the current logic makes some assumptions that aren't really required. Specifically, the problems are in the Encoder abstraction.

1. The Encoder abstraction is sealed so we can't create a version that works with the GoBuilda Pinpoint system. The Pinpoint system gives you raw access to their x and y encoders so it should be as simple as making a GoBuilda compatible Encoder and then creating a customized TwoWheelLocalizer. After that everything should just "work".
2. The current Encoder abstraction requires a DcMotorController. When looking at the usage of this object, it doesn't really need the DCMotorController. What it really needs is a "serial number" that it the tuning logic is using to cache some data. I would propose the Encoder interface require getSerialNumber() or getKey() method instead of requiring a DCMotorController.

Would you be open to changing these? I would also be willing to work on a PR for this, but I am not 100% sure where that code exists, perhaps it is not public.

If you're planning on getting the raw encoder positions and using them with the TwoWheelLocalizer, why not just plug your encoders straight into the Control Hub instead of going through the Pinpoint, so the setup and tuning is all standard? Maybe I'm not understanding what you're proposing.

[EddieDL]

@pxdal For "rough" tuning purposes only. Roadrunner's current tuning algorithms are built to work with raw encoder values.

I have something pretty much working and it seems to work really well. The challenge is that it isn't very agnostic.