search

nobleo / path_tracking_pid

Path Tracking PID offers a tuneable PID control loop, decouling steering and forward velocity
Apache License 2.0
129 stars 37 forks source link

Angle calculation of closestPointOnSegment() should be changed #134

Open cesar-lopez-mar opened 2 years ago

cesar-lopez-mar commented 2 years ago

Currently, closestPointOnSegment uses the angle of the start pose (or the one defined from start to end) to the returned Pose

A better idea is to interpolate the angle from start to end (when those angles are given). This will avoid "step" discontinuities of the returned Pose angle between segments, which I saw recently in one of our projects.

rokusottervanger commented 2 years ago

If this angle is used in feedforward action calculation, that would mean the robot starts steering before it reaches the pose that prescribes a rotation. Hence, it will "cut the corner". Is this desired behavior?

cesar-lopez-mar commented 2 years ago

Instead of "cutting corners", we avoid overshooting. In practice the robot will not move in straight lines connecting poses, it will do in curves, so it will never follow corners. The idea with the FF is to start steering such that by the time the robot reaches the next pose, it already has the right orientation.

cesar-lopez-mar commented 2 years ago

I gave it another thought and actually after the angle interpolation the FF action will be the same and also with discontinuities. This because the way it is computed, using a Pure-pursuit strategy (See https://www.hindawi.com/journals/ijvt/2016/2306818/ section 2.2). Along that segment the steering to reach the next pose will remain the same after interpolation.

Still the interpolation will have a positive impact by avoiding discontinuities in the calculated carrot (the PGP) when using track_base_link. So this issue remain valid, and perhaps we can open another one for having smoother FF.

rokusottervanger commented 2 years ago

Interesting... You're right. To get smoother FF, you'd need to use more information. You would also need to use points/poses further ahead. Is that an issue in any running project(s)?

About smoothing the PGP: maybe we should start by writing a test. This way, we can first see what the current behavior is, and it forces us to think about appropriate requirements before we start to add such an improvement feature.

cesar-lopez-mar commented 2 years ago

I did notice some "step-like" behavior in the steering of one of our vehicles, in the mean time I solved it by generating a path with higher resolution, but perhaps the path source is not always to modify so an interpolation feature might be useful. In any case, yes let's start with a test. We should take this to an internal discussion.