Swerve Sim - first pass

[gerth2]

Alrighty. I got something that smells decent, looks correct..... But very purposefully putting this out there as a draft for discussion.

This PR adds a java example for simulating swerve drive, including vision processing for odometery correction.

Open questions in my mind:

0. Is this useful at all?
1. Is this useful enough to be part of WPILib?
2. What is most useful for teams? Would making a reusable class be more useful than an example?
   . I like reusability, but the number of cross-dependent configurations makes me worry that folks are gonna struggle to configure it to be meaningful for their setup. I don't believe it would be nearly as "drop-in" as the other simulated mechanisms.
3. I assume photonvision probably shouldn't be directly in the example (ie, endorsement of 3rd party)? Is there a desire for an alternate? Or just rip it out entirely?
4. Motor controllers and encoders are all the "not-smart" flavor. It works great, and makes sim clean, but isn't probably a realistic permutation for any team

Ongoing work prior to a full release would probably involve some cleanup of constants (make sure everything the user should be touching for their bot is moved to one spot), trim out a few utility classes that I don't believe are actually needed currently, and review how well sim architecture was applied. I know I'd picked an alternate way of wrapping the gyro - there's probably a few architecture layers that could be trimmed away.

Discuss or reject as needed! Thanks folks!

[PeterJohnson]

My initial thoughts...

0. Yes, teams are definitely looking for examples like these.
1. I'm leaning towards yes, but it's definitely on the higher complexity side.
2. We're struggling with teaching RamseteCommand for similar reasons. The question in my mind is whether a small part of this could be refactored to be in the library (namely the non-camera pieces).
3. Correct, we shouldn't endorse a particular vision solution. If different vision solutions output equivalent types of data (e.g. estimated distance/pose) then we could make it "feel" more generic by removing specific references to a particular solution or making that part more modular (e.g. here's the 3 lines of code that get the data from NT for this particular vision option).
4. We pretty much have to do that for anything in the main part of WPILib, as we can't depend on vendor dependencies.

[gerth2]

To get a scale of the level of configuration needed,

Here's a strawman of a library class mirroring off of DifferentialDriveSim:

SwerveModuleSim

public SwerveModuleSim(
    DCMotor azimuthMotor,
    DCMotor wheelMotor,
    EncoderSim azimuthEncoder, 
    EncoderSim wheelEncoder,   
    double wheelRadius_m,
    double azimuthGearRatio,   //Motor-to-azimuth reduction
    double wheelGearRatio,     //Motor-to-wheel reduction
    double azimuthEncGearRatio,   //Motor-to-azimuth-encoder reduction
    double wheelEncGearRatio,     //Motor-to-wheel-encoder reduction
    double treadStaticCoefFric,
    double treadKineticCoefFric,
    double azimuthEffectiveMOI,
    double wheelEffectiveMOI,
){}

QuadSwerveSim

public QuadSwerveSim(
    double wheelBaseWidth_m,
    double weelBaseLength_m,
    double robotMass_kg,
    double robotMOI,
    SwerveModuleSim moduleFL,
    SwerveModuleSim moduleFR,
    SwerveModuleSim moduleBL,
    SwerveModuleSim moduleBR
){}

The MOI's could be calculated/assumed to help clean up config.

Modules could also be given as an array (as the name should hopefully indicate this is a 4-module swerve model, not 3 or 5 or whatever)

This could hit a big chunk of the functionality, while leaving a lot of the motor/encoder/gyro/vision specifics to teams (which could be hit with other official or unofficial examples).

Additional things to consider:

Doing this using LinearSystem internally - More of an internal architecture thing, but I know successfully developing that soon will be beyond my current abilities (though I do believe @mcm001 did kinda sorta volunteer to help with that at one point :P).

[gerth2]

Copying some additional discussion from Discord:

1. physics/Vector2d.java is a patch to add cross-product functionality to wpi's Vector2d - this should just be pushed to WPIlib.
2. physics/Force2d can be generalized to be a 2x1 matrix
3. physics/ForceAtDistance can (possibly) be generalized to a 2x1 matrix, and a skew-symmetric matrix

[calcmogul]

Here's the relevant section: https://en.wikipedia.org/wiki/Skew-symmetric_matrix#Cross_product

[gerth2]

A status update, nothing more: Progress is been made over in the test repo I'm using.

https://github.com/RobotCasserole1736/TestSwervePlantModel/tree/wpilib_stripout/src/main/java/frc/sim/wpiClasses contains what I'm looking to submit (along with unit tests and a barebones usage example)

[gerth2]

Since this has been dead a while, wanted to make sure we marked off where we're at:

1. Functionality has successfully been moved into dedicated, reusable java classes, ready for release in WPILib
2. Unit tests have been started, but are currently failing, due to issues releasing HAL resources at the end of this (or some other test). Mostly just needs debug.
3. A few teams have been contacted to provide timeseries data to characterize model accuracy, but no data available yet.
4. wpilib_stripout branch in this repo contains the latest "working-copy" of the application of theses classes. This eventually needs to be trimmed down into an example for wpilib.

[willtoth]

I started looking at this and had a few questions, though these may be more generic wpilibj questions.

Should the classes the Vector2d and Force2d move to a separate folder instead of drive or swerve, something like math, or geometry etc? Maybe edu.wpi.first.math.geometry.

We're getting fairly close to build season, not sure if this will get into the release or not. One suggestion to at least have this as an easy package that folks can drop into their projects would be to split out the changes to Vector2d into a separate PR. That way there is no longer a dependency to this change, and a user could simply drop the entire swerve folder in their project.

[gerth2]

Should the classes the Vector2d and Force2d move to a separate folder instead of drive or swerve, something like math, or geometry etc? Maybe edu.wpi.first.math.geometry.

Likely yes - I had not picked any specific location - agree a math/utils related location would be better. There was still an open question in my head whether creating such classes for general usage was the right path for wpilib (I don't have a great notion as to how they'd get used outside this particular swerve simulation). Per tyler's comments above, there are more matrix-friendly techniques that could be used instead.

I know I likely won't have the bandwidth between now and kickoff to really bring this to being production quality. The fact that this hasn't been validated against real-world data makes me leery to put it out there for general consumption. While it's good to keep referencing this (and the other repos where it's been applied - I still owe @jasondaming a review and some debug), the key is that consumers are still part of active development, not consuming a full finished product.

[willtoth]

That's understandable. Would you be willing to submit your changes to wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java in a separate PR then? That way the rest of the code could be included as a single folder into user code for the upcoming season. Would be a nice way to get feedback.

[gerth2]

That way the rest of the code could be included as a single folder into user code for the upcoming season.

So I think the best way to achieve that objective is to just reference this folder from this repo.

The reason I say that is because, as-is, this PR represents a departure from the way WPILib has been approaching physics simulation. For most mechanisms so far, the physics work has been done offline and compressed into matrices, which in turn play nicely with existing solutions like LinearSystem and friends.

The vector/force/force-at-pose classes in this PR are starting to build up a more "online" free body diagram solver. I haven't yet proven out this is required. It expands the complexity of support for WPILib (two simulation paradigms rather than one).

[jasondaming]

In my project I already have all of the changes in a separate folder and expect that is what teams will need to do for this year while this solution matures as I don't think there is any hope that this will make it into 2022. My goal is to get this out before the start of the season that can be used as a submodule

[csokolove]

What's the status of this PR? It would be super helpful for offseason swerves.

[gerth2]

Great question! I've been meaning to post an update here. My current opinion: I don't think this is ready for prime-time yet, for a few reasons:

The biggest - the functionality contained here is too complex and fraught for mass distribution and support. There's some real debugging and know-how required to integrate it properly, especially if 3rd party motors and motor controllers are used (which is like 99% of the use case). And I know there's use-cases it doesn't cover.

Case in point: one of the issues Jason (see above) ran into was that a CAN bus packet rate being applied to a particular getVoltage() output in some of the CTRE libraries. This took forever for us to find root cause on, because it had some really weird symptoms. Ultimately we weren't applying the CTRE library right.... but I don't think it's reasonable for the average team to quickly comprehend the difference between a getVoltage() that represents the physical voltage applied to motor windings, versus a getVoltage() that refers to a measured voltage sampled down to whatever the CAN bus is transferring. The two are different, and different in a way that makes or breaks this simulation's functionality.

I don't think WPILib has the bandwidth to support that level of debug with teams, and I'm worried about merging PR's which would trigger that - "well that's not our issue" is a demoralizing answer to give to a student who's just trying to get their software working, and I worry this PR is going to cause a huge uptick in those sorts of answers.

Additionally - The work of doing some real-world validation of the physics implemented is still not done, and I don't have line-of-sight to achieve it myself over the next ~8 months.

Finally, part of what enabled this to work well for our team was a set of custom dashboard widgets that allowed for quick, visual debug of the system behavior. The Oblog and NT4 work in flight lays some of the groundwork for this, but we need to make these things Sendable and have a good-enough visualization for folks to understand what's going on in the physics.

I'm definitely not the one to make a final decision on this and would be happy to help. But the couple of folks who have tried to pick it up have definitely make me realize there's a lot of work to be done on the deployment side still.

Finally, it's worth considering that I don't know if this is significantly better than just assuming perfect kinematics and using that for simulation. This PR adds some motor dynamics and a frictional model.... but for a good swerve system, these should be largely negligible. And, I would gander to say that if you're doing swerve simulation, you probably have some good hardware too.

One possible alternate approach- Incorporating this with SDS's libraries, and path planning (like what @jasondaming mentioned above) could help encapsulate it better to where teams won't have to worry about it. But, as we've been going back and forth, there's enough difficulty with dealing with vendor sim support that even this is fraught.

[truher]

If anyone came here looking for the "perfect kinematics" approach, you could use
the thing I made .... It's really intended to help with automated navigation rather than
work on swerve itself.