Generating very small paths makes position disagree with velocity

[bnhrkn]

When I want to generate a very short and straight path (only several centimeters only), the generated path coordinates overshoot and undershoot the target. The velocity does not agree with the change in the x.

This commit prints the following when squiggles_tst is executed.

Notice, the X position changes sign but the velocity remains positive. The result is that the robot moves smoothly really far in a straight line (when no path following algorithm is used).

Output

X: 0.00399803 Y: 0 Yaw: 0 Vel: 0.02
X: 0.00798439 Y: 0 Yaw: 0 Vel: 0.04
X: 0.0119479 Y: 0 Yaw: 0 Vel: 0.06
X: 0.0158779 Y: 0 Yaw: 0 Vel: 0.08
X: 0.0197643 Y: 0 Yaw: 0 Vel: 0.1
X: 0.0235973 Y: 0 Yaw: 0 Vel: 0.12
X: 0.0273679 Y: 0 Yaw: 0 Vel: 0.14
X: 0.0310673 Y: 0 Yaw: 0 Vel: 0.16
X: 0.0346874 Y: 0 Yaw: 0 Vel: 0.18
X: 0.0382204 Y: 0 Yaw: 0 Vel: 0.2
X: 0.041659 Y: 0 Yaw: 0 Vel: 0.22
X: 0.0449965 Y: 0 Yaw: 0 Vel: 0.24
X: 0.0482265 Y: 0 Yaw: 0 Vel: 0.26
X: 0.0513431 Y: 0 Yaw: 0 Vel: 0.28
X: 0.0543407 Y: 0 Yaw: 0 Vel: 0.3
X: 0.0572143 Y: 0 Yaw: 0 Vel: 0.32
X: 0.0599592 Y: 0 Yaw: 0 Vel: 0.34
X: 0.0625712 Y: 0 Yaw: 0 Vel: 0.36
X: 0.0650465 Y: 0 Yaw: 0 Vel: 0.38
X: 0.0673814 Y: 0 Yaw: 0 Vel: 0.4
X: 0.0695731 Y: 0 Yaw: 0 Vel: 0.42
X: 0.0716187 Y: 0 Yaw: 0 Vel: 0.44
X: 0.0735159 Y: 0 Yaw: 0 Vel: 0.46
X: 0.0752627 Y: 0 Yaw: 0 Vel: 0.48
X: 0.0768576 Y: 0 Yaw: 0 Vel: 0.5
X: 0.0782992 Y: 0 Yaw: 0 Vel: 0.52
X: 0.0795865 Y: 0 Yaw: 0 Vel: 0.54
X: 0.080719 Y: 0 Yaw: 0 Vel: 0.56
X: 0.0816963 Y: 0 Yaw: 0 Vel: 0.58
X: 0.0825185 Y: 0 Yaw: 0 Vel: 0.6
X: 0.0831858 Y: 0 Yaw: 0 Vel: 0.62
X: 0.0836989 Y: 0 Yaw: 0 Vel: 0.64
X: 0.0840586 Y: 0 Yaw: 0 Vel: 0.66
X: 0.0842662 Y: 0 Yaw: 0 Vel: 0.68
X: 0.084323 Y: 0 Yaw: 3.14159 Vel: 0.7
X: 0.0842308 Y: 0 Yaw: 3.14159 Vel: 0.72
X: 0.0839916 Y: 0 Yaw: 3.14159 Vel: 0.74
X: 0.0836075 Y: 0 Yaw: 3.14159 Vel: 0.76
X: 0.083081 Y: 0 Yaw: 3.14159 Vel: 0.78
X: 0.0824148 Y: 0 Yaw: 3.14159 Vel: 0.8
X: 0.0816118 Y: 0 Yaw: 3.14159 Vel: 0.82
X: 0.0806751 Y: 0 Yaw: 3.14159 Vel: 0.84
X: 0.0796081 Y: 0 Yaw: 3.14159 Vel: 0.86
X: 0.0784144 Y: 0 Yaw: 3.14159 Vel: 0.88
X: 0.0770975 Y: 0 Yaw: 3.14159 Vel: 0.9
X: 0.0756615 Y: 0 Yaw: 3.14159 Vel: 0.92
X: 0.0741104 Y: 0 Yaw: 3.14159 Vel: 0.94
X: 0.0724485 Y: 0 Yaw: 3.14159 Vel: 0.96
X: 0.0706803 Y: 0 Yaw: 3.14159 Vel: 0.98
X: 0.0688102 Y: 0 Yaw: 3.14159 Vel: 1
X: 0.0668431 Y: 0 Yaw: 3.14159 Vel: 1.02
X: 0.0647837 Y: 0 Yaw: 3.14159 Vel: 1.04
X: 0.0626371 Y: 0 Yaw: 3.14159 Vel: 1.06
X: 0.0604084 Y: 0 Yaw: 3.14159 Vel: 1.08
X: 0.0581027 Y: 0 Yaw: 3.14159 Vel: 1.1
X: 0.0557255 Y: 0 Yaw: 3.14159 Vel: 1.12
X: 0.0532821 Y: 0 Yaw: 3.14159 Vel: 1.14
X: 0.050778 Y: 0 Yaw: 3.14159 Vel: 1.16
X: 0.048219 Y: 0 Yaw: 3.14159 Vel: 1.18
X: 0.0456105 Y: 0 Yaw: 3.14159 Vel: 1.2
X: 0.0429584 Y: 0 Yaw: 3.14159 Vel: 1.22
X: 0.0402685 Y: 0 Yaw: 3.14159 Vel: 1.24
X: 0.0375467 Y: 0 Yaw: 3.14159 Vel: 1.26
X: 0.0347987 Y: 0 Yaw: 3.14159 Vel: 1.28
X: 0.0320305 Y: 0 Yaw: 3.14159 Vel: 1.3
X: 0.0292482 Y: 0 Yaw: 3.14159 Vel: 1.32
X: 0.0264575 Y: 0 Yaw: 3.14159 Vel: 1.34
X: 0.0236647 Y: 0 Yaw: 3.14159 Vel: 1.33515
X: 0.0208755 Y: 0 Yaw: 3.14159 Vel: 1.31515
X: 0.018096 Y: 0 Yaw: 3.14159 Vel: 1.29515
X: 0.0153322 Y: 0 Yaw: 3.14159 Vel: 1.27515
X: 0.0125901 Y: 0 Yaw: 3.14159 Vel: 1.25515
X: 0.00987539 Y: 0 Yaw: 3.14159 Vel: 1.23515
X: 0.00719413 Y: 0 Yaw: 3.14159 Vel: 1.21515
X: 0.00455212 Y: 0 Yaw: 3.14159 Vel: 1.19515
X: 0.00195514 Y: 0 Yaw: 3.14159 Vel: 1.17515
X: -0.000591092 Y: 0 Yaw: 3.14159 Vel: 1.15515
X: -0.00308092 Y: 0 Yaw: 3.14159 Vel: 1.13515
X: -0.00550876 Y: 0 Yaw: 3.14159 Vel: 1.11515
X: -0.00786912 Y: 0 Yaw: 3.14159 Vel: 1.09515
X: -0.0101566 Y: 0 Yaw: 3.14159 Vel: 1.07515
X: -0.0123659 Y: 0 Yaw: 3.14159 Vel: 1.05515
X: -0.0144918 Y: 0 Yaw: 3.14159 Vel: 1.03515
X: -0.0165292 Y: 0 Yaw: 3.14159 Vel: 1.01515
X: -0.0184732 Y: 0 Yaw: 3.14159 Vel: 0.995146
X: -0.020319 Y: 0 Yaw: 3.14159 Vel: 0.975146
X: -0.0220619 Y: 0 Yaw: 3.14159 Vel: 0.955146
X: -0.0236973 Y: 0 Yaw: 3.14159 Vel: 0.935146
X: -0.0252208 Y: 0 Yaw: 3.14159 Vel: 0.915146
X: -0.0266282 Y: 0 Yaw: 3.14159 Vel: 0.895146
X: -0.0279156 Y: 0 Yaw: 3.14159 Vel: 0.875146
X: -0.0290789 Y: 0 Yaw: 3.14159 Vel: 0.855146
X: -0.0301146 Y: 0 Yaw: 3.14159 Vel: 0.835146
X: -0.0310191 Y: 0 Yaw: 3.14159 Vel: 0.815146
X: -0.0317892 Y: 0 Yaw: 3.14159 Vel: 0.795146
X: -0.0324217 Y: 0 Yaw: 3.14159 Vel: 0.775146
X: -0.0329139 Y: 0 Yaw: 3.14159 Vel: 0.755146
X: -0.0332631 Y: 0 Yaw: 3.14159 Vel: 0.735146
X: -0.0334668 Y: 0 Yaw: 3.14159 Vel: 0.715146
X: -0.033523 Y: 0 Yaw: 0 Vel: 0.695146
X: -0.0334297 Y: 0 Yaw: 0 Vel: 0.675146
X: -0.0331854 Y: 0 Yaw: 0 Vel: 0.655146
X: -0.0327885 Y: 0 Yaw: 0 Vel: 0.635146
X: -0.0322381 Y: 0 Yaw: 0 Vel: 0.615146
X: -0.0315332 Y: 0 Yaw: 0 Vel: 0.595146
X: -0.0306734 Y: 0 Yaw: 0 Vel: 0.575146
X: -0.0296584 Y: 0 Yaw: 0 Vel: 0.555146
X: -0.0284883 Y: 0 Yaw: 0 Vel: 0.535146
X: -0.0271634 Y: 0 Yaw: 0 Vel: 0.515146
X: -0.0256845 Y: 0 Yaw: 0 Vel: 0.495146
X: -0.0240526 Y: 0 Yaw: 0 Vel: 0.475146
X: -0.0222691 Y: 0 Yaw: 0 Vel: 0.455146
X: -0.0203357 Y: 0 Yaw: 0 Vel: 0.435146
X: -0.0182544 Y: 0 Yaw: 0 Vel: 0.415146
X: -0.0160277 Y: 0 Yaw: 0 Vel: 0.395146
X: -0.0136584 Y: 0 Yaw: 0 Vel: 0.375146
X: -0.0111497 Y: 0 Yaw: 0 Vel: 0.355146
X: -0.00850504 Y: 0 Yaw: 0 Vel: 0.335146
X: -0.00572848 Y: 0 Yaw: 0 Vel: 0.315146
X: -0.00282433 Y: 0 Yaw: 0 Vel: 0.295146
X: 0.000202637 Y: 0 Yaw: 0 Vel: 0.275146
X: 0.00334725 Y: 0 Yaw: 0 Vel: 0.255146
X: 0.0066039 Y: 0 Yaw: 0 Vel: 0.235146
X: 0.00996657 Y: 0 Yaw: 0 Vel: 0.215146
X: 0.0134288 Y: 0 Yaw: 0 Vel: 0.195146
X: 0.0169836 Y: 0 Yaw: 0 Vel: 0.175146
X: 0.0206236 Y: 0 Yaw: 0 Vel: 0.155146
X: 0.0243411 Y: 0 Yaw: 0 Vel: 0.135146
X: 0.0281276 Y: 0 Yaw: 0 Vel: 0.115146
X: 0.0319745 Y: 0 Yaw: 0 Vel: 0.0951455
X: 0.0358723 Y: 0 Yaw: 0 Vel: 0.0751455
X: 0.0398114 Y: 0 Yaw: 0 Vel: 0.0551455
X: 0.0437815 Y: 0 Yaw: 0 Vel: 0.0351455
X: 0.0477718 Y: 0 Yaw: 0 Vel: 0.0151455
[==========] Running 19 tests from 5 test suites.
[----------] Global test environment set-up.
[----------] 6 tests from impose_limits_test
[ RUN      ] impose_limits_test.cap_velocity_1d
[       OK ] impose_limits_test.cap_velocity_1d (0 ms)
[ RUN      ] impose_limits_test.cap_accel_1d
[       OK ] impose_limits_test.cap_accel_1d (2 ms)
[ RUN      ] impose_limits_test.cap_both_1d
[       OK ] impose_limits_test.cap_both_1d (1 ms)
[ RUN      ] impose_limits_test.cap_velocity_2d
[       OK ] impose_limits_test.cap_velocity_2d (0 ms)
[ RUN      ] impose_limits_test.cap_accel_2d
[       OK ] impose_limits_test.cap_accel_2d (2 ms)
[ RUN      ] impose_limits_test.cap_both_2d
[       OK ] impose_limits_test.cap_both_2d (1 ms)
[----------] 6 tests from impose_limits_test (8 ms total)

[----------] 4 tests from io_test
[ RUN      ] io_test.serialize_path_test
[       OK ] io_test.serialize_path_test (0 ms)
[ RUN      ] io_test.deserialize_path_test
[       OK ] io_test.deserialize_path_test (0 ms)
[ RUN      ] io_test.wheel_vels
[       OK ] io_test.wheel_vels (1 ms)
[ RUN      ] io_test.pathfinder
[       OK ] io_test.pathfinder (3 ms)
[----------] 4 tests from io_test (6 ms total)

[----------] 3 tests from model_constraints_test
[ RUN      ] model_constraints_test.min_constraints
[       OK ] model_constraints_test.min_constraints (134 ms)
[ RUN      ] model_constraints_test.sharp_turn
[       OK ] model_constraints_test.sharp_turn (315 ms)
[ RUN      ] model_constraints_test.smooth_arc
[       OK ] model_constraints_test.smooth_arc (296 ms)
[----------] 3 tests from model_constraints_test (746 ms total)

[----------] 4 tests from plan_plath_test
[ RUN      ] plan_plath_test.nonzero_start_end
[       OK ] plan_plath_test.nonzero_start_end (0 ms)
[ RUN      ] plan_plath_test.zero_start
[       OK ] plan_plath_test.zero_start (1 ms)
[ RUN      ] plan_plath_test.zero_end
[       OK ] plan_plath_test.zero_end (1 ms)
[ RUN      ] plan_plath_test.zero_start_end
[       OK ] plan_plath_test.zero_start_end (1 ms)
[----------] 4 tests from plan_plath_test (4 ms total)

[----------] 2 tests from plan_path_test
[ RUN      ] plan_path_test.three_points_nonzero
[       OK ] plan_path_test.three_points_nonzero (45 ms)
[ RUN      ] plan_path_test.three_points_zero
[       OK ] plan_path_test.three_points_zero (2 ms)
[----------] 2 tests from plan_path_test (47 ms total)

[----------] Global test environment tear-down
[==========] 19 tests from 5 test suites ran. (814 ms total)
[  PASSED  ] 19 tests.```

</details>

[baylessj]

Thank you for catching this! I'm not entirely sure why this is happening off the top of my head, but my hunch is that something is a bit off with the math in Squiggles and this is the result. I'm quite busy these days and it'll likely be a little bit until I can get a fix merged up into Okapilib, but I'll definitely take a look to try and get this fixed!

[baylessj]

Hi @bnhrkn , sorry it's taken me so long to get back to you on this! A couple of findings on this:

• best I can tell, the root of the issue lies in the gradient descent algorithm used to find the ideal start/end velocity for a given path. You can avoid this algorithm by manually setting the start/end velocity of your path to a non-zero value, as a workaround.
• It's tricky to find sane defaults for those velocities in the gradient descent algorithm that will result in nice looking paths for more typical paths (like those included in the tests and visualization example currently) while also supporting tiny paths like this. I'm working on a solution to that in my baylessj/52-small-paths branch on this repo 😄