# Development of Continuum Robot Arm and Gripper for Harvesting Cherry Tomatoes
## 4 Kinematic and Kinetic Formulations
### D. Inverse Kinematic Solution  
According to the given set of variables \(\theta_{x,i}\), \(\theta_{y,i}\), \(\theta_{z,i}\) (\(i=1\), \(\cdots\), \(n\)) and \(l_{i}\) (\(i=1\), \(\cdots\), \(n-1\)), we calculate the end-point position by Equation (6),  
\[ \begin{pmatrix}\mathrm{P_{n}}\\ 1\end{pmatrix} =\mathrm{H_{0,n}} \begin{pmatrix}0\\ 0\\ 1\end{pmatrix}\begin{pmatrix}i_{n}&j_{n}&k_{n}&r_{n}\\ 0&0&0&1\end{pmatrix}\begin{pmatrix}0\\ 0\\ 1\end{pmatrix} = \begin{pmatrix}\mathrm{k_{n}}\mathrm{l_{n}}+\mathrm{r_{n}}\\ 1\end{pmatrix}  \tag{15}\]  
Taking a total differentiation of \(\mathrm{P_{n}}\)= \(\mathrm{k_{n}}\mathrm{l_{n}}\)+\(\mathrm{r_{n}}\) with respect to \(\theta_{\mathrm{x,i}}\), \(\theta_{\mathrm{y,i}}\), \(\theta_{\mathrm{z,i}}\) (\(\mathrm{i=1}\), \(\cdots\), \(\mathrm{n}\)) and \(\mathrm{l_{i}}\) (\(\mathrm{i=1}\), \(\cdots\), \(\mathrm{n-1}\)) and also motor angles \(\phi_{\mathrm{a}}\), \(\phi_{\mathrm{b}}\), \(\phi_{\mathrm{c}}\), \(\phi_{\mathrm{d}}\),  
\[\Delta\mathrm{P_{n}}=\frac{\partial\mathrm{P_{n}}}{\partial\mathrm{v}}\Delta \mathrm{v}+\frac{\partial\mathrm{P_{n}}}{\partial\phi}\Delta\phi \tag{16}\]  
where \(\mathbf{v}=(\theta_{\mathrm{x1}},\theta_{\mathrm{x2}}\), \(\cdots\), \(\theta_{\mathrm{xn}}\), \(\theta_{\mathrm{y1}},\theta_{\mathrm{y2}}\), \(\cdots\), \(\theta_{\mathrm{yn}}\), \(\theta_{\mathrm{z1}},\theta_{\mathrm{z2}}\), \(\cdots\), \(\theta_{\mathrm{z2m}}\), \(l_{1}\), \(l_{2}\), \(\cdots\), \(l_{\mathrm{n-1}}\)), \(\in\ \mathrm{R^{4n-1}}\) and \(\phi=(\phi_{\mathrm{a}}\), \(\phi_{\mathrm{b}}\), \(\phi_{\mathrm{c}}\), \(\phi_{\mathrm{d}}\) ). \(\frac{\partial\mathrm{P_{n}}}{\partial\mathrm{v}}\in\mathrm{R^{3\times 4n-1}}\) and \(\frac{\partial\mathrm{P_{n}}}{\partial\phi}\ \in\mathrm{R^{3\times 4}}\). Whereas, let \(\mathrm{w=}(\mathrm{w_{1}}\), \(\mathrm{w_{2}}\), \(\cdots\mathrm{w_{4n-1}})^{\mathrm{T}=0_{4n-1}}\) represent the \(4n-1\) equations provided by Equations (7), (8), (10) and (11), which also include \(\theta_{\mathrm{x,i}}\), \(\theta_{\mathrm{y,i}}\), \(\theta_{\mathrm{z,i}}\) (\(\mathrm{i=1}\), \(\cdots\), \(\mathrm{n}\)), \(\mathrm{l_{i}}\) (\(\mathrm{i=1}\), \(\cdots\), \(\mathrm{n-1}\)) and also the motor angles \(\phi_{\mathrm{a}}\), \(\phi_{\mathrm{b}}\), \(\phi_{\mathrm{c}}\), \(\phi_{\mathrm{d}}\). Taking a total differentiation for \(\mathrm{w=0_{4n-1}}\) as well, we have,  
\[\Delta\mathrm{w=}\frac{\partial\mathrm{w}}{\partial\mathrm{v}}\Delta\mathrm{v }+\frac{\partial\mathrm{w}}{\partial\phi}\Delta\phi=0_{4n-1} \tag{17}\]  
Figure 19: End-section kinetic structure. where \(\frac{\partial w}{\partial v}\in\mathrm{R}^{(4n-1)\times(4n-1)}\) and \(\frac{\partial w}{\partial\phi}\in\mathrm{R}^{(4n-1)\times 4}\). Since \(\frac{\partial w}{\partial w}\) is a square matrix, we can solve (19) with respect to the vector \(\Delta v\) as,  
\[\Delta v=-\left(\frac{\partial w}{\partial v}\right)^{-1}\frac{\partial w}{ \partial\phi}\Delta\phi \tag{18}\]  
Substituting (17) into (15), we have  
\[\Delta p_{n}=-\frac{\partial p_{n}}{\partial v}\Big{(}\frac{ \partial w}{\partial v}\Big{)}^{-1}\frac{\partial w}{\partial\phi}\Delta\phi+ \frac{\partial p_{n}}{\partial\phi}\Delta\phi=\] \[\Big{(}\frac{\partial p_{n}}{\partial\phi}-\frac{\partial p_{n}}{ \partial v}\Big{(}\frac{\partial w}{\partial v}\Big{)}^{-1}\frac{\partial w}{ \partial\phi}\Big{)}\Delta\phi=I\Delta\phi\]  
which can be solved for \(\Delta\phi\), by using a generalized inverse of the Jacobian \(\mathrm{J}\in\mathrm{R}^{3\times 4}\)  
\[\Delta\phi=\mathrm{J}^{\dagger}\Delta p_{n}+\mathrm{P}^{\perp}(\mathrm{J})\Psi \tag{19}\]  
where \(\mathrm{J}^{\dagger}\in\mathrm{R}^{4\times 3}\) is a generalized inverse of \(\mathrm{J}\) and \(\mathrm{P}^{\perp}(\mathrm{J})\in\mathrm{R}^{4\times 4}\) is a null projection operator of \(\mathrm{J}\), and \(\Delta\phi_{N}\in\mathrm{R}^{4}\) is a correction of \(\phi\) to minimize a positive scalar potential \(\varphi\) by making use of a redundant actuation.