## 3 After Failure Strategy Approach
### Motion Planning  
The last step of the recovery approach is the motion planning along \(d\). A polynomial motion law is adopted for its simple and fast application; however, it cannot constrain the acceleration along all the path, but only on the end-points. The adopted procedure starts from the definition of a fifth-degree polynomial motion law, constraining the end-values of the end-effector position, speed and acceleration  
\[ \begin{cases}x(0)=x_{0}\\ v(0)=v_{0}\\ a(0)=a_{0}\\ x(T)=0\\ v(T)=0\\ a(T)=0\end{cases}  \tag{23}\]  
where \(T\) is the total motion time, \(x_{0}\) and \(v_{0}\) are the position and the velocity, respectively, after the Connecting Path motion, and \(a_{0}\) the chosen starting acceleration. The value of \(a_{0}\), and therefore of \(T\), is firstly set as equal to maximum possible acceleration at half of the distance, considering a motion law composed by a positive constant acceleration section, followed by a negative constant one. The obtained motion law is tested to verify that it respects the acceleration constraints. Three scenarios can be identified:  
* The acceleration limits are respected and the trajectory is feasible and does not require any changes;
* The trajectory must be modified to respect the acceleration limits, but the end-effector stops at the SP;
* The trajectory must be modified to respect the acceleration limits and the end-effector goes beyond the SP and has to return. Figure 3: Maximum and minimum acceleration values along \(d\). Figure 4 represents these scenarios: in red, a feasible trajectory, in blue, a trajectory that stops at the SP, and in green, a trajectory beyond the SP. As anticipated before, these three trajectories have different starting points. If the resulting trajectory is feasible, i.e., the acceleration limits are respected, the motion law can be applied without any changes. However, since the other two cases do not respect the acceleration limits, the motion law should be adapted depending on the scenario. * _Trajectory stopping at the SP_: As the acceleration exceeds the limits, the value of acceleration adopted is set equal to the minimum limit in that position, instead of the one obtained from the polynomial motion law. While keeping the acceleration equal to the minimum value, it is possible to reach a point \(P\) along \(d\) that allows the end-effector to reach and stop at the SP if the acceleration is set as constant and equal to the minimum acceleration limit in that point. \(P\) can be identified since the position, speed and acceleration values are known. However, keeping a constant acceleration from \(P\) to SP means an abrupt change in the acceleration in \(P\), which is not feasible. Therefore, a new polynomial law is defined starting from \(P\) to the SP, constraining the initial and final values of the position, speed and acceleration; the required time for this section can be estimated as the time needed to stop with constant acceleration. Moreover, this last section is also feasible, since it follows a constant acceleration motion law, which, in turn, is feasible. * _Trajectory beyond the SP_: For greater distances from the SP, after setting the acceleration value equal to the limit, it is not possible to identify a point \(P\) that allows the end-effector to stop at the SP with a constant acceleration. It is preferable to decrease the acceleration value, i.e., setting it equal to the minimum limit for each position, until the end-effector stops. Then, similarly to the initial situation, it is possible to design a fifth-degree polynomial motion law that stops the end-effector in the SP. Due to its definition, this second motion law does not assure to keep the acceleration value lower than the maximum acceleration limit, thus it is possible to identify again the three cases presented above. However, since the end-effector is nearer to the SP after this first trajectory, it is less likely that the required acceleration does not respect the acceleration limits.