Retracement of position

[ba-robo]

Hello,

When using Ruckig, I encountered a problem: when the starting point speed and the ending speed are not reasonable (the speed is relatively large, but the displacement is relatively short), the planning result will be that the position retracts and the speed passes 0 twice. Although this cannot be called an algorithm bug, in practice the robot will behave strangely.

Could you provide an interface to identify this situation in advance, or throw an error? Thank you so much!

The parameters of retracement of position case are :

input.current_position = {-84.260261, -4.424676, 36.634818, 1.288789, 41.631032, 84.729626};
input.current_velocity = {-71.554086, 11.274635, -28.023171, -2.672747, 49.393527, 72.425634};
input.current_acceleration = {0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000};

input.target_position = {-92.184652, -3.174453, 33.529803, 0.992855, 47.101289, 92.750596};
input.target_velocity = {-46.743877, 7.365340, -18.306595, -1.746016, 32.267129, 47.313230};
input.target_acceleration = {0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000};

input.max_velocity = {75.000000, 60.000000, 60.000000, 95.000000, 70.000000, 110.000000};
input.max_acceleration = {100.000000, 100.000000, 100.000000, 100.000000, 100.000000, 200.000000};
input.max_jerk = {1000.000000, 1000.000000, 1000.000000, 1000.000000, 1000.000000, 2000.000000};

input.enabled = {true, true, true, true, true, true};

input.synchronization = Synchronization::Phase;

And the position and velocity are:

[cheng9911]

I've encountered this issue as well. I'd like to raise an error for this kind of situation to prevent online trajectory planning. However, at the moment, I'm not sure how to handle it properly. Should I modify the underlying ' Result update(const InputParameter<DOFs, CustomVector>& input, OutputParameter<DOFs, CustomVector>& output) ' function?