# Kinesthetic Learning Based on Fast Marching Square Method for Manipulation
## 3 Learning Manipulation Trajectories via Fast Marching Square
### Fast Marching Learning Algorithm  
Fast Marching Square, as mentioned previously, creates a velocity map \(\mathbf{F}\) in which a path is optimal when the propagation velocity is higher. Hence, by modifying these  
Figure 10: Path taught using different strategies. (**a**) low velocity; (**b**) medium velocity; and (**c**) high velocity.  
velocities, the final calculated path can be reshaped. This fact is very useful for the learning step. Given a set of experiences, the main objective of Fast Marching Learning is to encode this information into the velocity map. As such, the final path would be biased by learned experiences without losing the main properties of Fast Marching Square, such as avoiding local minima and being smooth.  
The proposed algorithm takes as inputs the point-to-point paths from the robot's end effector learned with kinesthetic teaching, defined with Equations (4) and (5). As an output, it is intended to replicate the shape of learned paths when a similar task is commanded. Given that the outcome is based on learned experience, the final path is expected to be efficient, smooth and faster.  
The first step is labeling all the points contained in \(E\) as 1 in an empty workspace, that is to say, filled with 0. The resulting workspace is denoted as \(\mathbf{F_{p}}\). Then, a dilation operation is applied on \(\mathbf{F_{p}}\). The dilation size is defined by the _area of influence (aoi)_, specified in voxels in the case of 3D environments. After the dilation process is applied, the workspace is divided into regions filled with 1, where previous knowledge is found, and other zones with 0, where no information is available. For the latter ones, the algorithm works as FM\({}^{2}\) normally does.  
When the workspace is ready, Fast Marching is applied in the same way as in the first step for FM\({}^{2}\), turning the workspace \(\mathbf{F_{p}}\) into a velocity map. \(\mathbf{F_{p}}\) is linearly rescaled to be within the established bounds \([sat,1]\), where \(sat\) stands for \(saturation\), which is in charge of weighting the importance of new data with respect to the rest of the environment. With this, the final velocity map \(\mathbf{F}\) is obtained. This leads into a generalization in \(\mathbf{F}\) of the provided demonstrations. Finally, Fast Marching is applied on the entire workspace considering a unique wave source defined by the centroid of all final trajectory points \(P_{i}\in E\) and the velocity map \(\mathbf{F}\). A detailed description of the algorithm coding is shown in Algorithm 1.  
```
0:\(Experience\)\(E=\langle P_{1},P_{2},\ldots,P_{K}\rangle\)
0:Modified velocity map \(\mathbf{F}\), reproduction field \(\mathbf{T}\)\(\mathbf{F_{p}}\leftarrow\{0\}\); for\(i=1\)to\(K\)do for\(j=1\)to\(N_{i}\)do \(x\gets P_{i,j}\); \(\mathbf{F_{p}}(x)\ :=1\); endfor endfor \(\mathbf{F_{p}}\leftarrow\) Dilate(\(\mathbf{F_{p}},aoi\)) \(x_{s}\leftarrow\{\forall x\in\mathbf{F_{p}}\mid\mathbf{F_{p}}(x)=0\}\); \(\mathbf{F_{p}}\leftarrow\) FastMarching(\(\mathbf{F_{p}},x_{s}\)) \(\mathbf{F}\leftarrow\)Rescale(\(\mathbf{F_{p}},sat,1\)); \(x_{s}\leftarrow\)Centroid(\(E\)); T \(\leftarrow\)FastMarching(\(\mathbf{F},x_{s}\)); return \(\mathbf{F}\), \(\mathbf{T}\)
```  
**Algorithm 1** Fast Marching Learning Algorithm.