TankModifier needs to produce negative velocity for inside wheel on tight curves

[DrewTheRat]

For tight curves in a trajectory, the Tank Modifier needs to sometimes produce negative velocity, etc. on the wheels on the inside of the curve. This would be for any curve where the radius is less than the wheelbase.

Right now the the distance value being calculated is correct except for the sign. I think the issue is in Pathfinder-Core/src/modifiers/tank.c lines 20-22 and 36-39:

double distance = sqrt(
    (left.x - last.x) * (left.x - last.x)
    + (left.y - last.y) * (left.y - last.y)
);

That calculation always yields a positive distance, even if the current point is behind the last point. I can tell there needs to be a check for that scenario, but I cannot quite figure it out.

Also, here is a trajectory that exhibits this issue:

Fit Method  : HERMITE_CUBIC
Time Step   : 0.02
Max Velocity: 4.0
Max Accel.  : 3.0
Max Jerk    : 60

Wheelbase   : 3.4

Trajectory (X,Y,Angle)
3.00,  23.21, 0.00
17.00, 23.21, 0.00
20.65, 20.50, 90.00
17.42, 18.15, -180.00

[vargoose84]

I also wouldn't mind this enhancement. Sometimes it is unfeasible to do "farmer's turns" or s turns to get from point a to b

[JaciBrunning]

I'll look at this in more detail soon, but this is a known design constraint of Pathfinder, not necessarily a bug.

[DrewTheRat]

I am thinking something like this will do it:

Segment last_seg = original[i - 1];
if ((last_seg.x > seg.x && last.x < left.x) || (last_seg.y > seg.y && last.y < left.y) ||
    (last_seg.x < seg.x && last.x > left.x) || (last_seg.y < seg.y && last.y > left.y)) {
        distance = -distance;
}

Trying to determine if the left (or right) point is changing in the opposite direction as the center point.... There might be a cleaner way to do that. We'll try to test this today. If we have working code, I'll send a pull request.

(And BTW, thank you for all your work in creating Pathfinder.)

[JaciBrunning]

Sounds good. Keep in mind if you do PR I'll merge after bag day since it's way too close to the end of build to make any potential stability changes

[vargoose84]

I would like to echo Drew's comments and just say that your code has proven invaluable to our team.

Drew keep us up to date on your testing it may be very useful for our team

[vargoose84]

Drew did you test your change? How'd it end up?

[vargoose84]

Finally getting a chance to look at this again. We're you guys able to implement.

I'm ready to experiment! The implementation seems simple enough.

[DrewTheRat]

We needed to soften our turns for stability, so this ended up not impacting our profiles this year. Without that requirement, we've been heads-down on other issues, so I have had no time to experiment with this.

The code snippet I posted was to check if the one side of a tank drive wants to move in the opposite direction of the profile. I don't think it will help with completely reversed profile (#39), as the check is based on comparison to the original path.

[Gregadeaux]

Did a quick test with @DrewTheRat's snippet, and it seems like it isn't going to work. Unfortunately, the switch from positive to negative isn't a graceful switch, and ends up jumping the velocity absurd amounts.

[DrewTheRat]

@Gregadeaux - I saw that behavior also, which is why I haven't submitted a PR for this. I am looking into a better way test if one side is going in a different direction. It's been a while since I had to do this kind of math....

[tylertian123]

I know it's been a while since this issue was active, but I have some info that might help. I have worked on my own version of Pathfinder for my team here: https://github.com/Arctos6135/RobotPathfinder, and I have successfully found a way to make the inside wheel go backwards during tight turns.

The basic idea is that we need to figure out the velocities for the wheels using some calculations, instead of numerical integration. I derived a formula, relating the velocities for the left and right wheels (v1 and v2) to the overall velocity of the robot (v), the radius of the robot's base plate (b, half of the base width), and the radius of the path the robot's following (r). The equations look something like this:
v1 = v - (v/r) * b, v2 = v + (v/r) * b

Using this method, negative velocities can be generated on tight turns. Then, the distance can also be given the correct sign by checking the sign of the velocities:

if(leftVelocity < 0) {
	dxLeft = -dxLeft;
}
if(rightVelocity < 0) {
	dxRight = -dxRight;
}

That worked perfectly for me. However, the only problem that prevents using this method in Pathfinder is how to get the radius of the path (r). In my project, I got the radius by first calculating the curvature, then taking its inverse.
This requires that we know the derivative and second derivative of the path at any given point, which I believe is not something that Pathfinder has at this moment. I've looked a bit into the structure of this library, and (to me) it seems like that a very big structural change would be needed to incorporate this.

At this point, with Pathfinder 2 under development, it might not even be worth it to spend a large amount of time changing the basic structure just to get this feature working. However, if there's someone out there who is familiar with the structure and wants to incorporate this feature, I believe this is the way to do it.

If anyone is interested, the files that generate the trajectories are BasicTrajectory.java and TankDriveTrajectory.java. Hope this helps.