## 3 Numerical Methods for Nonlinear Kinematic Equations
### Definition of Objective Function in Inverse Kinematics  
For an open chain with \(n\)-degrees of freedom with respect to forward kinematics \((T(\boldsymbol{\Theta}),\boldsymbol{\Theta}\in\mathbb{R}^{n})\), the inverse kinematics problem can be stated as follows: Given a homogeneous transformation matrix, \(\boldsymbol{E}\in SE(3)\) which is defined as follows  
\[\boldsymbol{E}= \begin{bmatrix}\boldsymbol{R_{e}}&\boldsymbol{P_{e}}\\ \boldsymbol{0}&1\end{bmatrix} , \tag{19}\]  
find solutions \((\boldsymbol{\Theta}^{*})\) that satisfy  
\[T(\boldsymbol{\Theta}^{*})=\boldsymbol{E}. \tag{20}\]  
As previously mentioned, this problem can be transformed into the problem of how to get the value of \(\boldsymbol{\Theta}^{*}\) in order to minimize the objective function (\(f(\boldsymbol{\Theta})\)) of inverse kinematics. Formally, the problem can be defined as Equation (21)  
\[\boldsymbol{\Theta}^{*}=\arg\min_{\boldsymbol{\Theta}}f(\boldsymbol{\Theta}), \tag{21}\]  
where the objective function (\(f(\boldsymbol{\Theta})\)) of inverse kinematics is written as  
\[f(\boldsymbol{\Theta}) =\text{RErr}(\boldsymbol{\Theta})+\lambda\text{PErr}(\boldsymbol{ \Theta})\] \[=\text{trace}\left(\left(\boldsymbol{R}(\boldsymbol{\Theta})- \boldsymbol{R_{e}}\right)^{T}(\boldsymbol{R}(\boldsymbol{\Theta})-\boldsymbol {R_{e}})\right)+\lambda\left(\boldsymbol{P}(\boldsymbol{\Theta})-\boldsymbol {P_{e}}\right)^{T}\left(\boldsymbol{P}(\boldsymbol{\Theta})-\boldsymbol{P_{e }}\right), \tag{22}\]  
where \(\lambda>0\) is a scale factor, and \(\boldsymbol{R}(\boldsymbol{\Theta})\) and \(\boldsymbol{P}(\boldsymbol{\Theta})\) are expressed as  
\[\boldsymbol{R}(\boldsymbol{\Theta}) =\prod_{i=1}^{n+1}\boldsymbol{R_{i}}(\theta_{i}), \tag{23}\] \[\boldsymbol{P}(\boldsymbol{\Theta}) =\sum_{k=0}^{n}\left(\prod_{i=1}^{k}\boldsymbol{R_{i}}(\theta_{i} )\right)\boldsymbol{P_{k+1}}(\theta_{k+1}), \tag{24}\]  
where \(\boldsymbol{R_{i}}(\theta_{i}),\boldsymbol{P_{i}}(\theta_{i})(i=1,\cdots,n)\) can be calculated using Equations (8) and (9), respectively, and where \(\boldsymbol{P_{n+1}}(\theta_{n+1}),\boldsymbol{R_{n+1}}(\theta_{n+1})\) are defined as  
\[\boldsymbol{P_{n+1}}(\theta_{n+1}) =\boldsymbol{P_{m}}, \tag{25}\] \[\boldsymbol{R_{n+1}}(\theta_{n+1}) =\boldsymbol{R_{m}}. \tag{26}\]  
Equation (22) is based on the least-squares cost function. The first part of Equation (22) represents the sum of the squares of the errors of all elements of the current rotation matrix (\(\boldsymbol{R}(\boldsymbol{\Theta})\)) and the target rotation matrix \((\mathbf{R_{e}})\). The second part of Equation (22) represents \(\lambda\) times the sum of the errors of all elements of the current displacement matrix \((\mathbf{P}(\mathbf{\Theta}))\) and the target displacement matrix \((\mathbf{P_{e}})\). Therefore, if a solution does not exist for inverse kinematics, then \(\mathbf{\Theta}^{*}\) satisfies the required conditions as closely as possible in a least-squares sense. However, if a solution exists for inverse kinematics, then \(\mathbf{\Theta}^{*}\) exactly satisfies  
\[f(\mathbf{\Theta}^{*})=\min f(\mathbf{\Theta})=0. \tag{27}\]  
A constructive proof of this claim is provided below. **Proof. ** If a solution of inverse kinematics \((\mathbf{\Theta}^{*})\) exists, then \(\mathbf{R}(\mathbf{\Theta}^{*})=\mathbf{R_{e}}\) and \(\mathbf{P}(\mathbf{\Theta}^{*})=\mathbf{P_{e}}\) must be satisfied. So Equation (27) must also be established. On the contrary, if there is \(\mathbf{\Theta}^{*}\) so that Equation (27) holds, then \(\mathbf{R}(\mathbf{\Theta}^{*})=\mathbf{R_{e}}\) and \(\mathbf{P}(\mathbf{\Theta}^{*})=\mathbf{P_{e}}\) must also be satisfied. So Equation (20) must be established.