Pathfinder - Fast Motion Profiling for Java and C++, Tank and Swerve Drive

[AustinSchuh]

Originally Posted by Jaci

I chose that system because it seemed sensical if you view your robot starting from the midline and viewing the field from top-down. Again, you can change the followers if you want to modify the coordinate system

That means you either picked a left hand coordinate system, which is a pain to work with, or Z is down. Left hand coordinate systems have - signs that show up in weird spots in all the equations, and aren't common.

[artK]

Originally Posted by Jaci

The way it currently works is like so:
X+ is forward from where your robot starts.
X- is backward from where your robot starts.
Y+ is to the right of your robot where it starts.
Y- is to the left of your robot where it starts.

Positive headings are going from X+ towards Y+, Negative Headings from X+ to Y-. As for the actual following of the heading, that depends on where your gyroscope is zero'd to.

As with anything, these coordinates are useless unless you have the follower code to interpret them. X and Y coordinate directions can be flipped by just sending them to different motor outputs, for example

The way the splines were defined in Trajectory-Lib (which it looks like you kept everything in it) was as follows:

• The coordinate system used to solve it changes for each pair of adjacent waypoints.
• The origin is your starting waypoint.
• X+ is the ray from the origin to the second waypoint
• This picture describes what the rest of it looks like.

If we define theta to be the angle between any heading in the spline and the x-axis for that spline, |theta| < PI/2, or else lose the polynomial solution we came up with. This is a weakness in using our geometry, but it works in most of the cases you would see and comes up with a clean solution.

If you wanted to make a curve that violates those properties, you could put an intermediate waypoint on the desired curve. This isn't a perfect solution, but it fixes the coordinate system to the point where it can generate splines.

[Jaci]

This image describes Pathfinder's coordinate system, similar to what you'd find if you overlaid a cartesian plane on the game field, where the origin is where your robot starts.
On the graph above are plotted the waypoints:
[0, 0 @ 25 deg], [4.5, 2 @ 0 deg], [7.5, 3 @ 0 deg], [9, 1 @ 0 deg]

The heading is dependent on where your Gyroscope is zero'd to, which is usually where you set your robot up.

If you want to think of it another way, you could say that the X and Y axis are both parallel to the plane of the field carpet.

[Ben Wolsieffer]

I'm currently testing Pathfinder, and it seems to not be following the constraints specified in the config. It accelerates extremely fast, reaching a max speed of 25 m/s by the second point. This happens no matter what the max speed, acceleration and jerk are.

[AirplaneWins]

Im a little confused, what exactly do the coordinates mean, like how far is one unit. Can someone overlay this over a map of the field and show me?

[Ben Wolsieffer]

I was in a hurry when I wrote my last post (time restricted unbag periods are mixed blessing), so I'll try to explain the problem I am running into better now.

I was generating this waypoint sequence:

public static final Waypoint[] LOW_BAR_AUTONOMOUS_WAYPOINTS =
            { new Waypoint(0.5, 0.5, 0), new Waypoint(1, 0.5, Math.toRadians(45)) };

using this config (acceleration and max velocity are very low for testing):

public static final Trajectory.Config AUTONOMOUS_TRAJECTORY_CONFIG = new Config(Trajectory.FitMethod.HERMITE_CUBIC,
            Trajectory.Config.SAMPLES_HIGH, DRIVE_SUBSYSTEM_TRAJECTORY_PERIOD, 0.2, 0.5, 40);

When the trajectory was generated, it seemed to ignore the max acceleration and velocity values. The left and right trajectories accelerate extremely quickly, reaching 25 m/s within 0.01 seconds (also, jerk was around 24000 ). The speed maxes out at 25 m/s. I never checked the source trajectory, so I don't know if the it had the correct velocity values.

I just realized that I was not using the real wheelbase width, only a dummy value (0.5 m), but this is still reasonable and should not have caused this problem.

[Jaci]

Originally Posted by lopsided98

I was in a hurry when I wrote my last post (time restricted unbag periods are mixed blessing), so I'll try to explain the problem I am running into better now.

I was generating this waypoint sequence:

When the trajectory was generated, it seemed to ignore the max acceleration and velocity values. The left and right trajectories accelerate extremely quickly, reaching 25 m/s within 0.01 seconds (also, jerk was around 24000 ). The speed maxes out at 25 m/s. I never checked the source trajectory, so I don't know if the it had the correct velocity values.

I just realized that I was not using the real wheelbase width, only a dummy value (0.5 m), but this is still reasonable and should not have caused this problem.

I just generated a source trajectory with the values, and it seems absolutely fine, so I'd instead suggest that the issue is with the follower instead. The only thing I can think of is that the encoder values or kV or kA terms are incorrect in your follower, causing the issues.

[Jaci]

Originally Posted by AirplaneWins

Im a little confused, what exactly do the coordinates mean, like how far is one unit. Can someone overlay this over a map of the field and show me?

All the values pathfinder accepts can be in any unit you like, but they have to be consistent (i.e. if x/y is in feet, velocity must be in feet per second). We tend to use Meters as our units in most cases, but there's nothing stopping you using feet.

If your robot starts perpendicular to the midline (i.e. facing the outer works), X+ will be facing towards the tower, and Y+ will be facing the secret passage. You can flip these coordinates by reversing which motors each value goes to in your follower.

[Ben Wolsieffer]

I'm pretty sure the problem is not in the follower, because I found these velocity values by looking at the generated trajectory with the debugger. I will try to write a test application today to see if I can figure out what's wrong.

Maybe it has something to do with generating multiple trajectories at the same time. The robot has a thread pool generating 6 trajectories at once when it starts. Does the generation use any global variables that would cause race conditions?

[Jaci]

Originally Posted by lopsided98

I'm pretty sure the problem is not in the follower, because I found these velocity values by looking at the generated trajectory with the debugger. I will try to write a test application today to see if I can figure out what's wrong.

Maybe it has something to do with generating multiple trajectories at the same time. The robot has a thread pool generating 6 trajectories at once when it starts. Does the generation use any global variables that would cause race conditions?

it shouldn't do. I'll take a look when I get a free moment. I've filed a bug report against myself here for now

[Ben Wolsieffer]

I was able to reproduce the bug on my computer. The source trajectory that is generated is fine, but when the TankModifier is applied, the really huge velocities appear. My testing has shown that it happens whether or not multithreading is used.

[Jaci]

Originally Posted by lopsided98

I was able to reproduce the bug on my computer. The source trajectory that is generated is fine, but when the TankModifier is applied, the really huge velocities appear. My testing has shown that it happens whether or not multithreading is used.

Try this newest release. It fixes successive generations of trajectories and may solve this issue as a result. If you have any further issues, please direct them to the Github Issues page

https://github.com/JacisNonsense/Pathfinder/releases

[Ben Wolsieffer]

I finally got the time to get Pathfinder working (hopefully we will be able to use it at BattleCry@WPI), and here is a demo video of our robot following a simple trajectory that is similar to what we will use to get over a defense and line up for our auto shot.

https://youtu.be/m4pbv1_FC5I

Also, I attached a picture of a web interface I wrote (using pynetworktables2js) that allows me to see how well the robot is following the trajectory and easily tune the parameters. (Also, disregard the weird looking data, I was working on a problem when I took the screenshot)

[Jaci]

Originally Posted by lopsided98

I finally got the time to get Pathfinder working (hopefully we will be able to use it at BattleCry@WPI), and here is a demo video of our robot following a simple trajectory that is similar to what we will use to get over a defense and line up for our auto shot.

https://youtu.be/m4pbv1_FC5I

Also, I attached a picture of a web interface I wrote (using pynetworktables2js) that allows me to see how well the robot is following the trajectory and easily tune the parameters. (Also, disregard the weird looking data, I was working on a problem when I took the screenshot)

Looks good! What are you using to generate the graphs?

Once our robot lands back in Aus I'll see if I can do a demo with it, after my exams of course

[Ben Wolsieffer]

Originally Posted by Jaci

Looks good! What are you using to generate the graphs?

I am using Plotly.js, which is being fed data from NetworkTables. I'm starting to wonder if there is a better alternative, though, because it seems to have trouble dealing with data that updates many times per second.