**Algorithm 2** Simple Planner (*G*, **q***r*)

**INPUT:** *G* = {**x**1, **x**2,... **x***N*}, set of positions and orientations **q***r*, an initial random seed for joint positions **OUTPUT:** {**q**1, **q**2,..., **q***N*}, the joints configuration path 1: **function** SIMPLE\_PLANNER(G) 2: **q** = **q***<sup>r</sup>* 3: **for** *i* = 1, 2, . . . , *N* **do** 4: **x***<sup>i</sup>* = *G*{*i*} 5: **q***<sup>i</sup>* = Inverse\_Kinematics(**x***i*, **q**) 6: **q** = **q***<sup>i</sup>* 7: **end for return** *Q* = {**q**1, **q**2,..., **q***N*} (the joint path) 8: **end function**

**Algorithm 3** Inverse Kinematics(**x***i*, **q***r*)

**INPUT: x***i*, the position and orientation of the end effector **q***r*, the initial seed of the algorithm **OUTPUT: q**, joint positions 1: **function** INVERSE\_KINEMATICS(**x***i*, **q***r*) 2: **qi** = **qr** 3: **while x** = **x***<sup>i</sup>* **do** 4: **x**˙ = *Compute*\_*VW*(**x***i*, **x**) 5: *Ji* = Jacobian(**q***i*) 6: **q**˙ *<sup>i</sup>* = *J*† *i* **x**˙ 7: **q** = **q** + **q**˙ *<sup>i</sup>* ∗ Δ*t* 8: **x** = *f*(**q**) 9: **end while** 10: **return q** 11: **end function** 12: **function** COMPUTE\_VW(**x***i*, **x**) 13: //Computes linear and angular speed 14: //to reduce the error in **x** − **x***<sup>i</sup>* 15: **x**˙ = (**x** − **x***i*)/Δ*t* **return x**˙ 16: **end function**

A more general solution to (3) can be written as:

$$\dot{\mathbf{q}} = J^{\dagger}\dot{\mathbf{x}} + (I - J^{\dagger}I)\dot{\mathbf{q}}\_{0\prime} \tag{4}$$

where I is an (*n* × *n*) identity matrix and **q**˙ <sup>0</sup> is an arbitrary (*n* × 1) joint velocity vector. This solution includes the projector operator (*<sup>I</sup>* − *<sup>J</sup>*† *<sup>J</sup>*), which allows us to project a vector **q**˙ <sup>0</sup> on the null space of the initial solution provided by *J*†**x**˙. Gradient projection methods exploit this property and compute a joint speed vector **q**˙ as:

$$\dot{\mathbf{q}} = J^{\dagger}\dot{\mathbf{x}} + (I - J^{\dagger}I)\frac{\partial p\_0}{\partial \mathbf{q}}\tag{5}$$

*<sup>p</sup>*<sup>0</sup> being an arbitrary cost function that needs to be optimized and *<sup>∂</sup>p*<sup>0</sup> *<sup>∂</sup><sup>q</sup>* its gradient. Equation (5) indicates that the redundant degrees of freedom can be used to attain additional constraints, such as obtaining greater manipulability along the trajectory or avoiding collisions with the environment. Usually, manipulability is measured with the index introduced by Yoshikawa [4]:

$$
\omega = \sqrt{\det(JJ^T)}\tag{6}
$$

Additionally, using the manipulability has a strong impact when trying to avoid kinematic singularities. In particular, that situation happens whenever the matrix *J*, at some joint position **q**, has a rank less than *m*. This situation can be easily detected, since the manipulability index *ω* becomes null and the manipulator loses, at least, one degree of freedom, which jeopardizes its ability to complete the task.

#### **4. Trajectory Tracking Optimisation**

The manipulator task studied in this work corresponds to a tracking problem, whereby an end-effector task space trajectory needs to be mapped to a corresponding joint space trajectory. Given **x***i*, representing the position and orientation of the end effector with respect to a base reference system, the proposed methodology can be regarded as a trajectory optimisation problem for the set of *N* waypoint goals *G* = {**x**1, **x**2, ... **x***N*} defining the contact surface that the robot must follow with precision. Maximising the manipulability index along the trajectory whilst avoiding obstacles allows us to move away from singularities whilst facilitating the desired motion along the desired surface for the contact/visiting task being pursued.

The method starts by generating a set of *K* hypotheses on the path:

$$Q\_k = \{\mathbf{q}\_1, \mathbf{q}\_2 \dots \mathbf{q}\_N\}. \tag{7}$$

Each hypothesis *K* of the path ensures that the robot's end effector reaches the goal **x***<sup>i</sup>* for *i* = 1, ... *N*, and utilises an inverse kinematic method based on the Moore–Penrose pseudo inverse as described in Algorithm 2. The initial joint positions **q***r* of the manipulator are initialized randomly and, as a result, the resulting *K* paths *Qk* = {**q**1, **q**<sup>2</sup> ... **q***N*} are completely random with the property of |**q**˙ *<sup>T</sup>***q**˙ | being minimal for each different trajectory at each time step.

This set of *K* joint paths form a set of hypotheses that start from different and arbitrary initial **q**<sup>1</sup> joint configurations. Each of the generated trajectories is optimized at each time step *i* = 1, ... *N* in order to increase its manipulability index indicated in Equation (6) and, at the same time, avoid obstacles. For each of the *K* hypotheses over the trajectory, our method performs a sampling on the null space at each step *i* of the trajectory. The sampling is achieved by generating *H* samples around each joint configuration **q***<sup>i</sup>* . To generate each new sample, we compute a vector belonging to the null space as:

$$
\dot{\mathbf{q}}\_{\rm li} = (I - I^{\dagger}I)\dot{\mathbf{q}}\_{0\prime} \tag{8}
$$

where **q**˙ <sup>0</sup> represents an arbitrary vector that generates a sample of the null space. Next, *H* random movements are generated as:

$$\mathbf{q}\_h = \mathbf{q}\_i + \boldsymbol{\alpha} \cdot \dot{\mathbf{q}}\_h \Delta t. \tag{9}$$

Any of these new samples **q***<sup>h</sup>* around each joint configuration **q***<sup>i</sup>* does not alter the position and orientation of the end effector since **q**˙ *<sup>h</sup>* is only a self-motion and thus *J***q**˙ *<sup>h</sup>* = **0**. The variable *α* is chosen from a normal distribution and Δ*t* is an integration time. The time integration step Δ*t* and *α* are parameters of the algorithm.

At each time step, each of the new **q***<sup>h</sup>* (for *h* = 1, ... *H*) is a self motion around the joint positions **q***<sup>i</sup>* that can potentially improve its manipulability index while avoiding obstacles. For each **q***h*, we then compute a weight that accounts for the manipulability:

$$
\omega = \sqrt{IJ^T} \tag{10}
$$

$$
\mathcal{L} = 1/(\omega + \delta) \tag{11}
$$

$$\mathcal{W}\_{\omega} = e^{\frac{-\varepsilon}{\omega\_{\max}}},$$

where *ωmax* is the maximum observed manipulability of the mechanism. The parameter *δ* avoids the division by zero if the mechanism incurs in a singularity.

In order to consider the presence of obstacles, for each sample **q***h*, we compute the closest distance *d* of all the points of the robot arm to obstacles and with itself. The distance *d* is negative whenever any point of the manipulator lies inside an obstacle. We then compute:

$$\mathbf{x} = \max(\epsilon - d, 0) \tag{13}$$

$$\mathcal{W}\_0 = \exp(-\mathbf{x}/\lambda). \tag{14}$$

 is the minimum required distance to the obstacles and *λ* is a parameter that smooths the computed weights. During the experiments we have used = 0.2 m and *λ* = 0.5 m.

A weight is computed that accounts for the manipulability index and the distance to obstacles as:

$$\mathcal{W}\_{\eta h} = \mathcal{W}\_{\omega} \cdot \mathcal{W}\_{\vartheta}.\tag{15}$$

Finally, a sample **q***<sup>h</sup>* from the null-space is selected that maximizes the weight *Wqh* . The final path of the robot is obtained by selecting the path with the higher weight *Wqh* . The complete algorithm is described in Algorithm 4.

**Algorithm 4** Stochastic Planner(G, O) **INPUT:** *G* = **x**1, **x**2,... **x***N*, set of positions and orientations. **OUTPUT:** *Q* = **q**1, **q**2,... **q***N*, path of the robot. 1: **function** STOCHASTICPLANNER(G, O) 2: //Build K initial random paths. 3: **for** *k* = 1, 2, . . . , *K* **do** 4: **q***<sup>r</sup>* = *uni f orm*(1, *m*) 5: **for** *i* = 1, 2, . . . , *N* **do** 6: **q***i*+<sup>1</sup> = InverseKinematics(**x***i*, **q***i*) 7: **end for** 8: //Store a random path for the *K* particle. 9: *Qk* = {**q**1, **q**2,..., **q***N*} 10: **end for** 11: **while** convergence of ∑ *Wk* **do** 12: **for** *k* = 1, 2, . . . , *K* **do** 13: *pk* = *Gk* 14: **for** *i* = 1, 2, . . . , *N* **do** 15: *qi* = SampleFromNullSpace(*Gk*,*i*) 16: *wh* = ComputeWeights(*qh*) 17: //Find **q** that maximizes *W* = {*w*1,... *wH*} 18: **q**ˆ = *argmaxqh* (*Wh*) 19: //Add **q**ˆ to the path 20: **end for** 21: **end for** 22: **end while** 23: **return** *Q* = {**q**1, **q**2,..., **q***N*}, the joint path 24: **end function** 25: **function** SAMPLEFROMNULLSPACE(*Gk*,*i*) **INPUT:** Current joint position *i* at path *k*. **OUTPUT:** *H* samples *qh* from null space. 26: **for** *h* = 1, 2, . . . , *H* **do** 27: **<sup>q</sup>**˙ *<sup>h</sup>* = (*<sup>I</sup>* − *<sup>J</sup>*† *<sup>J</sup>*)**q**˙ <sup>0</sup> 28: **q***<sup>h</sup>* = **q***<sup>i</sup>* + · **q**˙ *<sup>h</sup>*Δ*t* 29: **end for return** *qh* = {*q*1, *q*2,..., *qH*}, samples from null space. 30: **end function**
