# Sampling-Based Motion Planning for Free-Floating Space Robot without Inverse Kinematics
## 3 Motion Planning Algorithm for FFSRs
Then, the iterative local planning process starts and continues until the termination condition is reached. 1. **Iterative local planning**: This process corresponds to Lines 3 to 19 of the pseudo-code. For local planning corresponding to GEE-GG, the purpose is to extend the node \(q_{Extended}\) toward the goal EE pose as much as possible. Moreover, the base attitude disturbance is required to be within a certain limit. First, in each iteration, two kinds of errors are calculated. The first kind is \(\Delta x_{e}\), which denotes the error between the goal EE pose and the EE pose at \(q_{Extended}\). The second kind is \(\Delta\mathbf{\Psi}_{b}\), which denotes the error between the referred target base attitude \(\mathbf{\Psi}_{b}\)\({}^{r}\) and the base attitude \(\mathbf{\Psi}_{b}\) at \(q_{Extended}\). \(\mathbf{\Psi}_{b}\)\({}^{r}\) is not the target base attitude that the planner needs to achieve accurately. \(\mathbf{\Psi}_{b}\)\({}^{r}\) is only used as a reference for starting the base coordination mechanism. Secondly, \(J_{g}\) and \(\mathbf{H}_{\omega}\) at \(q_{Extended}\) are calculated. Based on the pseudo-inverse of \(J_{g}\), \(\Delta x_{e}\) is mapped to the action space to get joint angular velocity \(\dot{\mathbf{\Theta}}_{1}\). \[\dot{\mathbf{\Theta}}_{1}\ =\ \big{(}J_{g}\big{)}^{+}\Delta x_{e}\] (22) \(\Delta x_{e}\) can be reduced if the robot moves with \(\dot{\mathbf{\Theta}}_{1}\) from \(q_{Extended}\). If \(\Delta\mathbf{\Psi}_{b}\) reaches a certain threshold \(\Delta\mathbf{\Psi}_{b}\)\({}^{r}\), base attitude coordination is triggered. Then, the base attitude is adjusted using the pseudo-inverse of \(\mathbf{H}_{\omega}\) within the null space of \(\mathbf{J}_{g}\)\({}^{r}\) and an action \(\dot{\mathbf{\Theta}}_{2}\) that can reduce \(\Delta\mathbf{\Psi}_{b}\) is generated. \[\dot{\mathbf{\Theta}}_{2}\ =\ \big{(}\mathbf{H}_{\omega}\bullet Null\big{(}J_{g} \big{)}\big{)}^{+}\Delta\mathbf{\Psi}_{b}\] (23) \[Null\big{(}J_{g}\big{)}\ =\ \mathbf{E}_{n\times n}-J_{g}{}^{+}J_{g}\] If base attitude coordination is triggered, \(\dot{\mathbf{\Theta}}_{2}\) is combined with \(\dot{\mathbf{\Theta}}_{1}\) to form the final action \(\dot{\mathbf{\Theta}}\) for a single iteration. \[\dot{\mathbf{\Theta}}\ =\ \dot{\mathbf{\Theta}}_{1}+Null\big{(}J_{g}\big{)}\dot{\mathbf{ \Theta}}_{2}\] (24) Otherwise, the final action for a single iteration only includes \(\dot{\mathbf{\Theta}}_{1}\). Then, multiplying \(\dot{\mathbf{\Theta}}\) by \(\mathbf{H}_{\omega}\), \(\mathbf{\Psi}_{b}\) corresponding to \(\dot{\mathbf{\Theta}}\) can be obtained. Integrating \(\dot{\mathbf{\Theta}}\) and \(\dot{\mathbf{\Psi}}_{b}\) from \(q_{Extended}\), a new configuration is obtained, and we let this configuration be the next \(q_{Extended}\). Repeat the above process until the \begin{table}
\begin{tabular}{c c c} \hline \hline
1 & X\_Goal & Goal EE pose \\ \hline
2 & General\_J & Generalized Jacobian matrix \\ \hline
3 & General\_Jacobian() & A function that is used for calculating General\_J \\ \hline
4 & f\_EE() & A function that is used for calculating EE pose \\ \hline
5 & Delta\_X & The error between the goal EE pose and the EE pose at q\_Present \\ \hline
6 & q\_Goal\_Base & The reference goal base attitude \\ \hline
7 & Delta\_q\_Base & The error between q\_Goal\_Base and q\_Present\_Base \\ \hline
8 & q\_Present\_Base & The current base attitude in local planning \\ \hline
9 & Dot\_Joint\_For\_EE & Joint velocity that can reduce the EE pose error \\ \hline
10 & Base\_Adjust\_Tresh\_Hold & The base attitude error threshold that indicates the need for base adjust \\ \hline
11 & Dot\_Joint\_For\_Base\_Adjust & Joint velocity that can reduce the base attitude error \\ \hline
12 & Null & A function that can calculate the null-space of a matric \\ \hline \hline \end{tabular}
\end{table} 
Table 3: Variables and functions used in Algorithm 3.

termination condition of local planning is triggered, then exit local planning and return to the new configuration and trajectory. 2.