# Kinesthetic Learning Based on Fast Marching Square Method for Manipulation
## 2 Fast Marching Applied to Path Planning  
The Fast Marching Square (FM\({}^{2}\)) method [30] is a path planning algorithm which is a variation of the original Fast Marching Method [31]. It is based on the idea of guiding the desired path by following light propagation. Mathematically, light propagation is defined by the Eikonal equation, which states that the speed of light is determined by the substance in which it is travelling [32]. This equation is solved by the solution of wave propagation, as stated in Equation (1):  
\[\phi=\phi_{0}e^{ik_{0}(\eta\pi-\epsilon_{0}t)} \tag{1}\]  
where \(\phi\) and \(\phi_{0}\) represent the wave value and initial wave value, respectively, \(\eta\) is the refraction index and \(c_{0}\) is the light speed. According to this definition, some properties of the proposed method can be derived:* The path that light follows is always the fastest feasible one, so the proposed planning method ensures the calculation of the path of least possible time. * Given the characteristics of Equation (1), if \(\eta(x)\) is \(\mathcal{C}^{\infty}\), then \(\phi\) is also \(\mathcal{C}^{\infty}\), and so are the calculated trajectories using the gradient method over this potential. This turns into smooth paths that avoid the need for extra refinement steps. * Since the method is based on wave propagation, if there is a feasible solution, it is always found, so it is complete. Following its original definition, fast marching solves the Eikonal equation applied on a rectangular orthogonal mesh, deriving into a \(O(n)\) algorithm where \(n\) is the total number of grid points. The discrete formulation of the Eikonal function can be seen in Equation (2):  
\[max\left(\frac{T-T_{1}}{\Delta x},0\right)^{2}+max\left(\frac{T-T_{2}}{\Delta y },0\right)^{2}=\frac{1}{F_{i,j}^{2}} \tag{2}\]  
where \(\Delta x\) and \(\Delta y\) are the grid spacing in the x and y directions, \(F_{i,j}\) is the wave propagation speed for the grid cell (\(i\),\(j\)) and \(T\) represents the arrival time of the Eikonal equation for each position. If this time \(T\) is represented for the different values \(T1\) and \(T2\), it is obtained that:  
\[T=T_{i,j}\] \[T_{1}=min(T_{i-1,j},T_{i+1,j}) \tag{3}\] \[T_{2}=min(T_{i,j-1},T_{i,j+1})\]  
where \(T\) sets the time in the current cell (\(i,j\)) and the values of \(T1\) and \(T2\) set the minimum values of the time in the neighboring cells previously visited to the current cell by the expansion of the Eikonal equation. Iterations of the propagation procedure on a grid can be visually observed in Figure 1.  
The final outcome is a matrix where each cell indicates the arrival time of the wave with respect to the propagation point. As a modification of FM, \(FM^{2}\) applies this procedure twice, deriving in safer and smoother paths. This path planning method (\(FM^{2}\)) can be applied to binary maps by propagating a wavefront that consider all obstacles as source points. The final matrix is considered as a velocity map \(\mathbf{F}\). To better show the differences between FM and \(FM^{2}\), the paths are calculated on a sample binary map and shown in Figures 2 and 3. Both figures show that the path obtained with the FM\({}^{2}\) method is smoother and reproducible by a robot. The velocity map values range from 0 to 1, representing the maximum speed allowed for the vehicle at each point of the map. Obstacles imply speeds equal to zero, whilst points in space far enough from obstacles will allow maximum speeds. When computing a path for the vehicles to follow, the FM\({}^{2}\) method will obtain the shortest path from the initial  
Figure 1: Fast Marching propagation on a grid. Colors from blue to red indicate cells solved in the same iteration [32]. (**a**) Iteration of FM with one wave in 2D and (**b**) time of arrival potential \(D(x)\) (third axis) represented by a color map. position to the goal position that lets the vehicles navigate at greater speeds. On the other hand, the original FM method will compute the shortest path, not taking into account the safety of the vehicle.