## 3 Numerical Methods for Nonlinear Kinematic Equations
### The ICCD Method
#### 3.3.2 Necessary Formulas for Solving Consecutive Prismatic Joints  
When there are consecutive prismatic joints in a robot manipulator, they are regarded as a whole. Moreover, these joint variables can be adjusted simultaneously in order to minimize the objective function of inverse kinematics. Assuming that the robot manipulator in question has consecutive prismatic joints from the \(i\)-th joint to the \(h\)-th joint, then the following can be achieved:  
\[\mathbf{R}_{i}=\mathbf{R}_{i+\mathbf{1}}=\cdots=\mathbf{R}_{h}=\mathbf{I},\quad(1\leqslant i<h \leqslant n). \tag{56}\]  
According to Equations (28), the objective function of inverse kinematics can be simplified to  
\[f(\mathbf{\Theta}_{i\sim h}) =\text{trace}\left(\left(\mathbf{R}_{\mathbf{cp}}-\mathbf{R}_{\mathbf{ti}}\right) ^{T}\left(\mathbf{R}_{\mathbf{cp}}-\mathbf{R}_{\mathbf{ti}}\right)\right)+\sum_{k=i}^{h}\theta _{k}^{2}+2\sum_{k=i}^{h-1}\sum_{j=k+1}^{h}\mathbf{v}_{k}^{T}\mathbf{v}_{j}\theta_{k} \theta_{j}\] \[\quad+\lambda\left(2\left(\mathbf{P}_{\mathbf{cp}}-\mathbf{P}_{\mathbf{ti}}\right) ^{T}\left(\sum_{k=i}^{h}\mathbf{v}_{k}\theta_{k}\right)+\left(\mathbf{P}_{\mathbf{cp}}- \mathbf{P}_{\mathbf{ti}}\right)^{T}\left(\mathbf{P}_{\mathbf{cp}}-\mathbf{P}_{\mathbf{ti}}\right) \right), \tag{57}\]  
where consecutive prismatic joints (\(\theta_{i}\cdots\theta_{h}\)) are joint variables and other joints are constant, and where  
\[\mathbf{\Theta}_{i\sim h} =[\theta_{i},\cdots,\theta_{h}]^{T}, \tag{58}\] \[\mathbf{R}_{\mathbf{cp}} =\prod_{j=h+1}^{n+1}\mathbf{R}_{j},\] (59) \[\mathbf{P}_{\mathbf{cp}} =\sum_{k=h}^{n}\prod_{j=h+1}^{k}\mathbf{R}_{j}\mathbf{P}_{k+\mathbf{1}}, \tag{60}\]  
\(\mathbf{R}_{\mathbf{ti}}\) and \(\mathbf{P}_{\mathbf{ti}}\) are defined as Equation (31) and Equation (32), respectively. It's not hard to conclude that when the joints are perpendicular to each other (\(\mathbf{v}_{k}^{T}\mathbf{v}_{j}=0\)), there is an analytical solution. In order to show the generality of the method, including the case that the joints are not perpendicular to each other (\(\mathbf{v}_{k}^{T}\mathbf{v}_{j}\
eq 0\)), this paper uses the combination of [PERSON]'s method and the CCD method to solve this problem. But note that this numerical solution is the same as the analytical solution when the joint satisfies the mutually perpendicular condition (\(\mathbf{v}_{k}^{T}\mathbf{v}_{j}=0\)). The gradient, the Hessian matrix, and the iteration formula of the CCD method with respect to the objective function (Equation (57)) are given below. The gradient of the objective function (Equation (57)) is defined as  
\[\bigtriangledown f(\mathbf{\Theta}_{i\sim h})=\left[\frac{\partial f}{\partial \theta_{i}},\cdots,\frac{\partial f}{\partial\theta_{h}}\right]^{T}, \tag{61}\]  
where  
\[\frac{\partial f}{\partial\theta_{j}}=2\left(\theta_{j}+\mathbf{v}_{j}^{T}\left( \sum_{k=i,k\
eq j}^{h}\mathbf{v}_{k}\theta_{k}\right)+\lambda\left(\mathbf{P}_{\mathbf{cp} }-\mathbf{P}_{\mathbf{ti}}\right)^{T}\mathbf{v}_{j}\right),\quad(j=i,\cdots,h). \tag{62}\]The Hessian matrix of the objective function (Equation (57)) is defined as  
\[\mathbf{H}(\mathbf{\Theta_{i\sim h}})=\bigtriangledown^{2}\!f(\mathbf{\Theta_{i\sim h}})= \left[ \begin{array}{cccc}\frac{\partial^{2}f}{\partial\theta_{i}^{2}}&\frac{ \partial^{2}f}{\partial\theta_{i}\partial\theta_{i+1}}&\dots&\frac{\partial^{2 }f}{\partial\theta_{i}\partial\theta_{h}}\\ \frac{\partial^{2}f}{\partial\theta_{i+1}\partial\theta_{i}}&\frac{\partial^ {2}f}{\partial\theta_{i+1}^{2}}&\dots&\frac{\partial^{2}f}{\partial\theta_{i+ 1}\partial\theta_{h}}\\ \vdots&\vdots&\ddots&\vdots\\ \frac{\partial^{2}f}{\partial\theta_{i}\partial\theta_{i}}&\frac{\partial^ {2}f}{\partial\theta_{h}\partial\theta_{i+1}}&\dots&\frac{\partial^{2}f}{ \partial\theta_{h}^{2}}\end{array}