*5.2. Simulation Study*

Three different case studies have been carried out in simulation with the same 4DoF planar manipulator. The workspace has been made to be increasingly challenging to test the effectiveness of the scheme under controlled but demanding conditions:


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 *ω*ˆ = (1/*N*) ∑ *wi*, 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,e), representing the path solution recovered from an inverse kinematic solution based on the Moore–Penrose pseudo inverse. The initial position **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. As the number of hypotheses K increases, we observe a higher success rate, while, at the same time, increasing the manipulability along the whole trajectory. It is worth noting that, for *K* = 10 hypotheses, the proposed algorithm achieves a success rate of 100%. Further

increasing *K* = 20, 50 hypotheses maintains the same success rate and allows us to obtain an even higher manipulability index along the trajectories.

**Figure 2.** A comparison of the manipulability metric evolution for case studies I, II and III. Cases IA, IIA and IIIA were carried out over a total of *N* = 2000 sample waypoints along the desired task-space trajectory. The blue line represents the trajectory that presents the maximum value in terms of mean manipulability along the line that the algorithm is able to achieve, red is the minimum and green the mean value across all the runs. The corresponding simulations IB, IIB and IIIB were obtained by using SCO with K=20 samples. (**a**) Case study I A, (**b**) Case study I B, (**c**) Case study II A, (**d**) Case study II B, (**e**) Case study III A, (**f**) Case study III B.


**Table 1.** Results of case studies I, II and III.

Table 1 also presents the results of case study II. As before, the "Simple Planner" in the first row refers to the results of the path obtained with Algorithm 2, where the simulation samples the path at a total of *N* = 2000 and, for a successful run, collisions with itself and the environment for a given trajectory must be avoided. The success rate of this algorithm is, again, very low. Given the increased complexity of the task when compared to run IA, the success rate is even lower. The results with SCO are collected under case study IIB, and as below different cases are investigated with varying initial hypotheses along the path: *K* = {1, 5, 10, 20, 30}. It can be observed how the proposed SCO is successful in producing a 56% of feasible solutions for *K* = 1 and 80% for *K* = 5, a marked improvement over the "Simple Planner" even with these low number of hypotheses. Moreover, as the number of hypotheses *K* increases, so does the success rate, while, at the same time, increasing the manipulability along the length of the trajectory. It is worth noting that, for *K* = 10, the proposed algorithm is able to achieve full success (rate of 100%). Further increasing *K* to 20 or 50 maintains flawless success and allows us to obtain even marginally higher manipulability along the trajectories. Table 1 also collects the results of case study III. As before, "Simple Planner" in the first row refers to the results of the path obtained with Algorithm 2. The success rate of this algorithm is, again, very low. The task in case study III is more demanding, which explains the low success rate, compared to case studies IA and IIA. Again, the results with SCO are collected under simulation IIIB with *K* = {1, 5, 10, 20, 30}. Following the same trends observed before, the proposed SCO produces a 33% of successful solutions for *K* = 1 and 80% for *K* = 5. The proposed algorithm also succeeds in this more demanding scenario, and as the number of hypotheses *K* increases, the success rate and the manipulability are increased. It is worth noting that for *K* = 20 the proposed algorithm is able to achieve full success (rate of 100%). As observed earlier, further increasing *K* beyond this point (to the maximum of 50 hypotheses in this case) only slightly increases the optimized manipulability while maintaining the same success rate.

Table 1 also presents computation times of each simulation carried out on an Intel™ Core i7 CPU @ 2.90GHz × 16 running Ubuntu 20.04 and Matlab™ R2018a. Mean computation times (seconds) and 2*σ* intervals are collected. As expected, a linear trend is observed in the computational effort.

#### *5.3. Real Experiments*

An experiment has been conducted on a physical manipulator to verify the performance of the proposed method under realistic settings. Figure 3 presents the real experimental setup with the 7R Rethink Robotics Sawyer cobot. The experiment consists of tracking a reference linear path on a flat surface whist keeping the tool in an orientation perpendicular to the surface throughout the motion. A force controller developed to maintain contact with the surface was implemented [18] to aid with the task at hand of simulating a surface conditioning assignment such as polishing, whilst the pose adopted along the path is set by the proposed SCO path planning strategy. The experimental setup is depicted in Figure 3a, consisting of a 7R Rethink Robotics Sawyer cobot, a force sensor Nano25 SI-25-25 attached to the robot end-effector, a small polishing tool proxy attached to the sensor (a cylinder of 43 × 43 × 10 mm), and a target flat surface to polish.

As per the test methodology adopted in the simulation cases to show the capabilities of the proposed method, the routine undertaken includes a comparison with the standard "Simple Planner" derived from the the Moore–Penrose solution (Algorithm 2). The initial conditions are set to be the same in both cases: an obstacle-free initial pose with average manipulability established prior. This was the initial condition fed to both trajectory planners. A video is supplied (https://arvc.umh.es/arte/AppliedSciences21.mp4, accessed on 11 November 2021) to visualise the full experimental setting and resulting motion, with several stills depicting the starting, mid- and end-points of the test trajectories also being collected in Figure 3. It can be observed how the optimality in manipulability being sought out by the SCO proposed planners derives configurations that keep the elbow link down, whereas in the "Simple Planner" case that is not the case, ultimately even compromising stability by traversing near-singular regions. The reader is referred to the video linked in the manuscript where the undesirable dynamic disturbances induced in the controller are clearly apparent with vibrations at compromised locations along the path, most notably at the mid-point (Figure 3e).

The final manipulability attained in both instances, shown in Figure 4, corroborates the ability of the algorithm to seek areas with higher manipulability, hence permitting operations with manipulator configurations away from singularity regions, ultimately leading to superior end-effector precision, less energy expenditure and overall safer trajectories whilst seeking to abide by the desired end effector path during execution.

(**a**) (**b**)

(**c**) (**d**)

**Figure 3.** Sawyer polishing task experiment: (**a**) setup: 7R manipulator with a force sensor attached to the robot end-effector, a tool consisting of a cylinder of 43 × 43 × 10 mm attached to the sensor and a flat surface target, (**b**) initial pose with minimal manipulability for both tests, (**c**,**d**) proposed SCO algorithm, (**e**,**f**) "Simple Planner" case.

**Figure 4.** Real polishing experiment with the Sawyer robot: (**a**) Manipulability comparison: attained manipulability evolution of the traversed path illustrated in Figure 3, with the proposed SCO scheme shown in red (top line), and the "Simple Case" Moore–Penrose solution depicted in blue (bottom line). Corresponding (**b**) "Simple Planner" trajectory evolution, and (**c**) Evolution of joint trajectories using the SCO algorithm.

#### **6. Conclusions**

An efficient stochastic algorithm able to produce obstacle-free configuration trajectories for a given workspace has been proposed. The random process exploits the particular kinematics of closed-chain mechanisms with redundant actuation to increase manipulability along a desired end-effector task-space motion in an iterative process. The stochastic solution remains close to optimal whilst affording computational tractability, being an attractive proposition for implementation on real robots. Results from tests in challenging simulation scenarios, as well as with a 7R manipulator constrained to undertake surface treatment tasks, have been presented to show the suitability of the proposed Stochastic Constrained Optimization (SCO) trajectory planner for redundant manipulators to be able to track arbitrary task-space paths. The challenging aspect of planning trajectories, where the robot must remain in close contact with non-smooth irregular surfaces whilst optimizing the manipulability index at each time step, represents an on-going research effort continuing with the line of work presented in this manuscript.

**Author Contributions:** Conceptualization, A.G.A. and J.V.M.; methodology, A.G.A. and J.V.M.; software, A.G.A.; validation, A.G.A. and J.V.M.; formal analysis, A.G.A.; investigation, A.G.A.; resources, J.V.M.; writing—original draft preparation, A.G.A. and J.V.M.; writing—review and editing, A.G.A. and J.V.M.; visualization, A.G.A. and J.V.M.; supervision, J.V.M. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research received no external funding.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Conflicts of Interest:** The authors declare no conflict of interest.
