## 2 Methods
### Path Connection
#### 2.3.2 Avoiding Singularities Cost
For a trajectory in joint space, the geometrically aware singularity cost can be represented using Equation (9):  
\[C[q(t)]=\sum_{i=0}^{n}d\left(k,J(q_{i})J^{T}(q_{i})\right)^{2}=\sum_{i=0}^{n} \
orm{\log\left(k^{-\frac{1}{4}}J(q_{i})J^{T}(q_{i})\right)}_{F}^{2} \tag{9}\]  
\(C[q(t)]\) represents the singularity cost function of the trajectory \(q(t)\). \(q_{i}\) (\(i\) = 0, 1... _n_) represents each planning point on the trajectory. \(J(q_{i})\) represents the Jacobian matrix at the planning point \(q_{i}\). \(d(A,B)\) represents the Riemann distance between matrices \(A\) and \(B\). \(\
orm{\ldots}_{F}\) represents the Frobenius norm. \(k\) is a diagonal matrix with elements \(\sigma_{k}\) as given values, satisfying the following equation:  
\[k= \begin{bmatrix}{\sigma_{k}}^{2}&0&0\\ 0&\ddots&0\\ 0&0&{\sigma_{k}}^{2}\end{bmatrix} \text{ where }\sigma_{k}\geq\sigma_{\text{max}} \tag{10}\]  
Equation (9) uses the square of the manipulability index multiplied by a scaling factor and takes the square of the Frobenius norm as the cost. The essence of this equation is to use the Riemann distance between the matrix k and the matrix \(J(q_{i})J^{T}(q_{i})\) as the cost. The matrix \(k\) represents the ideal case where all singular values of the Jacobian matrix of the robotic arm are greater than the maximum value \(\sigma_{max}\). In this case, the manipulability ellipsoid is in its most ideal shape. On the other hand, the matrix \(J(q_{i})J^{T}(q_{i})\) represents the manipulability ellipsoid at the planning point \(q_{i}\). Therefore, the higher the Riemann distance between the manipulability ellipsoid at \(q_{i}\) and the ideal manipulability ellipsoid, the higher the singularity cost. For trajectory planning in Cartesian space, the trajectory can be transformed to joint space using inverse kinematics \(f^{-1}\):  
\[q(t)=f^{-1}(x(t)) \tag{11}\]  
Thus, the equation for the geometrically aware singularity cost in Cartesian space is given by Equation (12):  
\[C[x(t)]=\sum_{i=0}^{n}d\left(k,J\left(f^{-1}(x_{i})\right)J^{T}\left(f^{-1}(x_ {i})\right)\right)^{2}=\sum_{i=0}^{n}\
orm{\log\left(k^{-\frac{1}{4}}J\left(f^ {-1}(x_{i})\right)J^{T}\left(f^{-1}(x_{i})\right)\right)}_{F}^{2} \tag{12}\]  
where \(x(t)\) represents the end effector trajectory in Cartesian space, and \(x_{i}\) represents the \(i\)-th end effector pose on the trajectory. When \(|J|=0\), it is not possible to compute the inverse kinematics solution, so \(C[x(t)]\) is set to infinity. Finally, the singularity index mentioned above is incorporated into the cost function of FMT\({}^{*}\) path planning, forming a new cost function \(c(x)^{*}\) with singularity avoidance, defined as Equation (13):  
\[c(x)^{*}=\min\{c(y)+\text{Cost}(y,x)+sig\cos t(x)\} \tag{13}\]  
where  
\[sig\cos t(x)=\
orm{\log\left(k^{-\frac{1}{4}}J\left(f^{-1}(x_{x})\right)J^{T} \left(f^{-1}(x_{x})\right)\right)}_{F}^{2} \tag{14}\]  
\(x_{x}\) represents the end effector pose of the robotic arm at path point \(x\). Compared to the original cost function, which only used the Euclidean distance from the path point x to the root node as the cost, the improved cost function also added the singularity index of Riemannian geometric perception as the cost function, so it can avoid the singularity of the robotic arm.