## 3 Numerical Methods for Nonlinear Kinematic Equations
### The NICCD Method for The Inverse Kinematics Problem  
The CCD method has great robustness and simple calculation process [13], but the convergence rate is slow when the iteration point approaches the optimal point. The ICCD method has the same problem. Conversely, [PERSON]'s method (with a quadratic rate of convergence) converges quickly when the iteration point approaches the optimal point. However, [PERSON]'s method often fails to converge when the initial estimate is not sufficiently close, and cannot search the descent direction when the second-order Hessian matrix is not positive definite matrix. In order to inherit the advantages of the two algorithms and discard their disadvantages, the NICCD method is proposed to solve the inverse kinematics problem. The determination of the scale factor and the iterative procedure for the NICCD method are given below.  
```
1: Initialization: Set \(\text{Err}_{g}=+\infty\), \(\text{flag}_{ca2}=0\), and determine groups \(G^{(k)}=\left\{\mathbf{g}_{1}^{(k)},\mathbf{g}_{2}^{(k)},\cdots,\mathbf{g}_{l}^{(k)}\right\}\)
2:for each \(i\in[1,l]\)do
3:if\(\mathbf{g}_{i}^{(k)}\) is a group of single revolute joint then
4: Determine \(\mathbf{g}_{i}^{(k+1)}\) (see Equation (55))
5:elseif\(\mathbf{g}_{i}^{(k)}\) is a group of single prismatic joint then
6: Determine \(\mathbf{g}_{i}^{(k+1)}\) (see Equation (54))
7:else
8: Set \(\text{flag}_{ca1}=1\)
9:while\(\text{Err}_{g}>\epsilon_{ca}\) or \(\text{flag}_{ca2}\
eq 2\)do
10:if\(\mathbf{g}_{i}^{(k)}\) is a group of consecutive parallel revolute joints then
11: Calculate \(\bigtriangledown f(\mathbf{g}_{i}^{(k)})\) and \(\mathbf{H}(\mathbf{g}_{i}^{(k)})\) (see Equations (61), (63), and Appendix B)
12:else
13: Calculate \(\bigtriangledown f(\mathbf{g}_{i}^{(k)})\) and \(\mathbf{H}(\mathbf{g}_{i}^{(k)})\) (see Equations (61 \(\sim\) 65))
14:endif
15:if\(\mathbf{H}(\mathbf{g}_{i}^{(k)})\) is a positive definite matrix and \(\text{flag}_{ca1}=1\)then
16: Set \(\text{flag}_{ca2}=1\)
17: Determine \(\mathbf{g}_{i}^{(k+1)}\) by [PERSON]'s method (see Equation (10))
18:else
19: Set \(\text{flag}_{ca2}=2\)
20: Determine \(\mathbf{g}_{Ni}^{(k+1)}\) by [PERSON]'s method (see Equation (10))
21:if\(\mathbf{g}_{i}^{(k)}\) is a group of consecutive parallel revolute joints then
22: Calculate \(\mathbf{g}_{Ci}^{(k+1)}\) by CCD method (see Equation (72) and Appendix B)
23:else
24: Calculate \(\mathbf{g}_{Ci}^{(k+1)}\) by CCD method (see Equation (67))
25:endif
26: Set \(\mathbf{G}_{C}^{(k+1)}=\left\{\mathbf{g}_{1}^{(k+1)},\cdots,\mathbf{g}_{Ci}^{(k+1)},\mathbf{g} _{i+\
u}^{(k)},\cdots,\mathbf{g}_{l}^{(k)}\right\}\) and \(\mathbf{G}_{i}^{(k+1)}=\left\{\mathbf{g}_{1}^{(k+1)},\cdots,\mathbf{g}_{Ni}^{(k+1)},\mathbf{g} _{i+\
u}^{(k)},\cdots\cdot\mathbf{g}_{l}^{(k)}\right\}\)
27: Calculate \(f_{caC}=f(\mathbf{G}_{C}^{(k+1)})\) and \(f_{caN}=f(\mathbf{G}_{N}^{(k+1)})\) (see Equation (28))
28:if\(f_{caC}<f_{caN}\)then
29: Set \(\mathbf{g}_{i}^{(k+1)}=\mathbf{g}_{Ci}^{(k+1)}\)
30:else
31: Set \(\mathbf{g}_{i}^{(k+1)}=\mathbf{g}_{Ni}^{(k+1)}\)
32:endif
33:endif
34: Set \(\text{Err}_{g}=\left\|\mathbf{g}_{i}^{(k+1)}-\mathbf{g}_{i}^{(k)}\right\|\)
35:if\(\text{Err}_{g}\in\epsilon_{ca}\)then
36:if\(\text{flag}_{ca2}\
eq 2\)then
37: Set \(\text{flag}_{ca1}=0\)
38:endif
39:else
40: Set \(\text{flag}_{ca1}=1\)
41:endif
42: Set \(\mathbf{g}_{i}^{(k)}=\mathbf{g}_{i}^{(k+1)}\)
43:endif
44:endif
45:endif
46:endif
47:endif
48:endfor
49:Set \(\mathbf{\Theta}^{(k+1)}=\mathbf{G}^{(k+1)}\)
```  
**Algorithm 1** The improved cyclic coordinate descent (ICCD) method.