# Position Selection and Collision-Free Path Planning for Fruit Picking Robots
## 2 Position and Orientation Selection
### Inverse Reachability Map  
While the reachability map shows the reachable area of the end-effector with a fixed robot base, the inverse reachability map demonstrates where to position the robot base so that the end-effector can reach a specific target point. The target fruits focused in this paper are Kanpei, a citrus cultivar, which are cultivated by suspending them separately on support structures during growth to prevent them from falling due to their weight or branches breaking, as shown in Figure 3. Since their pedicels grow upward, the roll and pitch angles of the end-effector of the harvesting robot are assumed to be zero. In addition, because the robot base cannot be lifted up and down, it is assumed that its vertical coordinate is also zero. Similar to generating the reachability map, the inverse reachability map is generated by voxelizing the space and varying the yaw angle of the end-effector position while solving the inverse kinematics. If the obtained joint angles satisfy the joint limits, the forward kinematics is solved using the obtained inverse kinematics solution to calculate the positions of the joints and check for collisions between the manipulator and obstacles. If there are no collisions, the manipulability score is calculated and stored. The same procedure is repeated for multiple targets, voxelizing the space around the centroid of the location of each target in a similar manner. Directly summing the individual maps could lead to issues where certain positions might have the highest score, despite there being no feasible solution for some fruit. To mitigate this, we assign negative values to positions where no solution exists and utilize the summed inverse reachability scores. If there is no common solution, different positions were selected for fruits. A pseudo-code for computing an inverse reachability map is shown in Algorithm 2. Once positions are  
Figure 3: Kanpei cultivation environment. determined, the end-effector orientation is selected with the highest single reachability score by varying the approach angle to the target location. ```
1: Set target \(\mathbf{T}(x_{t},y_{t},z_{t})\), voxel size and voxelization
2: Initialize Inverse Reachability Map (IRM)
3:for each voxel \(V_{i}\) position \(\mathbf{P}_{b,i}(x_{i},y_{i},0)\)do
4: Initialize inverse reachability score: \(IRMs\gets 0\)
5:for each orientation \(\mathbf{R}_{e,j}(0,0,\theta_{zj})\)do
6: Calculate inverse kinematics: \(\mathbf{\theta}\gets\mathbf{I}.\mathbf{K}(\mathbf{T}-\mathbf{P}_{b,i},\mathbf{R}_{e,j})\)
7:if\(\mathbf{\theta}\) exists then
8:if\(\mathbf{\theta}\) is within joint limits then
9: Calculate forward kinematics: \(\mathbf{F}.\mathbf{K}(\mathbf{\theta})\)
10:if not Link points collide with obstacle then
11: Calculate manipulability index: \(\mu\)
12: Update score: \(IRMs\gets IRMs+\mu\)
13:endif
14:\(IRMs\leftarrow-1\)
15:endif
16:\(IRMs\leftarrow-1\)
17:endif
18:\(IRMs\leftarrow-1\)
19:endfor
20: Save \((V_{i},IRMs)\)
21:endfor
22: Normalize entire inverse reachability scores
```  
**Algorithm 2** Inverse Reachability Map Construction  
Figure 4 depicts an example of an inverse reachability map for a random target. When there are no obstacles for the same target fruit, the map appears symmetrically, as shown in Figure 4a. However, when obstacles exist, the IRM appears restricted, as shown in Figure 4b. The target fruit is depicted as a green circle, and the position with the highest score is marked with a black circle, where red and blue respectively represent the maximum and the minimum values of the inverse reachability score, with yellow in the middle. Examples of the map for 15 obstacles and five target fruits are presented in Figure 5. The space was voxelized using a grid size of 50 mm, with the centroid of the targets as its center. Voxelized space spans \(1.4(\text{W})\times 1.4(\text{D})\) in meters. The orientation for approaching a target fruit of the end-effector ranges from \(-\pi\) to \(\pi\), with a grid size of \(\pi/8\) radians.