Auto builder

[mjansen4857]

Functionality in PPLib and the GUI that would allow the creation of entire autos without writing more code.

• Constructed with everything needed for path following commands

  • Pose/etc suppliers
  • Output consumers
  • PID constants
  • Kinematics
  • Event map
  • Etc

• Different versions for the different path following commands

  • SwerveAutoBuilder
  • MecanumAutoBuilder
  • RamseteAutoBuilder
  • etc

• Trigger event commands at stop points + start/end point

  This will need to wrap the event commands in another command so that they are not directly scheduled.

• Create entire auto from a path file. i.e. Command auto = autoBuilder.build("Path Name") returns a command group that runs like:

  • Reset odometry for start of path
  • Trigger events for start point
  • Follow path segment 1
  • Wait for configured wait time
  • Trigger events for stop point
  • Follow path segment 2
  • ...

• Configurable wait behavior

  • Wait before events
  • Wait after events
  • Deadline for events

• Configure stop point event behavior

  • Run sequentially
  • Run parallel

PathPlanner 2024 should have a bit of a re-work in mind to expand on auto builder stuff even more. Essentially, the GUI should allow the organization of events similarly to command groups. The can be nested and grouped in any combination of sequential/parallel as needed.

[WarrenReynolds]

This is some pretty cool, serious next level stuff you are suggesting here. While there would be a massive learning curve to understand what is going on under the hood to make sure that command subsystem requirements don't break everything. I think this would enable teams to create amazing auto routines.

It certainly would have helped our team in our first tournament for 2022 where we aimed for the sky but ended up having to not move in auto because we didn't quiet reach the sky.

I just wish I was smart enough to help contribute to the project.

Are you planning to implement this for 2023 or 2024?

[mjansen4857]

I think it's doable for 2023 but we'll see if it works without much issue.

I believe I can build the command group in a way that will make it so you don't really have to think about it. There's a few restrictions I can think of but won't know for sure until I implement it. I'll make sure to document any. It will still have the same restriction of event markers where a marker can't trigger a drive command or it will interrupt the path following command. However, you should be able to trigger drive commands, i.e. turn to target with vision, at stop points without issue.

[WarrenReynolds]

When I think about how this would be done, taking into consideration of how command groups need to be declared in code for all the requirements stuff to work, it makes my head hurt trying to work out how you are going to implement the creation of a command group on the fly so that it can be wrapped up and run from start to finish as a single command.

I look forward to seeing the implementation and if I can help in any way I more than happy too. Happy to be a guineapig if you want some outside teams to test out beta code.

[mjansen4857]

taking into consideration of how command groups need to be declared in code for all the requirements stuff to work, it makes my head hurt trying to work out how you are going to implement the creation of a command group on the fly so that it can be wrapped up and run from start to finish as a single command

Command groups have an addCommands method which I can use to add commands. To get the requirements declared right I can just pass the requirements of the command in the event map to the instant command that will schedule it and everything should hopefully just work. We'll see.

[WarrenReynolds]

In regards to "Reset Odometry at start of path", this is what killed us for our first regional in 2022 so I'd like to offer what our problem was and how we fixed it in case this issue arises in your implementation of "Reset Odometry as Start of path"

We were reading the starting pose of the robot from the first pose of the auto path that was selected in the auto chooser so that we could reset the pigeon fused heading and the odometry position of the robot to match the pose in the first trajectory point.

Initially we reset the fused heading and in the next line of code after this we updated the odometry coordinates/pose. On rare occasions the periodic odometry updating code that executed after the odometry position had just been reset would call GetRobotHeading and the value it returned was the heading of the robot before the fused heading had been reset. (This happens because of the way CAN devices broadcast stuff and the RIO caches it all so that it can be returned without sitting and waiting for a status frame update if someone wants to get info from a CAN device). Then on the next periodic odometry updating code execution the GetRobotHeading call would return the correct value (ie the one that it had just been reset to). This had the effect of making the robot odometry position rotate by a random amount in space which completely killed the directions that our autos headed off in.

What we ended up doing was splitting the "reset odometry at start of path" into two commands. The first thing was to run a command that got the initial heading from the first trajectory point of the selected auto and then call SetFusedHeading on the pigeon with this value. This command wouldn't end, until the heading value that was returned from a getFusedHeading call was exactly (or nearly exactly) the same as the value that was sent to SetFusedHeading. After this command finished we then launched the command to initialise the robot odometry position, knowing that future calls to update odometry would always have accurate robot heading values returned when getFusedheading was called. I don't know how other teams got around this issue, maybe then didn't reset fused heading and maintained an heading offset in a member variable or they were lucky enough that their calls to set Odometry position were delayed enough that this issue never arose.

Anyway, bottom line here is, I suggest a command call is used to "reset odometry at start of path" instead of calls to a subsystem methods to reset fused heading and set odometry position.

[mjansen4857]

You don’t actually need to reset your gyro at all, which is how most teams get around that. When you reset your odometry it stores a gyro offset to automatically account for it.

[WarrenReynolds]

That makes sense. However, for us, we used the gyro value in conjunction with distance and bearing to target (from a limelight) to do odometry position corrections throughout the match (every time we were still and could see the target we provided a sensor measurement update to the odometry). That way we could press a button to auto aim and the robot would rotate to face the target from anywhere on the field at anytime during the match.

If we didn't update the odometry throughout the match with this method the odometry would drift too far off as the match progressed. This year (2023) we hope to be able to localize from the April Tags around the field so the true gyro heading shouldn't be needed anymore.

What about field centric driving for Swerve Drive Robots? Do they query odometry to work out which way they are facing?

[MrRedness]

What about field centric driving for Swerve Drive Robots? Do they query odometry to work out which way they are facing?

You could do it either way. We just wrote a few lines of code in our gyro wrapper that handled an offset, which we set ourselves before resetting odometry. Also, we have our code written to zero the gyro on first call, but it waits until successful zero (never noticeable delay) before continuing, or at least we never had issues with it taking longer.

In summary, steps

1. Take starting heading/holonomic rotation of trajectory, subtract this value from all future gyro calls
2. Reset odometry with start pose, and get angle from gyro (which should be 0 - start rotation)
3. In teleop, 0 = front

[mjansen4857]

The rotation component of the pose you get from odometry is just the gyro angle with an offset applied based on what it was when you reset. So doing these steps is basically just doing what the odometry already does. You can use this rotation for everything you’d use your gyro for and it will take care of the offsets for you and you never need to reset the gyro. A lot easier to deal with.

Also a bit unrelated, but if you’re using the beta pplib you can use getInitialHolonomicPose to avoid creating it yourself.

[MrRedness]

The rotation component of the pose you get from odometry is just the gyro angle with an offset applied based on what it was when you reset. So doing these steps is basically just doing what the odometry already does. You can use this rotation for everything you’d use your gyro for and it will take care of the offsets for you and you never need to reset the gyro. A lot easier to deal with.

Yeah, I definitely noticed after the season, but I didn't think of it during the season. For most people, that is the better solution.

Also a bit unrelated, but if you’re using the beta pplib you can use getInitialHolonomicPose to avoid creating it yourself.

Yup! I noticed that and am very grateful. Thanks a lot for all the work you're pouring into this!

[WarrenReynolds]

We wrote both the heading reported by the Gyro and the heading reported by odometry to the dashboard and I noticed they weren't exactly the same (even after running commands that reset both to zero). Weirdly the heading reported by odometry would fluctuate and drift slightly from zero even when the robot was dead still. I assume this is something to do with the Kalman filter used in the update odometry periodic method that was running in the drive subsystem.

I wish I knew a bit more about how the odometry updates worked. I'd like to do a test when the robot is pinned in place and the swerve modules have enough torque to slip the wheels (or alternatively the robot is elevated so the wheels aren't touching the ground) and then issue a turn on the spot by twisting the joystick. I'm assuming the robot heading in odometry would change slightly due to the odometry updates receiving the swerve module states that indicate it is turning on the spot even though the robot is forced to stay facing the same direction.

If this was the case the heading reported by odometry would not be accurate and the gyro heading would be a more accurate source of heading.

Or maybe I have it all wrong and the odometry system relies entirely on the heading supplied to the update call to maintain the heading in the odometry and the module states are purely used to try and calculate the distance travelled in the direct of the supplied heading.

This would be an interesting test. Unfortunately we are currently focused on FLL at the moment so we won't get a chance to dig out last years robot and confirm or deny this theory. Can someone that has a swerve robot test this theory?

[mjansen4857]

Are you using SwerveDriveOdometry or SwerveDrivePoseEstimator? Odometry creates the pose with the gyro value directly, but the pose estimator tries to estimate a pose with both gyro and encoder data + smooths out noisy data and what not. If you are using the pose estimator, you can tweak your localMeasurementStdDevs argument to trust the gyro measurements more which should help ignore some wheel slip but I haven't done much testing with it outside of simulation so I don't know how well it works.

[mjansen4857]

Anyway, bottom line here is, I suggest a command call is used to "reset odometry at start of path" instead of calls to a subsystem methods to reset fused heading and set odometry position.

So the way I've settled on for the implementation of auto builder does not do this, but, it is designed in a way that allows for customization to different team's needs. For example, the default behavior of the fullAuto method is to call the reset pose method with an instant command, then follow paths, events, etc. If you'd like to change this you can create your own auto builder that inherits from BaseAutoBuilder, then override the resetPose method to return a command that resets pose, gyro, etc then waits for a given amount of time. This overriden method will then be used when constructing the full auto and it should just work.

Everything is compartmentalized like this so you can pick and choose what to customize.

[MrRedness]

Awesome job with this. Thanks for all your hard work! I wish my team could use this, but we've gotten pretty comfortable with the Timed framework and I don't think we're ready to give that up just yet.

[mjansen4857]

You can run commands manually within the timed framework if you'd like. Once you have your auto command just call command.Initialize(), command.Execute(), command.IsFinished(), and command.End() in your timed framework methods. You'd just have to make sure you're not running other stuff so the command isn't fighting with anything.

[WarrenReynolds]

Just so I've got this in my head.

So when you inherit from BaseAutoBuilder you will still have to supply a function that takes a Pose2D() and returns void to the constructor on the BaseAutoBuilder. But then you override the resetPose method on the class that was derived from the BaseAutoBuilder class to do what we wanted in terms of running a custom command that does the set/ check loop stuff to make sure getRobot Pose is giving correct values.

Could the resetPose parameter to the constructor of BaseAutoBuilder be made option, so if a team is happy to leave their gyro as it is from power on, they don't need to add a parameter here.

Alternatively is there a way to supply this ResetPose parameter to the constructor so it does nothing when it is called? I know there would be I just don't know the syntax.

[mjansen4857]

Yeah. I originally had it as a member of the specific auto builders but then decided to move it to the base so it wouldn't be repeated multiple times.

Assuming you're using c++, you can just pass an empty function: [](frc::Pose2d pose){}

I'll add an overload constructor to BaseAutoBuilder real quick that does this for you. I left overloads out of the main auto builders since I want the reset pose functionality to be visible so people don't miss it and wonder why its not working. But, if you're inheriting from BaseAutoBuilder yourself its ok to assume you know about it.

[WarrenReynolds]

Yes, we are C++, don't ask my why I've stayed on C++, It's the language I learnt back in the early 90's when I did my degree. Back then, the most complicated things we did was linked lists with dynamic runtime memory allocation and memory de-allocations as elements were added or removed from the lists. The language has certainly grown a lot since then. I'm starting to wish I'd devoted the time to learn java so I didn't have to deal with all the move semantics stuff.

[MrRedness]

Yes, we are C++, don't ask my why I've stayed on C++, It's the language I learnt back in the early 90's when I did my degree. Back then, the most complicated things we did was linked lists with dynamic runtime memory allocation and memory de-allocations as elements were added or removed from the lists. The language has certainly grown a lot since then. I'm starting to wish I'd devoted the time to learn java so I didn't have to deal with all the move semantics stuff.

What do you mean, moves are awesome 😎!