WayMaker: A new waypoint editor for path planning

Befor you get too excited, this currently doesn’t exist except as a gutted version of the Glass dashboard. The plan is to create a tool that will allow creating waypoints on the Field2d widget, then export those waypoints as probably a JSON to the robot, where the robot code can generate trajectories based on those points.

Advantages:

  • Unlike PathWeaver, this tool would not generate trajectories. This is simply a graphical editor for waypoints with the ability to export as JSON. This means it’s robot-independent, and the path drawing team member needs not worry about having proper velocity, acceleration, and track width constraints.
  • It works in the Field2d coordinate system, which by definition matches the rest of WPILIB’s coordinates.
  • It will be designed with feedback from using PathWeaver in mind. (Hint, hint, drop said feedback below)

Thoughts? Questions?

9 Likes

Is there a reason that JSON is used as opposed to CSV or another serialization format? Other than there being JSON support in WPILib already.

There’s certain metadata the robot code would need to create the trajectory correctly: for quintic splines, a list of control vectors and a reversed flag; for cubic splines, a starting pose, list of interior translations, an ending pose, and a reversed flag. That doesn’t cleanly map to CSV. JSON supports key-value pairs and lists, which is pretty much all we need.

We’ll probably also want a file format version key so we can make file format changes later if we want.

1 Like

Last season we used a couple of simple routines to plot trajectories and waypoints in Field2d directly. The fine folks maintaining WPILib have since added a number of enhancements to Field2d that will make it even more useful for this purpose. (Thank you, Peter!)

The idea is that you can plot the waypoints and also the trajectory generator output onto Field2d. But we discovered that Field2d is actually interactive, meaning that you can drag-and-drop your waypoints to a new position or orientation. If you continuously read back the waypoints and generate & plot a new trajectory, you can get interactive trajectory editing right on your robot/in glass.

I’ll have to dig out the code I was using, but it was just a couple of short < 10-line functions added into my project. Here’s a video clip of what was possible using last season’s Field2d functionality to edit trajectories in a Romi project.

This has enormous potential. I look forward to seeing what you can do with this.

https://drive.google.com/file/d/1-flzEdd5KNqZgqwb0b8E0i4RT3kJ53wY/view?usp=sharing

EDIT: Here are the routines I was using, added to my Drivetrain class:

  public List<Pose2d> getTrajectoryStatePoses(Trajectory trajectory) {
    List<Pose2d> statePoses = trajectory.getStates().stream()
          .map(state -> state.poseMeters)
          .collect(Collectors.toList());

    return statePoses;
  }

  public void plotTrajectory(Trajectory trajectory) {
    fieldSet("trajectory", getTrajectoryStatePoses(trajectory));
  }

  public void plotWaypoints(List<Pose2d> waypoints) {
    fieldSet("waypoints", waypoints);
  }

  public List<Pose2d> fieldGet(String name) {
    return m_field2d.getObject(name).getPoses();
  }

  public void fieldSet(String name, List<Pose2d> poses) {
    m_field2d.getObject(name).setPoses(poses);
  }

  // And this gets called from your Periodic function, or whenever a waypoint is edited:

  private void regenerateTrajectory() {
    // Realtime trajectory editing:
    List<Pose2d> waypoints = m_drivetrain.fieldGet("waypoints");
    Trajectory trajectory = trajectoryForPath(waypoints, false);
    m_drivetrain.plotTrajectory(trajectory);
    m_drivetrain.plotWaypoints(waypoints);   
  }

EDIT2: Looks like I missed a function. This was refactored from the Ramsete example:

  private Trajectory trajectoryForPath(List<Pose2d> path, boolean reversed) {
    var autoVoltageConstraint =
        new DifferentialDriveVoltageConstraint(
            new SimpleMotorFeedforward(DriveConstants.ksVolts, 
                                       DriveConstants.kvVoltSecondsPerMeter, 
                                       DriveConstants.kaVoltSecondsSquaredPerMeter),
            DriveConstants.kDriveKinematics,
            10);

    TrajectoryConfig config =
        new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, 
                             AutoConstants.kMaxAccelerationMetersPerSecondSquared)
            .setKinematics(DriveConstants.kDriveKinematics)
            .addConstraint(autoVoltageConstraint)
            .setReversed(reversed);

    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
      path,
      config);

      return trajectory;
  }
1 Like

This confirms what I thought might be possible. A few questions:
Are drawn paths persistent across reboots of both the robot and the DS laptop?
Was there any way to save the paths?
Given that the trajectory previewing is done basically on a network server (the robot) with limited uptime, I’d say this is part of a solution, but of limited usefulness, especially without access to the robot.

HOWEVER
Suppose the modifications we make to Field2d to allow rendering control vectors (position, rotation, tangent length, reversal, which is a better way to define the curve than just position and rotation) get merged back into one of the competition dashboards. I could see non-persistently “tweaking” the path and re-generating directly in robot code prior to running the robot being a useful tool.

1 Like

It really depends on where the initial path is coming from in your robot design. Your robot code has to reload the waypoints into Field2d after any reset, but there are many ways you may choose to do that.

Not shown in my code is how the initial set of waypoints gets created in the first place. We generally sidestepped using PathWeaver in favor of just defining our waypoints in code, or by “other means.” In the video, an auto routine had preloaded a List of Pose2d into the waypoints displayed in Field2d.

Any (List of Pose2d waypoints) can define a path segment. The code I’ve presented doesn’t show how to handle multipart bounce paths with reversals. We did that using a new path for each forward/reverse segment, IIRC.

In some cases, we simply printed out any edited waypoint lists to the debug console to be copy+pasted back into our source code. But it wouldn’t be too difficult to save/load waypoints to a JSON or CSV file. We had many other sources for path data, but that is a topic all its own.

We were doing lot of remote work last season using the Romi, so our students did actually have near unlimited access to a robot. But even without hardware, it isn’t much work to instrument your code to simulate your robot. Then you can run all of the path planning in your Robot Code, running “live” in the simulator without a real robot. The Romi runs full-time in simulator mode, so this interactive Field2d hack was more obvious to us than it might have been had we been working solely with our competition robot.

Somehow I mind-blanked that simulation exists, even though I’m literally running one at the moment…

1 Like

Prove to me that we aren’t all inside of a simulation right now… :stuck_out_tongue:

I can’t. In fact, I will posit that Michael Jackson was simply an animation glitch.

Off topic in my own thread. Shame on me.

Don’t know if this is out of scope, but support for swerve drive with translation heading and robot heading would be nice.

1 Like

I’m considering this. WPILIB doesn’t have a trajectory that can generate a separate curve for robot heading.

I was actually about to put out a request for use cases for such a feature. How complex do your heading changes get during a trajectory?

That requires some API design work outside of the tool. You’re not only generating a 2D trajectory at that point, but also a separate heading profile. Currently, we tell teams to just use a ProfiledPIDController for that. The swerve drive kinematics constraint doesn’t take the alternate heading into account though, so it’s not perfect.

1 Like

Another tricky part of that is temporally linking the heading waypoints with the trajectory waypoints post-generation. With some “flag” API like has been suggested many times before it could be done, but still no plans to implement that AFAIK.

The way we used field2d last season was to pull waypoints (POJO or a pathfinder .json object, we got both working) from a file and, using the SIM window, we plotted the points on the field. We did not do this with our actual robot, but with the Romi. We had it’s field2d print back to the SIM window so qe could see the difference between our planned path, the one it thought it was taking, and the actual path it took.

We also updated our planned path in robot periodic which allowed us to use glass to change the waypoints in real time.

To answer a question you had above, we did not figure out a way to save those updated waypoints.

So, we jumped into google sheets (we were fully remote or hybrid for the duration last year. Sheets was all we needed and allowed for two people to work on it simultaneously and not be in the same room) and manually entered them.

We could not have done it without the help of @veg who guided us through most of the process.

2 Likes

Teaser:

2 Likes

What languages/tools are you writing this in? I did something similar in the past with Opengl as a side project.

Also, how modular is this? Could I easily port in a field image and dimensions of an FTC field and use it for our FTC Team?

1 Like

This is in C++/ImGui, the same toolset as the FRC Simulation and Glass dashboards. In fact, it’s basically just the field widget from those, but with the backend swapped and a little bit of extra code to allow the field objects (pose sets) to define interpolated points for the connection spline.

Re custom fields, it already supports the PathWeaver format. See Adding field images to PathWeaver — FIRST Robotics Competition documentation. If you want to throw together a JSON and field image, I’d be happy to get you a screenshot of how it looks. (I actually have no idea how the preview will look on a field that size, given the spline generator assumes FRC fields)


Hacked one together. These boxes are 0.5m square.

1 Like

What does the FTC trajectory suite look like? Looks like at least in Java (FTCLib) it borrows a lot of code from WPILib, which likely means your path will use the same generator this preview does.

Could you point me in the direction of the Java (FTCLib)? I’ve seen very little standardized motion control from FTC. I’ve been very interested in rewriting the wpilib command scheduler so it is compatible with FTC code. I hypothesize if that can be done, then nearly all of WPILIB could be used for FTC.

My original question was mainly wondering if this could be used as a standalone for multiple different robotics projects, or if it was coupled with something else.

1 Like