## 2 Methods
### Path Connection
#### 2.3.2 Avoiding Singularities Cost  
The original FMT* algorithm only considers the Euclidean distance as the cost for path expansion, resulting in the generation of the shortest path. However, for trajectory planning of robotic arms in Cartesian space, avoiding singularities is an important consideration. Therefore, the singularity index can be incorporated into the cost function of the FMT* algorithm. Defining the joint space \(Q\) and the operational space \(X\) (in Cartesian space), the following relationship holds:  
\[\text{f}:Q\to X \tag{5}\]  
By using the forward kinematics matrix \(f\), the joint space \(Q\) of the robotic arm can be mapped to the operational space \(X\). \[J(q)=\frac{\partial f}{\partial q} \tag{6}\]  
Figure 5: Comparison of search trees and optimal paths with different connection radii, and the red path is the optimal path found. The Jacobian matrix \(J(q)\) is the partial derivative of the forward kinematics matrix \(f\) concerning the joint angle vector \(q\), determined by the configuration of the robot's joints. The singularity of the robotic arm can be determined by the determinant of the Jacobian matrix. If the determinant of the Jacobian matrix is zero, i.e., \(J(q)=0\), it indicates that the robotic arm is in a singular configuration. According to the theory of singular value decomposition of matrices, the Jacobian matrix of any joint configuration can be decomposed into singular values. \[J(q)=U\sum V \tag{7}\]  
where  
\[\Sigma=\left[ \begin{array}{ccccc}\sigma_{1}&0&\ldots&0&0\\ 0&\sigma_{2}&\ldots&0&0\\ \vdots&\vdots&&\vdots&\vdots\\ 0&0&\ldots&\sigma_{m}&0\end{array} \right] \tag{8}\]  
\(U\in\Re^{m\times m}\) and \(V\in\Re^{n\times m}\) are both orthogonal matrices, \(\Sigma\) is a diagonal matrix with singular values of the Jacobian matrix \(J(q)\) as its elements, and \(\sigma_{1}\geq\sigma_{2}\geq\ldots\geq\sigma_{m}\geq 0\). While the Jacobian determinant can qualitatively determine singularity, many singularity indices use the singular values of the Jacobian matrix to quantitatively measure the distance between the robotic arm and the singular configuration. The condition number \(K(J)\) is defined as the ratio between the maximum singular value \(\sigma_{1}\) and the minimum singular value \(\sigma_{m}\) of the Jacobian matrix \(J(q)\). The dexterity index \(D(J)\) is derived from the condition number index and is related to the minimum condition number of the intermediate joint angles \(q_{i}(i=2,\,3,\,4,\,5)\). The manipulability index \(W(J)\) is the determinant of the product of the Jacobian matrix and its transpose. When the manipulability index \(W(J)=0\), the robotic arm is in a singular region, and the larger the value of \(W(J)\), the further the distance from the singularity region. The aforementioned singularity indices are all related to the manipulability ellipsoid. The manipulability ellipsoid can be described by the singular value matrix, and the principal axes of the manipulability ellipsoid are determined by the singular values \(\sigma_{i}\) of the Jacobian matrix. Figure 6 shows the manipulability ellipsoid of a two-link robotic arm under different joint configurations. From Figure 6, it can be observed that as the two-link robotic arm moves closer to the singularity point, the area of the manipulability ellipsoid becomes smaller and flatter in shape. Conversely, when the arm moves further away from the singularity point, the area of the manipulability ellipsoid becomes larger and approaches a more spherical shape. The shape of the manipulability ellipsoid is related to the singular values of the Jacobian matrix; thus, the singularity of the robotic arm can also be deter  
Figure 6: Manipulability ellipsoids under different joint configurations. the manipulability ellipsoid. Recently, geometrically aware singularity indices have been proposed [22; 23].