Initial conditions with brake phase violated for step1

[Fubini2]

Dear Pantor,

following issue. In our tests of ruckig we often see conditions where the final state of the initial brake phase does not satisfy the conditions on velocity. Here is a little simplified example for this behavior (used version for the results was commit-id 13c5e7f) :

` input.current_position = { 18879.4705012938102 };
input.current_velocity = { 110.229330653798357 };
input.current_acceleration = { 470.272491202182493 };

input.target_position = { -18879.4705012938102 };
input.target_velocity = { 0.035 };
input.target_acceleration = { 0.0 };

input.max_velocity = { 0.035 };
input.max_acceleration = { 1014.76263156153846 };
input.max_jerk = { 20000.0 };

`

If I debug into this I can see that the initial state of the remaining OTG fot step 1 is

	Name	Wert	Typ
▶	p.brake	{duration=0.13762231604309033 t={ size=2 } j={ size=2 } ...}	ruckig::BrakeProfile
	p.p[0]	18891.253748155214	double
	p.v[0]	25.708579960392669	double
	p.a[0]	-1014.7626315610985	double
	inp.max_velocity[dof]	0.035000000000000003	double
	inp.max_acceleration[dof]	1014.7626315615385	double
	inp.max_jerk[dof]	20000.000000000000	double

Clearly the initial velocity violates the maximal velocity. As far as I understand your theory the reason for the initial brake phase was to get a safe kinematic state. Hence my questions:

1. What maybe negative effects can we expect when the initial conditions for step 1 are violated?
2. How important is it for the Step1 and later on Step 2 conditions satisfy all prerequisites on velocity and acceleration?
3. How trustworthy is the OTG solution if the initial conditions are not met? Is it still time optimal for example.
4. How exact should the brake phase hit the bounds on velocity/acceleration? In the above example it missed the velocity by far.

I have to admit I mainly tested this on my fork including the prototype for the final acceleration phase where we had issues with accuracy in

bool check(double jf, double vMax, double vMin, double aMax, double aMin)

This method deliberately misses out on testing the initial state and only checks whether the target state and some intermediate states fulfill the conditions on accuracy and velocity resp. acceleration. Here we tested with special test cases where final acceleration phase must be symmetric to the initial brake phase by simply setting:

for (int idx = 0; idx < DOFs; ++idx) { input.current_position[idx] = -input.target_position[idx]; input.current_velocity[idx] = input.target_velocity[idx]; input.current_acceleration[idx] = -input.target_acceleration[idx]; }
In these cases we got a lot of errors where our adapted ruckig was not able to find valid solutions because some conditions inside the check-method failed and hence either ErrorExecutionTimeCalculation or ErrorSynchronizationCalculation was raised . Therefore for tests I integrated backward from the desired final state inside the check method and additionally tested this results on the side of the initial brake phase which brought us to the above conclusion that the OTG solution after the end of the initial brake phase is not yet satisfying the conditions on velocity as seen above and if tested would also raise the same errors.

Btw. I have seen you started adding first commits with the final acceleration phase. Can we expect this feature to be officially supported with the next release 0.6.0?

I am sorry this got quite lengthy.

As always keep up your great work!

Thanks
Fubini

[Fubini2]

Maybe another slight addition to the previous questions:

What are the exact cases you need to solve for the brake phase? In your paper you write

"It can be seen by distinction of cases that a resulting profile includes up to two time-steps tib0 and tib1 with corresponding jerk jib0 ∈ {−jmax, jmax} and jib1 = 0."

Looking at the code and your remark on #75 it seems that phase 2 in some cases also assumes jib1 ∈ {−jmax, jmax}.

Thanks
Fubini

[Fubini2]

Yesterday I dug a little deeper into the issues of the initial velocity after the brake phase not being the maximal or negative velocity bound. So I plotted the brake phase (unfortunately GitHub refuses to let me upload the plot).

Doing this I found out the problem seems to be that in

void BrakeProfile::velocity_brake(double v0, double a0, double vMax, double vMin, double, double aMin, double jMax)

t_to_v_min_with_constant is smaller than t_to_v_max_with_constant even though as the plot showed its the other way round in reality. Hence I looked at the formula for t_to_v_min_with_constant

const double t_to_v_min_with_constant = aMin / (2 * jMax) - (v_at_a_min - vMin) / aMin;

and I think it should be

const double t_to_v_min_with_constant = - aMin / (2 * jMax) - (v_at_a_min - vMin) / aMin;

(the sign of the first term is wrong). Can you confirm this?

Unfortunately this did not solve my problem completely. Now I enter the step 1 calculations with supposedly correct initial maximal velocity but know step 1 does no longer find a solution at all (ErrorExecutionTimeCalculation).

Fubini

[pantor]

Hi Fubini,

unfortunately I can't answer your questions in all details currently (but I'll do that in the next few days), but I really appreciate that you're digging into this!

You're right that the velocity might still be above its limit at the end of the braking trajectory. This is not really communicated in the paper, so I see where this confusion comes from. In general, a braking trajectory requires three jerk steps. However, if the last steps becomes necessary (due to a velocity violation), the main profile requires less steps (as it will directly start at the velocity limit and no acceleration step is necessary). On top, the mathematical equations for the main profiles include this case very easily. So for both reasons (1. for performance, and 2. for code simplicity), I've decided to "merge" the last step of the braking profile into the main profile. So to be exact, the braking profiles are not there to brake the kinematic state fully below its limits, but only to make the main profile calculation work for alle cases...

You can see in the brake calculation that the second step (j[1] = ...) is always zero. The step 2 I've referred to in #75 would actually be the first step of the main profile. This is more of an "implementational detail" of Ruckig, so this shouldn't have any effects on time optimality, etc.

[pantor]

Just some more quick notes:

• I think const double t_to_v_min_with_constant = aMin / (2 * jMax) - (v_at_a_min - vMin) / aMin; should be fine. I'll have a more in-depth look at this later on, but a negative first term results in failing tests.
• There are probably some numerical issues that make the final acceleration profile a bit harder than just copying the inverse brake trajectory.
• I'm working on this feature, but I can't promise any release date for the open source version.

[Fubini2]

Maybe just a few hints, where we have seen problems with stability as well:

void PositionStep1::time_all_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax) {: ... // ACC1_VEL const double t_acc0 = std::sqrt(a0_a0/(2*jMax_jMax) + (vMax - v0)/jMax); .., // ACC0_VEL const double t_acc1 = std::sqrt(af_af/(2*jMax_jMax) + (vMax - vf)/jMax);
sometimes the values inside the sqrt get sligthly negative (about -1e-13).

Also we have seen jumps at the end of the optimal trajectory caused by the difference between integrating and returning the exact theoretical final position inside

void at_time(double time, Vector<double>& new_position, Vector<double>& new_velocity, Vector<double>& new_acceleration, size_t& new_section) const {
...
// Non-time synchronization if (t_diff_dof >= p.t_sum[6]) { // Keep constant acceleration std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = Profile::integrate(t_diff_dof - p.t_sum[6], p.pf, p.vf, p.af, 0); continue; }
If the trajectory is evaluated exactly at its time duration one always gets the exact theoretical final position. Evaluating an epsilon left of the time duration you get the integrated value with an positional error up to 1e-8. This jump can in our use cases lead to unwanted spikes in the higher order derivatives. We have also seen similar problems inside my final acc phase prototype since it integrates the acc phase backwards not hitting exactly the forward integrated rest of the trajectory. Hence in my opinion one should always forward integrate and start the acc phase integration from the forward integrated final position and not the theoretical exact final position.

Fubini

[Fubini2]

Regarding

const double t_to_v_min_with_constant = aMin / (2 * jMax) - (v_at_a_min - vMin) / aMin

I think the formula is correct under the assumption you do not want to reduce the velocity to vmax as you have written before. The phase exactly hitting vmax would require to use negative amin leading to a positive sign for both terms and essentially adding them up.

[pantor]

The jumps at the end of the trajectory should now be fixed - Ruckig now uses the integrated values instead of the target values.

[pantor]

I'll close this for now in favor of #72, or are there any more questions?