Dashboard for Trajectory Following

Hi CD
This year we are using trajectory following for skill challanges. But we don’t have a way to visualise our robot’s trajectory for debugging purposes. Is there any pathweaver like dashboards for showing us the trajectory of the robot?

2 Likes

Not currently. There is an issue to allow path previewing with Glass, but it’s not implemented yet.

1 Like

I created a simple (?), offline method of seeing the trajectories. In the Java code, we write out a CSV file of the trajectory position at 0.1 second intervals. Then we have some Python code which plots that on a field map. Not great, and a bit clunky, but it does show the trajectories, including some problems.

2 Likes

Glass does show the current robot pose (use the Field2d class), just doesn’t show the full path. It does support secondary objects (not just the robot), and so you can (ab)use this feature to show e.g. path waypoints, there’s just not support yet for drawing a line showing a full path.

3 Likes

So if there isn’t no way to show the robots position with the trajectory is there a way to show the trajectory generated by the code

If there it is no problem for you can you share it so maybe we can make something with guidance from it

Of course. You can see the code in our Github repo:

For the writing, look at src/main/java/frc/robot/commands/TrajectoryWriter.java. You can see it used in …/commands/AutoNavPaths.java. You call it once per trajectory (in order if there are multiple, such as Bounce) and once per waypoints.

For the plotting, look at fieldmap.py at the very top directory (kind of messy, sorry). This will plot the field layout and overlay the trajectory. You would run it on the command line like (for the barrel challenge):
fieldmap.py -m barrel -t barrel_traj.csv
and it outputs a PNG file. The trajectory is red and the provided waypoints are blue dots.

Happy to answer questions. Like I said, it is not the most convenient, but it does seem to work.

4 Likes

We think BobTrajectory is pretty neat.

1 Like

Thanks a lot. This will probably satisfy our needs but if anyone has another suggestion or method I will be happy to hear it

Here is @Peter_Johnson’s suggested solution with creating secondary objects in Field2d. I have an object for every pose in a trajectory I had. There doesn’t seem to be an easy way to set the size or image of these objects automatically. Perhaps, you could make some sort of script to edit imgui.ini, or show your trajectory by moving one secondary object instead.

1 Like

You don’t need a separate object for each one; each object can have up to 255/3 poses. That will at least reduce the number of unique objects.

2 Likes

So, can you share the code that produced that? I had seen the Field2d object support, but it was not clear (to me) how it could be used at this time.

Here it is with Peter’s advice on using the same object.

//Trajectory trajectory = ...;
//Field2d fieldSim = ...;
fieldSim.getObject("trajectory").setPoses(
   trajectory.getStates().stream()
      .map(state -> state.poseMeters)
      .collect(Collectors.toList()));

With custom image in simulator:

6 Likes

This is really cool. Thank you for sharing. How do we format the csv?

It looks like we need IsWaypoint, X, Y. I can get IsWaypoint to work, but I am having difficulty with x and y for some reason. Do you have a sample csv we could look at?

@DavidTaylor This thread is amazing because it helps us understand how we can easily tune the trajectories and compare them with the robots (which is especially helpful for the Romi.

We are going to try some of these other recommendations too, but wanted to start here.

1 Like

When I try to use this method glass crashes. I checked the size of the pose2d list and it shows 431 objects in the list. The amount of the pose in your trajectory seems to be more than 85 pose too. How did yours show on the glass? Did you used multiple objects?

I was wrong. I divided list into smaller ones but still when I try to show these on the glass it crashes.

Thank you for sharing this code snippet! I just dropped it into my project and it works perfectly! This will be massively helpful to our team when we are debugging our auto routines.

1 Like

My files are plain CSV with columns:

IsWaypoint,Time,X,Y,Heading

The plotting only uses IsWaypoint, X and Y, but I could see plotting heading for some points.

Note that the distances are in inches and angles in degrees, since that was the most convenient for humans.

I can’t attached a CSV file, so here is a bit of one file (lots of lines deleted in the middle).

Summary

IsWaypoint,Time,X,Y,Heading
0,0.0,42.0,90.0,0.0
0,0.02,42.00787397979494,90.00002379473102,6.641923113243333E-4
0,0.04,42.03149591917973,90.00009517892404,0.0026567692452973326
0,0.06,42.0708658181544,90.0002141525791,0.005977730801918999
0,0.08,42.12598367671894,90.00038071569617,0.01062707698118933
0,0.1,42.19684949487334,90.00059486827526,0.01660480778310833
0,0.12000000000000001,42.28346327261761,90.00085661031636,0.023910923207676002
0,0.14,42.38582500995175,90.00116594181951,0.032545423254892326
0,0.16,42.50393470687576,90.00152286278467,0.04250830792475732
1,0.0,42.0,90.0,0.0
1,0,150.0,90.0,
1,0,175.00000000000003,60.0,
1,0,150.0,35.0,
1,0,125.0,60.0,
1,0,150.0,85.0,
1,0,240.0,95.00000000000001,
1,0,265.0,120.0,
1,0,240.0,155.00000000000003,
1,0,214.99999999999997,120.0,
1,0,280.0,40.0,
1,0,320.0,40.0,
1,0,320.0,80.0,
1,0,270.0,90.0,
1,0.0,60.0,90.0,180.0

1 Like

Thanks. I was missing time.

Does it crash if you use fewer items (like 60-70)? Is this on Linux, Mac, or Windows? If you can link me to your poses/code, I’d like to figure out what’s causing the crash.