enhancement of ‘PositionThirdOrderStep1::time_acc0_two_step’

[Ydaos]

In function of ‘PositionThirdOrderStep1::time_acc0_two_step’, the situation of 'Three step - Removed aMax', maybe should add 'Solution 2'

const double h0 = 3 * (af_af - a0_a0 + 2 * jMax * (v0 + vf));
const double h2 = a0_p3 + 2 * af_p3 + 6 * jMax_jMax * pd + 6 * (af - a0) * jMax * vf - 3 * a0 * af_af;
const double h1 = std::sqrt(2 * (2 * h2 * h2 + h0 * (a0_p4 - 6 * a0_a0 * (af_af + 2 * jMax * vf) + 8 * a0 * (af_p3 + 3 * jMax_jMax * pd + 3 * af * jMax * vf) - 3 * (af_p4 + 4 * af_af * jMax * vf + 4 * jMax_jMax * (vf_vf - v0_v0)))));

// Solution1
profile->t[0] = (2 * h2 + h1) / (2 * jMax * h0);
profile->t[1] = -h1 / (jMax * h0);
profile->t[2] = (-4 * a0_p3 - 2 * af_p3 + 6 * a0_a0 * af + 12 * jMax_jMax * pd - 12 * (af - a0) * jMax * v0 + h1) / (2 * jMax * h0);
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;

if (profile->check<ControlSigns::UDDU, ReachedLimits::ACC0>(jMax, vMax, vMin, aMax, aMin)) {
  add_profile(profile);
  return;
}

// Solution2
profile->t[0] = (2 * h2 - h1) / (2 * jMax * h0);
profile->t[1] = h1 / (jMax * h0);
profile->t[2] = (-4 * a0_p3 - 2 * af_p3 + 6 * a0_a0 * af + 12 * jMax_jMax * pd - 12 * (af - a0) * jMax * v0 - h1) / (2 * jMax * h0);

if (profile->check<ControlSigns::UDDU, ReachedLimits::ACC0>(jMax, vMax, vMin, aMax, aMin)) {
  add_profile(profile);
  return;
}

(t2 > 0) depend on 'h0' and 'jMax'. But 'h0' no always larger than zero. So use 'jMax' sign to choose solution miss true result. Fro example,

jMax = 3;
a0 = af = v0 = 0;
vf = pd = 2.25;
// t = {0.5, 1, 0.5, 0, 0, 0, 0};