LabVIEW trajectory library

@BCR-Jim - So I don’t know much about swerve drive. I also haven’t thought too much about next steps. I thought I would maybe 0) concentrate on getting the teams I help through there competitions… 1) fix bugs anyone found, 2) document the VI’s, 3) set the VI properties to make them more efficient (subroutine, and inline where useful), 4) implement things more of the things that were left out. For 4) I was thinking of the missing differential drive constraint; maybe adding an acceleration limiting constraint like pathfinder has (“jerk”), although I’m uncertain it this had real world benefits or not (I think it might); seeing what others thought would be useful – like swerve drive or whatever. I’d also be happy to have help with the implementation…

@MazeOfEncryption - On a different subject, the through bore encoders work really nicely…

There are a couple of reasons for putting common code in Periodic Tasks and using global variables with wrapper subVI’s to write “demands” to the the Periodic Tasks code.

  1. so not to duplicate code…
  2. More importantly, to not get into interaction issues when switching from autonomous to teleop. For example if the drive system is taking a “target” then rate limiting it to get actual “demand”, the rate limiting depends on remembering its previous state. Putting the code in Auto and in teleop might cause an un-wanted step change when switching between the two. Same thing with a state machine to control a shooter. (And last year there was some blurring of the two modes.)
  3. I just think it is easier. (Not very scientific.)
  4. Maybe to keep the teleop more “lightweight” so it can’t fall behind. (Also note that I put the network table writes in a different loop in periodic tasks so not to affect the control loop. LabVIEW may be running this in a different thread, although it may run code in the same loop in different threads too … I think.)

I have been teaching Acquire, Decide, Act method. (Acquire reads, scales and filters sensor values – include joysticks. Decide uses this data to decide what to ask each system to do – what drive speed do we want, what elevator height do we want, what ball shooter/gatherer system mode do we want. Act takes the desired things set by Decide and runs the low level control code, rate limiting, position control, state machines, whatever to turn these demands into actuator outputs (motor demands, solenoids demands, etc…) This makes it easier to compartmentalize the code and allows more people to work on coding.

The detriments of this separation are perhaps a little bit of extra latency. (at most 20 milliseconds). Generally this didn’t seem to be enough to worry about.

In the end – either way would work okay.

1 Like

Ah, that makes a lot of sense. I didn’t think about the fact that periodic tasks would be running during auto too. Thank you!

Edit: Also, what about through-bore encoders?

If I have your team correct, I noticed your robot at the GOS week zero event. Specifically noticed the through bore encoders. We ended up getting these to help see what our shooter is doing and control the speed. (We were the team scaring others with our shooter… I hope we have some of the that fixed. :slight_smile: )

We were at the GoS week 0 scrimmage, but our team doesn’t have any through-bore encoders on our robot. I’m from the Titanium Titans.

Awesome to see more resources like this developed for LabVIEW teams.
1712 built our own path drawer to work with our pure pursuit controller in 2018. The lead programmer on that effort (now an alumni) wrote a whitepaper on the effort. The actual code can be found here, and here are specific links to the robot simulator, pure pursuit controller, and path drawer.
Huge thanks to 2168 for supplying one of the underlying algorithms and for 254’s 2014 autonomous routines being the original inspiration to implement pure pursuit and motion profiling.

Here’s a presentation that was given at 272’s 2018 Comcast Bootcamp
pure-pursuit(1).pdf (13.0 MB)

And our 2019 control system’s handout
Dawgma2019ControlSystemsHandout.pdf (509.4 KB)

I did a bit of black magic with property nodes…

That’s really fun.

1 Like

Our team is playing with your trajectory generator and follower code. I have to say - this is some awesomely clean LabVIEW code.

If we weren’t locked out of the school, we were planning on getting it running on the robot this weekend.

I had a couple questions. Is the ‘reverse’ function of the WPI trajectory code in there somewhere that I missed? Or are running backwards trajectories not implemented?

Next - what’s the difference between Max_Velocity and “Real Max Speed”?

Last - is there an easy way to remove a point once you’ve added it?

We were planning on hacking in a swerve trajectory follower in as quickly as possible in the leadup to our competition scheduled for this weekend. Under the circumstances, I might now do a proper port of all the WPIlib swerve and mecanum classes, following your style.

Hi

Sorry, I haven’t looked at this in a couple of days.

I’ll try and answer your questions.

  1. reverse. This does exist in the trajectory config data structure. There isn’t a method to set this other than using cluster pack/unpack. Reverse is not implemented in the trajectory creation tool yet. I haven’t tested any of the reverse logic yet. (Initially I only implemented enough to get things working. I’ve gone back to add missing compatibility, but it is far from complete. The first things implemented, ramsete and odometry were the least “identicial” with the java/c++. I’ve updated the ramsete, but not the odometry. I’ve also updated the splines so that cubic and quintic are implemented. I updated the tool too. I haven’t published these updates yet. I was trying to get a little more done before doing this.)

  2. Max Velocity and Real Max Speed. – The trajectory creation utility that uses pathfinder didn’t have a method to constrain the speed of the individual wheels of differential drive robot (or any other kind of robot.) I added “real max speed” to be the top actual physical speed of the robot. The trajectory is created using “max speed”.
    image

Then the trajectory is divided into individual wheel demands. If any of these exceeds “real max speed”, an indicator lights shows this. Max speed can be reduced and the trajectory re-created. (Not a very elegant solution, but it worked for me…)
image

  1. In the utilities, you can delete an array element or entire data set. Run the program. (It might even work when the program isn’t running.) Right click on the data, and select “Delete element” (I found this after getting frustrated and playing around some…)
    image
    If you select the entire array, you can delete all the data elements.
    image

If I misinterpreted the questions, let me know. Hope this helps.

Let me know if you are interested in the newest version of this code. If so, I’ll try and I’ll package and post.

I really briefly looked at the swerve stuff. It looked like some or all of the following three classes would need to be implemented.
Kinematics directory

  • swerveDriveKinematics
  • swerveDriveOdometry
  • swerveModuleState
    Trajectory\Constraint directory
  • swerveDriveKinematics (Note that constraints are implemented somewhat differently, organizationally, than the java/c++)

I’ve made some significant changes to the libraries. I’ll try and post the updated code so you can use that as the base.

If you implement the swerve, I’d really like to include it with the rest of the library if you are willing to provide it.

One or two more things for today. I found that changing the ramsete gain parameters allows the simulated robots to perform much better. (2 -> 3.5, 0.7 -> 0.35) Also, I was able to get pathfinder trajectory creation to work on the roboRIO. I have a new robot sample project that uses this. (There are now 4 sample robot projects and a sample dashboard for running with the simulated robots.) This isn’t posted yet. Hopefully today or tomorrow.

Absolutely I’m interest in the newest code. I can say without reservation that barring a major problem, this will be our navigation system next year. Our current system works well and is very quick to create a new auto, but it’s not as accurate as trajectory planning.

I was going to go through and start commenting it thoroughly, but if you’re still updating it I’ll wait. If you want to tell me VI’s that you aren’t touching, I can go through and thoroughly comment those and send them back to you. I want these to be understandable from the viewpoint of my incoming freshmen. I’m pretty excited about this! Working through it and understanding it is a great placeholder for robotics meetings.

As far as I’m concerned, once you are done this should be incorporated into the basic robot project in the same way that WPILib gives new java/etc programmers the ability to do trajectories.

Here is a mostly up to date snapshot of the library.FRC_LabVIEW_Trajectory_Library_Routines_00_01.pdf (76.8 KB)

Another quick note before I forget. When originally creating this library using only the Pathfinder “paths”, I implemented a way to execute a trajectory backwards. This allowed a robot to use the same trajectory data structure in either direction. When implementing the WPI library calls this functionality disappeared, but I’ve been thinking about resurrecting it, at least as part of one of the samples.

:astonished:

Damn. That’s a lot of work on your part. Thank you so much.

I posted the ZIP file of library version 1.1. Use the original link. Make certain to get the file with the newest date and version, since both ZIP files are sill there.

This version has a much more complete implementation of the spline routines, a more complete implementation of the ramsete controller, 2 additional robot samples, and some other things.

Documentation is still a work in progress. There is a spreadsheet listing what is and isn’t done yet.

For convenience, here is the link (same link as the first post).
https://drive.google.com/open?id=14KZICKT-SQjeS49M0bDEz_fxdLbrXlEP

Jim S.

Those three and SwerveControllerCommand in place of RamseteCommand and RamseteController. Ironically, that’s actually significantly easier; it’s just three individual controllers for the two axes and rotation. All the fancy stuff is in the kinematics and odometry.

I’ll have to take a closer look at whether to also include generic swerve classes or not. I don’t think I need to, but at that point I might just do it for completion.

@JSIMPSO

Jim, I was playing with this today and I noticed that when I use the pathweaver utility to generate a path, even with the dT set to .020, the resulting output file is in steps of .060

Out of curiosity, I switched over to the trajectory utility, the time steps are variable. For instance, for the default path, the time steps look like this:
#-------- Time
0
0.33561
0.47256
0.5763
0.66269
0.73791
0.80515
0.86633
0.9227
0.97516
1.02434

I was always under the impression that for these trajectories you would expect to see regular time intervals - and with the pathfinder dt set at .020 I would expect 20 ms intervals. Are my assumptions wrong?

I sent you a direct message with more information… The WPILIB routines output samples based on deviations so the samples don’t have an even dT. I haven’t looked at the source of PathWeaver so I don’t know what it does. The library has a trajectory sample routine to get an interpolated sample given any time. I kept this as it was for the LabVIEW library.