RFC: New Waypoint Editor tool

[shueja]

Pinging @calcmogul because of previous Discord discussion on this.
Is your feature request related to a problem? Please describe.
Not exactly a problem. This is a discussion / draft/RFC about a hypothetical waypoint editor, similar to PathWeaver but built for WPILib Trajectories and delegating generation of the trajectory from the given waypoints to a component of the robot code.

Describe the solution you'd like
Scope: At the time of posting, my stance is that the editor should not need to know anything about a particular robot’s characteristics. A Trajectory has the same waypoints independent of a robot, and the robot code can generate its own trajectory out of a given set of waypoints and robot-specific constraints. One of the eventually-discovered shortfalls of PathWeaver was that it required constraints and generated frame-by-frame points for the robot to follow.

My proposed alternative would instead export the “curve handle “ waypoint sets for later robot use.

Further thoughts:

• The waypoint export file should be JSON instead of CSV.
• The tool should use ImGUI and build off the Field2d widget, which already has support for building indexed sets of poses in the GUI.

For the record, I have already started on an ImGUI app that uses the Field2d without its Network Tables backend. I’d like to develop this project and contribute back to WPILIB if this would be welcome, but I want to hear use cases and/or problem statements with current PathWeaver, as well as feature requests.

[calcmogul]

Some thoughts from the FRC Discord discussion:

• The new tool should use North-West-Up axes convention with the origin in the bottom-left for poses. PathWeaver doesn't save waypoints in that format, so loading the file format in robot code requires transforming the poses by the field dimensions. It's doable, but it needlessly complicates the user-facing API.
• We can warn on large changes in curvature (turning radius) that correspond to large wheel accelerations; these can drastically slow the robot down after constraints are applied.
• It shouldn't be too hard to write a PathWeaver file format converter. We did that in SysId for frc-characterization data JSONs.

[sciencewhiz]

While not currently supported by Pathweaver (wpilibsuite/PathWeaver#244), there is definitely an advantage to being able to visualize the affects of the path with the trajectory constraints, without needing to implement a robot side visualization.

[calcmogul]

The path shape isn't changed by the trajectory constraints though. They only affect the velocity along the path.

[shueja]

Also, in my experience, a lot of a robot not following a path super accurately is blamed on constraints changing the path, when it's just physics not agreeing with the constrained velocity.

[calcmogul]

Being able to visualize the expected wheel velocities and voltages of the trajectory (not the path) is useful, but that requires knowledge of the robot physics model, and that generally lives in the robot simulation code. Plots of the velocity and acceleration of the center of the robot, on the other hand, aren't that useful.

If we did decide to allow adding constraints as properties of the path, it wouldn't be that hard to add time-domain plots using ImPlot (like we did in SysId). The main concern I have is the maintainability of the physics configuration UI and backend.

[shueja]

• The new tool should use North-West-Up axes convention with the origin in the bottom-left for poses.

This is one of the many reasons I'm pursuing Field2D. (Plus thanks to Peter, it already has most of the GUI capability. I will have to do something more complex than Pose2D because I need vectors with variable magnitude and reversal flag.

[shueja]

A bit of terminology clarification, now that there's a distinction that needs to be made.

This editor (which I've working-named Waymaker) does not generate trajectories. It does generate paths, in the sense that a path is a continuous vector-handle-defined curve with a direction along the curve. In other words, by setting the waypoints (position, rotation, tangent length) the user defines mathematically the course of the path. I intend to access the same functionality that is used in robot code to generate and display that path based on the given waypoints. The "waypoints export" simply expresses that path in terms of its handles.

However, velocity along that path and/or feasibility for a given robot to actually accomplish that accurately are issues for trajectory constraints and/or physics. Neither of those is something I want to bring in to Waymaker. My goal is to allow drawing the shape of the path on a field image, and to allow that drawing to serve as the description of the path in robot code, up until the point it gets generated into a trajectory with velocity.

If I am going completely off the rails with this understanding, please stop and correct me before I start coding it.

[calcmogul]

Makes sense to me.

[calcmogul]

For spline editting UI, we should avoid the way PathWeaver does it: requiring clicking and dragging the center of an existing spline to insert new waypoints.

One alternative would be left-click on the spline for waypoint creation and right-click for waypoint removal. Depends on whether we'll need right-click for a context menu.

The Glass Field2D widget might already have this in a context menu, but in case it doesn't, it would make sense to display the waypoint coordinates in a tooltip on hover and drag. PathWeaver displays that information in a separate window pane, but it's too out-of-context, in my opinion.

[shueja]

I was about to post a "stretch goal/dream goal" for spline editing, which is curve fitting to a click-drag stroke. I could imagine a strategy meeting at comps, where someone draws out the auto paths with a stylus on a touchscreen laptop, they get automatically curve-fit and "waypointed", and dropped onto the next code deploy, for the robot to run seamlessly. Obviously this is nowhere near that simple to implement, and I'm totally dreaming at this point, but it is an algorithm that has had some usage. Blender has "pencil stroke to curve", for example.

[shueja]

We know that there are always at least two waypoints. When adding a new waypoint, it is either between two existing ones, an extension added after the existing "end" point, or an extension added before the "start" point. I'm leaning towards a "click on an existing point then drag to either bisect a segment or extrude the path past its previous bounds."

[shueja]

Glass displays coordinates, rotation, and set index when hovering over a pose. It has a right click pop up menu that defaults to the mouse-position coordinates but allows exact entry, and also allows setting the index. I think you can also right click on a pose and edit it after creation.

[shueja]

Thought.
Should heading be completely independent from the rotation of the waypoint? In differential drive it has to be codirectional with the velocity, but in holonomic drives it doesn't have to be. I'm not sure how much support WPILib has or is planning to have for independent heading. That would also be a robot-specific setting that the editor would have to know about, or the importer would have to just ignore it, in which case the editor should warn "independent headings will be ignored on tank drive robots".

If this is a feasible feature, it would be an add over PathWeaver, which I assume did not implement it because it was made before there was standard controllers for trajectory and swerve.

[calcmogul]

I'm not sure how much support WPILib has or is planning to have for independent heading.

We have nothing planned. The swerve drive kinematics constraint requires knowledge of that separate heading, but we just haven't bothered to change things. It wasn't really worth the engineering effort when users can just ignore the trajectory heading and use a separate trapezoid profile for the heading instead, and it's close enough.

[shueja]

I'd have to hear from swerve teams to see what their actual use cases are for independent heading. If it's to follow the trajectory without rotating, then it's not something to set per-waypoint.

[jlmcmchl]

We ran swerve on 3538 for IR@Home - I can certainly see it having value for games like 2019 where facing the right direction @ the end of a path was a critical component of autonomous design. In a swerve drive, you can't drive at top speed and rotate at top speed at the same time, they're coupled. While you could leave some headroom in your max velocity / accel to accommodate turning while translating, there's no easy way to ensure that translating gets priority (if you're rotating too quickly, you will translate more slowly, and fall behind your goal trajectory state).

Could be implemented cheaply as a post-processing step? Since you could adjust your trajectory duration based on how much time is added to turn while translating. I'm not sure if it would grant the same benefits as a coupled trajectory generation step though.

[shueja]

I talked with Tyler (calcmogul) on the Discord earlier, and determined that generation of swerve/independent heading trajectories is not something for WayMaker or WPILib at this point. However... I do plan to allow setting a heading independent of the instantaneous direction of travel. My current brain plan is to allow a mode switch between "heading is either forwards or backwards along your direction of travel" for diff drive, and full independent for holonomic.
I also plan to provide robot code snippets (in lieu of a WPILib merge or a separate vendordep) that will read the WayMaker JSONs and produce either a Trajectory, with the independent heading ignored, or a list of waypoints in a Java/C++ native object. I intend to leave the use of that produced information up to others to integrate into what have they. It doesn't seem proper to have the dashboard do too much in customization of the trajectory beyond points.