# An Efficient Stochastic Constrained Path Planner for Redundant Manipulators
## 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 \(\mathbf{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=\{\mathbf{x}_{1},\mathbf{x}_{2},\ldots\mathbf{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}\ldots\mathbf{q}_{N}\}. \tag{7}\]  
Each hypothesis \(K\) of the path ensures that the robot's end effector reaches the goal \(\mathbf{x}_{i}\) for \(i=1,\ldots N\), and utilises an inverse kinematic method based on the Moore-Penrose pseudo inverse as described in Algorithm 2. The initial joint positions \(\mathbf{q}_{r}\) of the manipulator are initialized randomly and, as a result, the resulting \(K\) paths \(Q_{k}=\{\mathbf{q}_{1},\mathbf{q}_{2}\ldots\mathbf{q}_{N}\}\) are completely random with the property of \(|\mathbf{q}^{T}\mathbf{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 \(\mathbf{q}_{1}\) joint configurations. Each of the generated trajectories is optimized at each time step \(i=1,\ldots 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 \(\mathbf{q}_{i}\). To generate each new sample, we compute a vector belonging to the null space as:  
\[\dot{\mathbf{q}}_{h}=(I-J^{\dagger}f)\dot{\mathbf{q}}_{0}, \tag{8}\]  
where \(\dot{\mathbf{q}}_{0}\) 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}+\alpha\cdot\dot{\mathbf{q}}_{h}\Delta t. \tag{9}\]  
Any of these new samples \(\mathbf{q}_{h}\) around each joint configuration \(\mathbf{q}_{i}\) does not alter the position and orientation of the end effector since \(\dot{\mathbf{q}}_{h}\) is only a self-motion and thus \(\dot{\mathbf{q}}_{h}=\mathbf{0}\). The variable \(\alpha\) is chosen from a normal distribution and \(\Delta t\) is an integration time. The time integration step \(\Delta t\) and \(\alpha\) are parameters of the algorithm. At each time step, each of the new \(\mathbf{q}_{h}\) (for \(h=1,\ldots H\)) is a self motion around the joint positions \(\mathbf{q}_{i}\) that can potentially improve its manipulability index while avoiding obstacles. For each \(\mathbf{q}_{h}\), we then compute a weight that accounts for the manipulability:  
\[\omega=\sqrt{JJ^{T}} \tag{10}\] \[c=1/(\omega+\delta)\] (11) \[W_{\omega}=e^{\frac{-c}{\omega\max}}, \tag{12}\]  
where \(\omega_{max}\) is the maximum observed manipulability of the mechanism. The parameter \(\delta\) avoids the division by zero if the mechanism incurs in a singularity. In order to consider the presence of obstacles, for each sample \(\mathbf{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:  
\[x=\max(\epsilon-d,0) \tag{13}\] \[W_{o}=exp(-x/\lambda). \tag{14}\]  
\(\epsilon\) is the minimum required distance to the obstacles and \(\lambda\) is a parameter that smooths the computed weights.