# An Efficient Stochastic Constrained Path Planner for Redundant Manipulators
## 3 Kinematics of Redundant Manipulators
```
1:\(G=\{\mathbf{x}_{1},\mathbf{x}_{2},\ldots\mathbf{x}_{N}\}\), set of positions and orientations \(\mathbf{q}_{r}\), an initial random seed for joint positions OUTPUT:\(\{\mathbf{q}_{1},\mathbf{q}_{2},\ldots,\mathbf{q}_{N}\}\), the joints configuration path
2:functionSimple_Planner(G)
3:\(\mathbf{q}=\mathbf{q}_{r}\)
4:for\(i=1,2,\ldots,N\)do
5:\(\mathbf{x}_{i}=G\{i\}\)
6:\(\mathbf{q}_{i}=\text{Inverse\_Kinematics}(\mathbf{x}_{i},\mathbf{q})\)
7:\(\mathbf{q}=\mathbf{q}_{i}\)
8:endforreturn\(Q=\{\mathbf{q}_{1},\mathbf{q}_{2},\ldots,\mathbf{q}_{N}\}\) (the joint path)
9:endfunction
```  
**Algorithm 2** Simple Planner (\(G\), \(\mathbf{q}_{r}\))  
```
1:\(\mathbf{x}_{i}\), the position and orientation of the end effector \(\mathbf{q}_{r}\), the initial seed of the algorithm OUTPUT:\(\mathbf{q}\), joint positions
2:functionInverse_Kinematics(\(\mathbf{x}_{i}\), \(\mathbf{q}_{r}\))
3:\(\mathbf{q}_{i}=\mathbf{q}_{r}\)
4:while\(\mathbf{x}\
eq\mathbf{x}_{i}\)do
5:\(\dot{\mathbf{x}}=Compute\_VW(\mathbf{x}_{i},\mathbf{x})\)
6:\(J_{i}=\text{Jacobian}(\mathbf{q}_{i})\)
7:\(\dot{\mathbf{q}}_{i}=J_{i}^{\dagger}\dot{\mathbf{x}}\)
8:\(\mathbf{q}=\mathbf{q}+\dot{\mathbf{q}}_{i}\ast\Delta t\)
9:\(\mathbf{x}=f(\mathbf{q})\)
10:endwhile
11:return\(\mathbf{q}\)
12:endfunction
13:functionCompute_VW(\(\mathbf{x}_{i}\), \(\mathbf{x}\))
14://Computes linear and angular speed
15://to reduce the error in \(\mathbf{x}-\mathbf{x}_{i}\)
16:\(\dot{\mathbf{x}}=(\mathbf{x}-\mathbf{x}_{i})/\Delta t\)return\(\dot{\mathbf{x}}\)
17:endfunction
```  
**Algorithm 3** Inverse Kinematics(\(\mathbf{x}_{i}\), \(\mathbf{q}_{r}\))  
A more general solution to (3) can be written as:  
\[\dot{\mathbf{q}}=J^{\dagger}\dot{\mathbf{x}}+(I-J^{\dagger}J)\dot{\mathbf{q}}_{ 0}, \tag{4}\]  
where I is an \((n\times n)\) identity matrix and \(\dot{\mathbf{q}}_{0}\) is an arbitrary \((n\times 1)\) joint velocity vector. This solution includes the projector operator \(([PERSON]^{\dagger}J)\), which allows us to project a vector \(\dot{\mathbf{q}}_{0}\) on the null space of the initial solution provided by \(J^{\dagger}\dot{\mathbf{x}}\). Gradient projection methods exploit this property and compute a joint speed vector \(\dot{\mathbf{q}}\) as:  
\[\dot{\mathbf{q}}=J^{\dagger}\dot{\mathbf{x}}+([PERSON]^{\dagger}J)\frac{\partial p_{ 0}}{\partial\mathbf{q}} \tag{5}\]  
\(p_{0}\) being an arbitrary cost function that needs to be optimized and \(\frac{\partial p_{0}}{\partial\mathbf{q}}\) 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 [PERSON] [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 \(\mathbf{q}\), has a rank less than \(m\). This situation can be easily detected, since the manipulability index \(\omega\) becomes null and the manipulator loses, at least, one degree of freedom, which jeopardizes its ability to complete the task.