Infinite Recharge at Home AutoNav challenges using WPILib's drive simulator

[Joe_Ross]

Can’t access your robot? You can still practice for the the Infinite Recharge at Home AutoNav challenges using WPILib’s drivetrain simulator that was introduced this year. I’ll walk through the process I used as well as some of the issues I had to work around.

Here’s an example with the Slalom challenge.

Install 2021 WPILib software
I started by installing the 2021 WPILib Software. Instructions here: WPILib Installation Guide — FIRST Robotics Competition documentation
An overview of the simulation capability is available here: Introduction to Robot Simulation — FIRST Robotics Competition documentation

Create Drivetrain simulation project
I created a new project from the State Space Drive Simulation example project. It would probably be possible to do the same thing with the Simple Differential Drive Simulation example project as well. There’s more information here: Drivetrain Simulation Tutorial — FIRST Robotics Competition documentation

Generate Waypoints using Pathweaver
I used Pathweaver to generate the waypoints. Make sure to set the export unit to Always Meters to match with WPILib’s trajectory classes. The version of Pathweaver in WPILib 2021.1.2 (the current release) doesn’t have field images for the Infinite Recharge at Home challenges. However @jdaming created a pull request to add them: Added the 5 IR @ Home Autonomous Challenges by jasondaming · Pull Request #200 · wpilibsuite/PathWeaver · GitHub Until there’s a new release, you can add them following the instructions here: Adding field images to PathWeaver — FIRST Robotics Competition documentation.

Here’s what my path looked like:

I found that the exported coordinates weren’t right. I had chosen project units of feet to match the lengths in the manual, but to export in meters, to match WPILib Trajectories. If you use meters for both, it should be fine, or this PR should be in the next release.

Incorporating Pathweaver Trajectory into Drivetrain Simulation Project

I followed the directions to read the Pathweaver trajectory here: Importing a PathWeaver JSON — FIRST Robotics Competition documentation replacing the voltage constraint, trajectory config and trajectory with the code to read the Pathweaver trajectory.

Pathweaver was supposed to generate the trajectory in my project’s deploy directory. I think I set up the project wrong, but it was easy enough to copy the file into the deploy directory.

Running in Simulation

Next I ran the code in simulation. Introduction to Robot Simulation — FIRST Robotics Competition documentation

From the NetworkTables menu in the simgui, I added the field from the smartdashboard namespace. Then you can right click on the title, expand the field item, and select choose image. From there, change the file type to Pathweaver JSON and point it to the json file from pathweaver earlier. You can now enable autonomous.

At this point it looked like this.

I’ll follow-up with how I improved the trajectory following using frc-characterization

[Joe_Ross]

Here’s some things I did to improve the following.

Fixing Starting Orientation

The first issue that I noticed was that the simulated robot would start out in the wrong orientation, and would have to turn around. This is because the DriveSubsystem resetOdometry() method resets the robot’s position on the field, but doesn’t reset the gyro accordingly. This wouldn’t be an issue with a real robot that can’t instantaneously transport across the field. One way to work around this to add m_gyroSim.setAngle(pose.getRotation().getDegrees()); in the resetOdometry() method prior to calling m_odometry.resetPosition

Running frc-characterization
On a real robot, running frc-characterization to characterize the drivetrain would be the next step to improve performance, so I decided to try to run it on the simulated robot. This was the most complex step, but definitely paid dividends.

I followed the [Robot Characterization — FIRST Robotics Competition documentation](https://Robot Characterization) instructions up to the point of generating a project. Normally Robot Characterization will deploy a project to your robot to move the robot and log data. However, in this case the movement is provided through the simulation code so I couldn’t run the Robot Characterization project as-is. I took the the generated project and copied the relevant sections into my project, adapting for different variable names and methods. In hindsight, it may have been cleaner to copy the drive simulation stuff into the robot characterization project rather then vice versa, however Robot Characterization’s project isn’t really designed for opening in VS Code.

The next step was to run frc-characterization’s logger while the the simulated robot was running with all the logging code installed. In order to minimize the chance of networktables connection issues, I ran both the driver station and simulator gui at the same time by selecting both halsim_gui.dll and hallsim_ds_socket.dll and disabling the simulator gui’s driver station. Make sure the team number in frc-characterization’s logger is set to 0 to indicate that it should connect to localhost. I did have to close and open the robot simulation and the logger a few times to get it to connect.

Once it was connected, I was able to run the logger and analyzer per the directions. I replaced the constants ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter, and kPDriveVel with the values from the analyzer.

One issue that I had was that the track width returned from the analyzer was much too large. I did not use this value.

When I re-ran the trajectory, it followed the trajectory much better. It was still jerky, so I increased the RAMSETE zeta value, and ended up with this:

Next Steps

There’s definitely still things that could be improved.

Figure out why the track-width from frc-characterization is so far off from the simulated track width.
Figure out the best way to determine kvVoltSecondsPerRadian and kaVoltSecondsSquaredPerRadian which are used in the drivetrain model but don’t appear to be calculated by frc-characterization.
Try some of the other courses.

[jdaming]

Here is my version of the Bounce Path! Using the reverse spline features of PathWeaver makes it easy to configure backwards driving too!

[Joe_Ross]

I think you get a few penalties for touching markers

[jdaming]

I am going to wait till I see exactly how well this sim translates to our real robot before optimizing too much!

[Joe_Ross]

Here’s the barrel racing path. Only change I made was to define a new path in pathweaver.

[Joe_Ross]

@Joe_Ross

Figure out why the track-width from frc-characterization is so far off from the simulated track width.

I found a few issues that led to the wrong track-width.

• The first was that frc-characterization expected the accumulated gyro angle, while the drivetrain simulation program limited it to -180/+180 degrees. I had converted to Radians like it expected, but I had to send the unlimited gyro angle.
• Further, frc-characterization expects the distances in terms of wheel rotations, and I had been sending encoder distances/rates in meters.
• Finally, frc-characterization wasn’t converting the trackwidth computation from wheel rotations to the units I specified. This was reported in this issue: Track-width computation doesn't account for wheel circumference · Issue #166 · wpilibsuite/frc-characterization · GitHub

Once accounting for all of those, it reported the correct track width.

@Joe_Ross

Make sure the team number in frc-characterization’s logger is set to 0 to indicate that it should connect to localhost. I did have to close and open the robot simulation and the logger a few times to get it to connect.

After updating to pynetworktables 2021.0.0, I was able to always connect without having to close and reopen anything.

[jdaming]

I had a very difficult time connecting frc-char to simulation. The worst part is what worked was just trying the exact same thing over and over and usually within about 10 tries I got it connected. I would love to look more into figuring out what the issue is here.

[chauser]

If you’re on Windows the issue is probably: pynetworktables client can't connect to networktables server on Windows 10 "localhost" · Issue #116 · robotpy/pynetworktables · GitHub
It appears that it is fixed in version 2021.0.0 as @Joe_Ross said. You can upgrade with

pip install --upgrade pynetworktables

and I think frc-characterization will pick up the new version.

[Piphi5]

Just an update, the trackwidth errors have been fixed on the frc-characterization tool. I believe you just need to run an update to get those changes integrated.

[auscompgeek]

I don’t think those changes have actually made it into a release yet, judging by the tags on the repo.

[tjNeu]

I’m experimenting with this too, but could you explain what files you copied over into the deploy directory?

[jdaming]

You want to copy the *.wpilib.json files that are generated when you click “Build Paths” in PathWeaver. You can then load these files in your program and run that trajectory

[Kaleb_Dodd]

Autonav Challenge Times

I was able to get 4.18s (as reported by the code) in the bounce path WPILIB simulator with the example code with a max speed at 4m/s (instead of 3) and max angular as the default. Just copied over our PID class and our DriveToPoint function from our 2020 code. https://drive.google.com/file/d/1fBaD-enKtWkSzFP72Bk65y84UWxLenoZ/view

[JReid]

Our team is new to Java this year (11 years using LabVIEW). The learning curve has been pretty steep so far. Yet, we’ve successfully gotten our 2020 (SparkMAX/NEO-based) robot to drive using a basic command-based framework. We’ve also been able to get the simulation working and following paths that we generate, thanks to your excellent notes in this thread. Finally, we’ve been able to characterize our actual robot using the frc-characterization tool.

We’re now at the point where we need to get our actual robot driving with the trajectory code. Thus, the point of this post for help. docs.wpilib.org has been our friend so far, but after reading it over it again and again, we’re stuck and need help directing us in the next step.

As we look at the different written/generated code from the command-based, the simulation, and the characterization routines, we see a lot of differences between them in structure, organization, and content. Since we’re new to Java, it is a bit overwhelming trying to reconcile and understand the differences.

The best we can tell is that we should modify the simulation project code to match the hardware in our actual robot. We think this would be accomplished by modifying Constants.java and DriveSubsystem.java in the simulation code to match our hardware. Is this a logical next step?

If it is, then what about all of the simulation “stuff”? Does all of that need to get removed? I don’t have an expectation that the simulation will work after we switch the code from PWM Talons to SparkMAX’s from what I’ve read. Same with changing the default gyro to a NavX. So, do we keep the simulation framework intact, or is it necessary to remove it?

Or, as an alternative option, should we take the existing command-based code that we have and attempt to modify it by adding all of the kinematics and odometry, etc.?

We’re aware of the LabVIEW repositories that are available, and are working that option in parallel, but it has not been as successful.

I hope these questions make sense. We have limited access to our physical robot, so that is why we’re asking in this forum. We have to maximize our implementation and debugging time, and we want to avoid starting down a path without some counsel from people more experienced than us.

[Amicus1]

I don’t know what your code looks like, so I’m not sure what simulation stuff you have. Do you have your code posted?

[JReid]

I had to go learn git. Hopefully this will help…

NorsemenRobotics/2021SimulationCode

[jdaming]

Learning git/github is a skill that I think you will really appreciate as you use it. It makes many things centered around coding MUCH easier. We even used it for our LabVIEW code both in doing LabVIEW professionally and when using it on a LabVIEW team. It doesn’t have all of the amazing benefits that text based has built in but it still has enough to make it worth it!

You are in a tough spot with simulation using SPARK MAX and a NavX. Not having direct support for simulation doesn’t make things useless it just involves more care around stubbing in comparable components. One of the core ideas of simulation was that this was just code you would have to add on top of your existing code base to get it to run and that it would still run your existing code base just fine when deployed to your robot. Keep using if (RobotBase.isSimulation()) { // If our robot is simulated to choose between code that is needed for simulation and the real robot code. As more components gain simulation support you will be able to remove more of these statements and use your code “naturally”.

I only see your 2021SimulationCode repository we would need to see your existing repository to provide help on that.

Here is a link to our code. Since we use non-Falcon CTRE a lot more stuff we have is “supported” but the pidgeon like the NavX is unsupported so at least that may be helpful to you.

[gerthworm]

@JReid

I had to go learn git. Hopefully this will help…

As you continue along your journey, I did want to ensure you were aware of LVMerge.exe. I haven’t used it recently, but it saved us more than once back in college while recombining multiple developer’s work together.

[jdaming]

I apologize as it looks like the NavX does have simulation support here.

Hopefully things are working out for you let us know if you have other questions about getting up and running!