# Smooth Interpolation Design with Consideration of Corner Tolerance Constraints for Robotics
## 2 Linear Motion Planning
\begin{cases}0&if\cos B=0\\ \text{atan2}(a_{21}/\cos B,a_{11}/\cos B)&otherwise\\ \end{cases}  \tag{4}\]  
To transform \(R_{1}\) into \(R_{2}\), \(R_{1}\) must be multiplied by a rotation matrix \(R_{12}\). The relationship between \(R_{1}\) and \(R_{2}\) can be expressed as follows, where \(\mathbf{R}_{12}\) represents the rotation matrix from \(R_{1}\) to \(R_{2}\):  
\[R_{2}=R_{12}R_{1} \tag{5}\]  
\[R_{12}=R_{2}{R_{1}}^{-1}=\mathbf{R}_{\phi,\mu} \tag{6}\]  
The matrices \(R_{1}\) and \(R_{2}\) can be obtained from the orientation commands \(\text{P}_{\text{i}}\) and \(\text{P}_{\text{i+1}}\). Matrix \(R_{12}\) can then be calculated from Equation (6). However, the parameters \(u\) and \(\phi\) are still unknown. The equivalent angle-axis representation can be employed to determine matrix \(R_{12}\) by applying \(\mathbf{R}_{\phi,\mu}\). As shown in Figure 2b, the angle between the \(u\) vector and the \(i\)-\(k\) plane is represented by \(\gamma\), and \(\beta\) represents the angle between the \(u^{\prime}\) vector and the \(k\)-axis. The projection of  
Figure 2: (a) Rotation in and (b) parameters of the equivalent angle–axis representation. the \(u\) vector onto the \(i\)-, \(j\)-, and \(k\)-axes is represented by \(u_{i}\), \(u_{j}\), and \(u_{k}\), respectively. The following relationships can be determined from Figure 2b:  
\[\sin\gamma=u_{j} \tag{7}\]  
\[\sin\beta=\frac{u_{i}}{\sqrt{{u_{i}}^{2}+{u_{k}}^{2}}} \tag{8}\]  
\[\cos\gamma=\sqrt{{u_{i}}^{2}+{u_{k}}^{2}} \tag{9}\]  
\[\cos\beta=\frac{u_{k}}{\sqrt{{u_{i}}^{2}+{u_{k}}^{2}}} \tag{10}\]  
The rotation matrix \(R_{\phi,u}\), which describes the rotation \(\phi\) around vector \(u\), can be obtained as follows. First, vector \(u\) is rotated by angle \(\gamma\) on the \(i\)-axis to vector \(u^{\prime}\) located on the \(ik\) plane; the corresponding rotation matrix is represented by \(R_{\gamma,i}\). Vector \(u^{\prime}\) is then rotated by angle \(-\beta\) on the \(j\)-axis to vector \(u^{\prime\prime}\), which is located on the \(k\)-axis. This rotation matrix is represented by \(R_{-\beta,j}\). Vector \(u^{\prime\prime}\) is then rotated by \(\phi\) on the \(k\)-axis; the rotation matrix is \(R_{\phi,k}\). The reverse sequence of rotations and their respective opposite angles can be expressed as \(R_{\beta,j}\) and \(R_{-\gamma,i}\). After this sequence of rotations, the matrix \(R_{\phi,u}\) can be represented as  
\[R_{\phi,u}=R_{-\gamma,i}R_{\beta,j}R_{\phi,k}R_{-\beta,j}R_{\gamma,i} \tag{11}\]  
\[R_{\phi,u}= \begin{bmatrix}1&0&0\\ 0&c\gamma&s\gamma\\ 0&-s\gamma&c\gamma\end{bmatrix}\begin{bmatrix}c\phi&0&s\phi\\ 0&1&0\\ -s\phi&0&c\beta\end{bmatrix}\begin{bmatrix}c\phi&-s\phi&0\\ s\phi&c\phi&0\\ 0&0&1\end{bmatrix}\begin{bmatrix}c\phi&0&-s\beta\\ 0&1&0\\ s\phi&0&c\beta\end{bmatrix}\begin{bmatrix}1&0&0\\ 0&c\gamma&-s\gamma\\ 0&s\gamma&c\gamma\end{bmatrix}  \tag{12}\]  
where \(s\gamma=\sin\gamma\), \(c\gamma=\cos\gamma\), \(s\beta=\sin\beta\), \(c\beta=\cos\beta\), \(s\phi=\sin\phi\), and \(c\phi=\cos\phi\). By substituting Equations (7)-(10) into Equation (12), the following equation can be derived, where the abbreviation \(v\phi=1-cos\phi\) is used:  
\[R_{\phi,u}= \begin{bmatrix}{u_{x}}^{2}v\phi+c\phi&u_{x}u_{y}v\phi-u_{x}s\phi& u_{x}u_{x}v\phi+u_{y}s\phi\\ u_{x}u_{y}v\phi+u_{x}s\phi&u_{y}{}^{2}v\phi+c\phi&u_{y}u_{x}v\phi-u_{x}s\phi \\ u_{x}u_{x}v\phi-u_{y}s\phi&u_{y}u_{x}v\phi+u_{x}s\phi&u_{z}{}^{2}v\phi+c\phi \end{bmatrix} = \begin{bmatrix}a_{11}&a_{12}&a_{13}\\ a_{21}&a_{22}&a_{22}\\ a_{31}&a_{32}&a_{33}\end{bmatrix}  \tag{13}\]  
The rotational angle and the unit vector can be obtained from Equations (14) and (15). After \(u\) and \(\phi\) are obtained, \(\phi\) can be used as the variable for interpolation. \[\phi=\pm\cos^{-1}[(a_{11}+a_{22}+a_{33}-1)/2] \tag{14}\]  
\[u= \begin{bmatrix}u_{x}\\ u_{y}\\ u_{z}\end{bmatrix} =\frac{1}{2\phi} \begin{bmatrix}a_{32}-a_{23}\\ a_{13}-a_{31}\\ a_{21}-a_{12}\end{bmatrix}