# An Efficient Stochastic Constrained Path Planner for Redundant Manipulators
## 5 Results
### Simulation Study  
Three different case studies have been carried out in simulation with the same 4 DoF planar manipulator. The workspace has been made to be increasingly challenging to test the effectiveness of the scheme under controlled but demanding conditions:  
* Case study I: In order to complete the task, the robot must follow a trajectory perpendicular to the given surface shown in Figure 1a. During the trajectory the robot must not collide with the surface or with itself. This restriction imposes that any point belonging to links 1, 2 and 3 must be at a distance higher than \(\delta=0.2\) m to the surface. In addition, any joint in the robot must be at a distance higher than \(\delta=0.2\) m from any other joint center;
* Case study II: In this simulation, the same restrictions considered in case study I apply. However, the surface path to be tracked is made more demanding, as can be seen in Figure 1b. The changes in the normal to the surfaces are accomplished by a set of smooth changes;
* Case study III: As above, but a workspace with a near-full overhang around the robot makes tracking particularly challenging for collision checking during the surface following task, almost encroaching the robot fully by its surroundings and severely limiting its mobility, as depicted in Figure 1c. An extensive set of runs has been carried out in order to compare the outcome of our proposed approach. For each trajectory produced by the algorithm, the mean manipulability value is computed as \(\hat{\omega}=(1/N)\sum w_{i}\), considering that the trajectory has \(N\) different waypoints. In addition, the whole trajectory is computed to check for collisions with the environment and self-collisions. The success rate accounts for the number of generated trajectories that do not collide with any obstacle. Figure 2 presents the results in terms of manipulability at each time step along the single line environment (case studies I, II and III). For all graphs, the blue line represents the joint trajectory that possesses the higher mean manipulability that the algorithm can achieve. The trajectory with minimum mean manipulability is also shown in red, whilst green presents a trajectory that lies in the mean value of all the manipulabilities obtained. We compare the results of the proposed Stochastic Constrained Optimization (SCO) method on the right (Figure 2b,d,f) with the output of Algorithm 2 on the left (Figure 2a,c), representing the path solution recovered from an inverse kinematic solution based on the Moore-Penrose pseudo inverse. The initial position \(\mathbf{q}_{r}\) used for this algorithm is selected randomly. It is clearly apparent how SCO is able to consistently produce trajectories that attain a higher degree of manipulability along the trajectory, while, at the same time, accomplishing the task at hand. Please, note also the low dispersion achieved in the manipulabilities represented in Figure 2b,d,f, indicative of the dependable ability of SCO to produce high manipulability regardless. Table 1 presents the results of case studies I, II and III. The first row of each simulation, named \"Simple Planner\", represents the results obtained using Algorithm 2 when applied to each of the workspaces represented in Figure 1. The following rows, represent the results of the SCO algorithm with varying number of samples K. In order to evaluate the success rate, we check that the robot does not collide with obstacles or derives in self-collision during the resulting trajectory. As expected, the success rate of this algorithm is low during the three simulations. In addition, it also produces trajectories that exhibit low manipulability overall along the path. The first row of simulation IB uses the proposed SCO Algorithm 4 using a single hypothesis on the path (\(K=1\)). In this case, the planner is able to produce a significantly higher success rate (76%) compared to Algorithm 2 (simple planner). In addition, the trajectory is optimized, both avoiding obstacles and increasing the mean manipulability.