# Sampling-Based Motion Planning for Free-Floating Space Robot without Inverse Kinematics
## 3 Motion Planning Algorithm for FFSRs
**Base coordination mechanism**: The base coordination mechanism corresponds to Lines 4 and 7-11 of the pseudo-code. The reason for designing this mechanism is as follows: First, the joint space has 7 DOF, while the EE pose task has 6 DOF and the base attitude has 3 DOF. Secondly, the main task of the planner is to plan a trajectory for the EE to reach the goal pose, and the base attitude is not required to reach a certain value accurately. Actually, the base attitude just needs to be coordinated to keep its disturbance within a certain range. Moreover, adjusting the base attitude within the null space of the generalized Jacobian matrix is prone to singularity. It should be avoided as much as possible. Therefore, this paper introduces a reference goal base attitude \(\mathbf{\Psi}_{b}^{r}\) and a threshold \(\Delta\mathbf{\Psi}_{b}^{r}\) to guide the adjustment of the base attitude. \(\mathbf{\Psi}_{b}^{r}\) is designed according to the base attitude disturbance limit interval \(\left[ \begin{array}{cc}\mathbf{\Psi}_{b-\min}&\mathbf{\Psi}_{b-\max}\end{array} \right]\). It is usually chosen at the center of this interval. \(\Delta\mathbf{\Psi}_{b}^{r}\) is less than half of the length of \(\left[ \begin{array}{cc}\mathbf{\Psi}_{b-\min}&\mathbf{\Psi}_{b-\max}\end{array} \right]\). \(\Delta\mathbf{\Psi}_{b}\) denotes the error between the base attitude corresponding to \(\mathbf{q}_{Extended}\) and the reference target base attitude. In each iteration of local planning, if \(\Delta\mathbf{\Psi}_{b}\) is less than \(\Delta\mathbf{\Psi}_{b}^{r}\), the base attitude will not be adjusted. If \(\Delta\mathbf{\Psi}_{b}\) is greater than \(\Delta\mathbf{\Psi}_{b}^{r}\), the base attitude is adjusted. In this way, this base attitude coordination mechanism avoids the occurrence of singularities in each iteration of local planning as much as possible. Meanwhile, the base attitude disturbance can meet the requirements. ```
1q_Near = Nearest_Node(Tree, X_Goal);
2q_Present = q_Near; Last_Checked_Config = q_Present
3 Ball_End = false
4 While (Shall_End = false) J_base = Jacobian_Base(q_Present); General_J = General_Jacobian(q_Present) Delta_X = X_Goal - f_EE(q_Present); Delta_q_Base = q_Goal_Base - q_Present_Base
5 Dot_Joint_For_EE = pinv(General_J) * Delta_x
6 If Max(Delta_q_Base) > Base_Adjust_Tresh_Hold
7 Dot_Joint_For_Base_Adjust = pinv(J_base * Null(General_J)) * Delta_q_Base
8 Else
9 Dot_Joint_For_Base_Adjust = 0
10 Endif
11 Dot_Joint = Dot_Joint_For_EE + Null(General_J) * dot_joint_For_Base_Adjust
12 Dot_Base = J_base * dot_joint
13q_Present = q_Present + [Dot_Base; Dot_Joint] * Time_Step
14 If Max(q_Present - Last_Checked_Config) > Collision_Check_Tresh_Hold
15 Is_Collision = Collision_Check(q_Present)
16 last_Checked_Config = q_Present
17 Endif
18 Shall_End = Is_Collision or Is_Q_Present_Over_Tresh_Hold
19 or Is_Reaching_The_Goal or Is_Over_Iteration
20 Endwhile
21 Is_Extend = ifq_Present is far enough from q_Near
22 Return
```

**Algorithm 3** Extend_Toward_Goal

### Collision detection mechanism

The collision detection mechanism is the same as that in Section 3.2.1D. **Termination conditions and return for iterative local planning**: (1) The collision detection module detects a collision; (2) the configuration exceeds the limit, that is, the base attitude disturbance exceeds the limit or the joint angle exceeds its range; (3) the target is reached; (4) the number of iterations reaches the upper limit. When any one of these four conditions is met, iterative local planning is terminated. Then, in Line 17 of the pseudo-code, the algorithm tests if the tree is extended far enough from the initial \(\boldsymbol{q}_{Extended}\). If the generated new configuration is very close to \(\boldsymbol{q}_{Extended}\), then we let Is_Extend equal to false, and the local planning results will not be added to the tree. ## 4 Simulation

The selected object for simulation is an FFSR with a base satellite and a 7-joint manipulator.