#!/usr/bin/env python
# coding: utf-8

# [中文版](http://nbviewer.jupyter.org/github/w407022008/All-of-Notes/blob/master/Robotics/The%20spatial%20rotation.ipynb)

# # Ⅰ.Spatial Rotation  
# 
# There are usually four ways to describe spatial transformations: Quaternions, Transformation Matrix, Euler angles, and Axis–angle representation.  
# This article will first introduce these four methods separately, and then explore the relationship between these four methods.  
# The diagram below clearly shows the conversion relationship between them.

# ![transformation](https://github.com/w407022008/All-of-Notes/blob/master/Robotics/Transformation.png?raw=true)

# ## 1. 绕轴转动 Axis–angle representation
# 
# This method is a geometric description method as well as the euler angle description method. This method defines a rotation axis and a rotation angle, and the counterclockwise direction is the positive direction of rotation along the rotation axis direction, as shown in the following figure.    
# In mathematics, the axis–angle representation of a rotation parameterizes a rotation in a three-dimensional Euclidean space by two quantities: a unit vector e indicating the direction of an axis of rotation, and an angle θ describing the magnitude of the rotation about the axis. 

# ![Axis–angle representation](https://upload.wikimedia.org/wikipedia/commons/thumb/7/7b/Angle_axis_vector.svg/300px-Angle_axis_vector.svg.png)

# The angle $\theta$ scalar multiplied by the unit vector e is the axis-angle vector
# 
# $$ \theta = \theta \vec{e}$$

# ## 2. 四元数 Quaternion
# 
# The quaternion is a mathematical expression of the rotation around the axis. It consists of the trigonometric value of the rotation angle and the rotation axis vector. It has four components, usually written in the following three forms:  
# $$q=(q_w,q_x,q_y,q_z)^T=(\alpha,\vec{\omega})^T=q_w+q_x\vec{i}+q_y\vec{j}+q_z\vec{k}$$
# where,$\alpha=cos(\frac{\theta}{2}),\vec{\omega}=sin(\frac{\theta}{2})\vec{e}$

# ### Quaternion operation and properties
# 
# - addition：$p+q=(a+\vec{u})+(t+\vec{v})=(a+t)+(\vec{u}+\vec{v})$  
# - Multiplication (Glassman product)：$p*q=(a+\vec{u})*(t+\vec{v})=at-\vec{u}\vec{v}+a\vec{v}+t\vec{u}+\vec{u}\times\vec{v}$  
# - Dot product (Euclidean inner product)：$p·q=(a,b,c,d)·(t,x,y,z)=at+bx+cy+dz=\frac{p*q+q*p}{2}$  
# - Inverse operation & conjugate operation：$q^{-1}=q^*=(q_w,-q_x,-q_y,-q_z)^T$
# - Pure quaternion：$p=x\vec{i}+y\vec{j}+z\vec{k}$ represents the point of a Cartesian coordinate system  
# - Unit quaternion：the length of the mold is1，$|q|=\sqrt{a^2+b^2+c^2+d^2}=1$

# ### Expression of spatial transformation in the global coordinate system：
# 
# - ** Representation of a process of pivoting：**Usually we use a ** unit quaternion** to indicate a rotation, such as:$q=cos(\theta)+sin(\theta)(\vec{i}+\vec{j}+\vec{k})$means that a space rotates $\theta$ degree counterclockwise around the axis of rotation $(\vec{i},\vec{j},\vec{k})$  
# 
# - **a representation of a Cartesian point：**Usually we use a **pure quaternion** to represent a point, such as:$P=x\vec{i}+y\vec{j}+z\vec{k}$ represents a point (x,y,z)。  
# 
# Therefore a vector $P_{in}$ rotating $\theta$ degree around the axis of rotation$\omega$ arrives at the other vector $P_{out}$.The processe can be reprensented as：$$P_{out}=q*P_{in}*q^{-1}$$$$=0+[(q_w^2+q_x^2-q_y^2-q_z^2)x+2(q_xq_y-q_wq_z)y+2(q_xq_z+q_wq_y)z]\vec{i}$$$$+[2(q_xq_y-q_wq_z)x+(q_w^2+q_y^2-q_x^2-q_z^2)y+2(q_yq_z-q_wq_x)z]\vec{j}$$$$+[2(q_xq_z-q_yq_w)x+2(q_yq_z-q_wq_x)y+(q_w^2+q_z^2-q_x^2-q_y^2)z]\vec{k}$$  
# 
# - **Rotate continuously** $q_1,q_2$，can be seen as a one rotation $q=q_1*q_2$，which is：$$P_{out}=q_1*q_2*P_{in}*q_2^{-1}*q_1^{-1}=q*P_{in}*q^{-1}$$

# # 3.Euler Angles
# 
# - **Order of rotating the axis**  
# 
#     Without considering the difference between a fixed coordinate system and a body coordinate system, there are usually 12 combinations of rotary axes, which are:  
#     - **Proper Euler angles** (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)
#     - **Tait–Bryan angles** (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)    
#     Tait–Bryan angles often referred to as *Heading, Elevation, and Bank* OR *Yaw, Pitch, and Roll*. Usually, we will collectively refer to the two types of combinations as Euler angles.
# 
# 
# - **Expression in the global coordinate system(extrinsic)：**  
#     The global coordinate system is also called the original coordinate system and the stationary coordinate system. The rotation process is sequentially rotated around the coordinate axis of the global coordinate system in a certain rotation order.
# 
# - **Expression in local coordinate system(intrinsic)：**  
#     The local coordinate system is also called the following coordinate system and the body coordinate system. The rotation process is sequentially rotated around the coordinate axis of the local coordinate system in a certain rotation order.

# # 4.变换矩阵 Rotation Matrix
# 
# ## Matrix meaning：  
# 
# - **Space**：An n-dimensional matrix can usually represent an n-space, such as: three mutually orthogonal vectors $(1,0,0)^T,(0,1,0)^T,(0,0,1)^T$，can be made into a tensor$\left [\begin{matrix}
#   1 & 0 & 0\\  0 & 1 & 0 \\  0 & 0 & 1 \end{matrix}\right]$, which represent a space.
# - **Transformation**：An n-dimensional matrix can usually also describe a spatial transformation, such as:$\left [\begin{matrix} 0\\1\\0  \end{matrix}\right]=\left [\begin{matrix} 0 & -1 & 0\\1 & 0 & 0\\0 & 0 & 1  \end{matrix}\right]\left [\begin{matrix} 1\\0\\0  \end{matrix}\right]$，which represents a space $\left [\begin{matrix} 1\\0\\0  \end{matrix}\right]$ rotates $90^o$ degree around the axis of z.
# - **Spatial Transformation**：Describe a space $S_1$ rotated to get another space $S_2$, writen as:$S_2=RS_1$
# 
# ## Spatial Transformation：  
# 
# - **Rotation around the coordinate axis:**
#     - around x：$R_x=\left [\begin{matrix} 1&0&0\\0&cos(\theta)&-sin(\theta)\\0&sin(\theta)&cos(\theta)  \end{matrix}\right]$
#     - around y：$R_y=\left [\begin{matrix} cos(\theta)&0&sin(\theta)\\0&1&0\\-sin(\theta)&0&cos(\theta)  \end{matrix}\right]$
#     - around z：$R_z=\left [\begin{matrix} cos(\theta)&-sin(\theta)&0\\sin(\theta)&cos(\theta)&0\\0&0&1  \end{matrix}\right]$
# - **Rotation around the coordinate axis one by one:**
#     - **Global frame：**$R_G=R_3R_2R_1$ represents the order of rotation:1、2、3
# 
#     - **Local frame：**$R_L=R_3R_2R_1$ represents the order of rotation:3、2、1

# # 5.Quaternion $\Leftrightarrow$ Angle-Axis representation
#     
# 
# a. Rotating an angle counterclockwise around $\vec e$, writen as：
# $$P_1\longrightarrow _{angle}^{\vec e} \longrightarrow P_2 \Leftrightarrow P_2=qP_1q^{-1} \Leftrightarrow P_2=RP_1=R_QP_1$$  
# 
# b. Rotating an angle clockwise around $\vec e$, writen as：
# $$P_1\longrightarrow _{-angle}^{\vec e} \longrightarrow P_2 \Leftrightarrow P_2=q^{-1}P_1q \Leftrightarrow P_2=RP_1=R_Q^TP_1$$
#   
# Here, the axis $\vec e$ is an arbitrary unit vector (x, y, z) defined in the global coordinate system, and angle is the scalar rotation angle.  
# 
# q is the unit quaternion defined by $\vec q$ and angle: $q=q_w+q_xi+q_yj+q_zk=cos\frac{angle}{2}+(xi+yj+zk)sin\frac{angle}{2}$  
#   
#   Therefore：$$ \left\{
# \begin{aligned}
# q_w & = & cos\frac{angle}{2} \\
# q_x & = & x·sin\frac{angle}{2} \\
# q_y & = & y·sin\frac{angle}{2} \\
# q_z & = & z·sin\frac{angle}{2}
# \end{aligned}
# \right.
# \Leftrightarrow
# \left\{
# \begin{aligned}
# angle & = & 2\arccos {q_w} \\
# x & = & \frac{q_x}{\sqrt{1-q_w^2}} \\
# y & = & \frac{q_y}{\sqrt{1-q_w^2}} \\
# z & = & \frac{q_z}{\sqrt{1-q_w^2}}
# \end{aligned}
# \right.
# \tag{1}
# $$

# # 6.Quaternion, Angle-Axis representation $\Leftrightarrow$ Rotation Matrix
# 
# Equivalent mathematical expression: $P_2=qP_1q^{-1}\Leftrightarrow P_2=R_QP_1$：  
# ## Quaternion,Angle-Axis $\Rightarrow$ Matrix
# 
# - Rotate an angle counterclockwise：  
# $$R=R_Q=\left [\begin{matrix}
# q_w^2+q_x^2-q_y^2-q_z^2 & 2q_xq_y-2q_zq_w & 2q_xq_z+2q_yq_w \\
# 2q_xq_y+2q_zq_w & q_w^2+q_y^2-q_x^2-q_z^2 & 2q_yq_z-2q_xq_w \\
# 2q_xq_z-2q_yq_w & 2q_yq_z+2q_xq_w & q_w^2+q_z^2-q_x^2-q_y^2 \\
# \end{matrix}\right]=\left [\begin{matrix}
# tx^2+c & txy-zs & txz+ys \\
# txy+zs & ty^2+c & tyz-xs \\
# txz-ys & tyz+xs & tz^2+c \\
# \end{matrix}\right]\tag{2}$$
# 
# - Rotate an angle clockwise： $R=R_Q^T$
# 
#     where：*$\begin{matrix}
# c=cos(\theta) & t=1-c \\
# s=sin(\theta) & \vec e=(x,y,z) \end{matrix}$*  

# ## Matrix $\Rightarrow$ Quaternion
# 
# 
# if  $R=\left [\begin{matrix}
#       a_{11} & a_{12} & a_{13} \\
#       a_{21} & a_{22} & a_{23} \\
#       a_{31} & a_{32} & a_{33} \\
#       \end{matrix}\right]$  then  
# 
# $$\left\{
# \begin{aligned}
# 4q_w^2=1+a_{11}+a{22}+a{33}\\
# 4q_x^2=1+a_{11}-a{22}-a{33}\\
# 4q_y^2=1-a_{11}+a{22}-a{33}\\
# 4q_z^2=1-a_{11}-a{22}+a{33}\\
# 4q_wq_x=a_{32}-a_{23}\\
# 4q_wq_y=a_{13}-a_{31}\\
# 4q_wq_z=a_{21}-a_{12}\\
# 4q_xq_y=a_{21}+a_{12}\\
# 4q_xq_z=a_{13}+a_{31}\\
# 4q_yq_z=a_{23}+a_{32}
# \end{aligned}
# \right.$$

# # 7.Euler Angles $\Leftrightarrow$ Rotation Matrix  
# 
# Considering that the description of the Euler angle has a distinction between the global coordinate system and the local coordinate system, and the requirements of the rotation axis order, the rotation around the coordinate axis is explained in detail below:  
# 
# ## Euler $\Rightarrow$ Matrix
# 
# - **Global frame: **$R_G=R_3R_2R_1$ represents the order of axis: 1、2、3
#     - **Proper Euler angles：**
#         - The order: x-y-x:$$R_G=R_xR_yR_x$$
#         - The order: x-z-x:$$R_G=R_xR_zR_x$$
#         - The order: y-x-y:$$R_G=R_yR_xR_y$$
#         - The order: y-z-y:$$R_G=R_yR_zR_y$$
#         - The order: z-x-z:$$R_G=R_zR_xR_z$$
#         - The order: z-y-z:$$R_G=R_zR_yR_z$$
#     - **Tait–Bryan angles：**
#         - The order: x-y-z:$$R_G=R_zR_yR_x$$
#         - The order: x-z-y:$$R_G=R_yR_zR_x$$
#         - The order: y-x-z:$$R_G=R_zR_xR_y$$
#         - The order: y-z-x:$$R_G=R_xR_zR_y$$
#         - The order: z-x-y:$$R_G=R_yR_xR_z$$
#         - The order: z-y-x:$$R_G=R_xR_yR_z$$
# - **Local frame: **$R_L=R_3R_2R_1$ represents the order of axis: 3、2、1
#     - **Proper Euler angles：**
#         **Consistent with the expression of Proper Euler angles of the local coordinate system**
#     - **Tait–Bryan angles：**
#         - The order: x-y-z:$$R_L=R_xR_yR_z$$
#         - The order: x-z-y:$$R_L=R_xR_zR_y$$
#         - The order: y-x-z:$$R_L=R_yR_xR_z$$
#         - The order: y-z-x:$$R_L=R_yR_zR_x$$
#         - The order: z-x-y:$$R_L=R_zR_xR_y$$
#         - The order: z-y-x:$$R_L=R_zR_yR_x$$
# 
# Considering the consistency of the mathematical forms, various forms of matrices are provided here. When a matrix of a mathematical form is specifically needed, the table can be looked up as follows:   
# 
# **i.e. $X_1Y_2Z_3$ represents all rotations of a mathematical form such as $R_xR_yR_z$, where $c_i, s_i$ represent $cos(<\vec{x}>),sin(<\vec{x}>)$,for sign $X_i$**
# 
# |Proper Euler angles|Tait-Bryan angles|
# |:---:|:---:|
# |![](https://wikimedia.org/api/rest_v1/media/math/render/svg/22ecc7d65613b53c38208ccdb9f3a05206222593)|![](https://wikimedia.org/api/rest_v1/media/math/render/svg/38d5c50b0be4c13b2b3a43ce32b37a14ab8c1f94)|
# |![](https://wikimedia.org/api/rest_v1/media/math/render/svg/47d2cef1c56a3058cf76c6717c72266c1588dcf5)|![](https://wikimedia.org/api/rest_v1/media/math/render/svg/88ca944aa8cbd89b7116b5171bd85a65472b5f9d)|
# |![](https://wikimedia.org/api/rest_v1/media/math/render/svg/c36e63d05a8166d8162ee9e29dd60fa4e40468de)|![](https://wikimedia.org/api/rest_v1/media/math/render/svg/315bca69e4cbda3218014b5a9b4878aa89231064)|
# |![](https://wikimedia.org/api/rest_v1/media/math/render/svg/fd69086f396f6ef72eed8e95b333537b60b406c9)|![](https://wikimedia.org/api/rest_v1/media/math/render/svg/ecef3eab0b5f54b66c7107f3ceeeb319a2e0e756)|
# |![](https://wikimedia.org/api/rest_v1/media/math/render/svg/547e522037de6467d948ecf3f7409975fe849d07)|![](https://wikimedia.org/api/rest_v1/media/math/render/svg/45583b01e584306f30f616307c8e53c349705b12)|
# |![](https://wikimedia.org/api/rest_v1/media/math/render/svg/1ece122a99938c6d8209a08822ec978d9492a170)|![](https://wikimedia.org/api/rest_v1/media/math/render/svg/3e7474f3e097e2f4bfe1e529fa321b5e0645275c)|

# ## Matrix, Quaternion, Angle-Axis $\Rightarrow$ Euler
# 
# According to the R matrix, we can inversely derive the corresponding Euler angle. However, if you encounter the form $\frac{0}{0}$, such a result is not desirable.    
# 
# For example：For the **global coordinate system, the Tait–Bryan angles with X-Y-Z order** has:  
# 
# $$ \left\{
# \begin{aligned}
# x & = & \arctan\frac{m_{21}}{m_{22}} & = & \arctan\frac{ 2q_yq_z+2q_wq_x}{ 2q_w^2+2q_z^2-1}  & = & \arctan\frac{tyz+xs}{tz^2+c} \\
# y & = & -\arcsin(m_{20}) & = & -\arcsin(2q_xq_z-2q_wq_y) & = & -\arcsin(txz-ys)  \\
# z & = & \arctan\frac{m_{10}}{m_{00}} & = & \arctan\frac{ 2q_xq_y+2q_wq_z}{ 2q_w^2+2q_x^2-1} & = & \arctan\frac{txy+zs}{tx^2+c}
# \end{aligned}
# \right.
# \tag{4}
# $$

# # 8. Euler Angles $\Leftrightarrow$ Quaternion
# 
# Considering that the description of the Euler angle has the distinction between the global coordinate system and the local coordinate system, and the requirements of the rotation axis order, the following example shows how to derive the Quaternion from the Euler angle:     
# 
# - **Global frame**：$P_{out}=(q_3*q_2*q_1)P_{in}(q_3*q_2*q_1)^{-1}$ represents successively rotating around the axis of 1-2-3
# - **Local frame**：$P_{out}=(q_3*q_2*q_1)P_{in}(q_3*q_2*q_1)^{-1}$ represents successively rotating around the axis of 3-2-1
# 
# ## Euler $\Rightarrow$ Quaternion
# - **Example 1**：**Global frame** successively rotating around the axis of **X-Y-Z**
# there are:  
# $$q_G=q_z*q_y*q_x=\left [\begin{matrix}q_w\\q_x\\q_y\\q_z\end{matrix}\right]=
# \left [\begin{matrix}
# cos\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}+sin\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}sin\frac{<\vec z>}{2} \\
# -cos\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}sin\frac{<\vec z>}{2}+sin\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}cos\frac{<\vec z>}{2} \\
# cos\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}+sin\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}sin\frac{<\vec z>}{2} \\
# cos\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}sin\frac{<\vec z>}{2}-sin\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}
# \end{matrix}\right] \tag{a}$$
# 
# - **Example 2**：**Global frame** successively rotating around the axis of **X-Z-Y**
# there are:  
# $$q_G=q_y*q_z*q_x=\left [\begin{matrix}q_w\\q_x\\q_y\\q_z\end{matrix}\right]=
# \left[\begin{matrix}
# cos\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}-sin\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}sin\frac{<\vec z>}{2} \\
# cos\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}sin\frac{<\vec z>}{2}+sin\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}cos\frac{<\vec z>}{2} \\
# cos\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}sin\frac{<\vec z>}{2}+sin\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}cos\frac{<\vec z>}{2} \\
# cos\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}-sin\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}sin\frac{<\vec z>}{2}
# \end{matrix}\right] \tag{b}$$
# 
# - **Example 3**：**Local frame** successively rotating around the axis of **X-Y-Z**
# there are:  
# $$q_L=q_x*q_y*q_z=\left [\begin{matrix}q_w\\q_x\\q_y\\q_z\end{matrix}\right]=
# \left[\begin{matrix}
# cos\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}-sin\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}sin\frac{<\vec z>}{2} \\
# sin\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}+cos\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}sin\frac{<\vec z>}{2} \\
# cos\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}-sin\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}sin\frac{<\vec z>}{2} \\
# sin\frac{<\vec x>}{2}sin\frac{<\vec y>}{2}cos\frac{<\vec z>}{2}+cos\frac{<\vec x>}{2}cos\frac{<\vec y>}{2}sin\frac{<\vec z>}{2}
# \end{matrix}\right] \tag{c}$$

# # 9. Significance of $P_2=RP_1$  
# 
#   a. $P_2=RP_1 \rightarrow$ indicates that in the coordinates of $P_1$, where is $P_1$ going?  
#   b. $P_3^T=P_1^TR \rightarrow$ indicates that in the coordinates of $P_1$, where is $P_1^T$ coming from?  
#   c. $P_4=R^TP_1 \rightarrow$ indicates that in the coordinates of $ P_4 $, where is the absolute stationary vector $ P_1 $ (the description of $ P_1 $ in the own coordinate system)?
#   
# **!Attention!**:   
#     1.$b \Leftrightarrow c$  
#     2.$R^{-1}=R^T$
# 
# Normally, we use the formula a to express the spatial transformation of the moving object in the global coordinate system; use the formula c to express the relative position of the absolute stationary object in the moving coordinate system, which is often used for position calibration.

# # Ⅱ.Some applications of quaternions

# - In the spatial multi-link mechanism, the end position of a series link after a single degree of freedom rotation:  
# 
#     - Position：$P=\sum_{i=1}^n  q_i P_i q_i^{-1}$ ,$P_i$ is the length vector of each link，$q_i$ is the current rotation quaternion, determined by the axis $\vec q_i$ and the angle $\alpha _i$
#     
#     - attitude：$\left\{\begin{aligned}
#     &\vec d=\vec d_{ini} \rightarrow_{\alpha_1,\alpha_2...\alpha_n}^{\vec q_1,\vec q_2...\vec q_n}\rightarrow \vec d_n &\\
#     &\vec q_i \rightarrow_{\alpha_1,...,\alpha_{i-1}}^{\vec q_1,...,\vec q_{i-1}} \rightarrow \vec q_i & ， i=2,3,4...
#     \end{aligned}\right.$,  
#     

# # Ⅲ.Precautions
# 
# - **欧拉角**：  
#     - **Gimbal lock**：这是欧拉角的一个本质上的缺点，因为他本身来源于陀螺仪的建模，所以不可避免的存在90°时的死锁现象（奇点，自由度退化），详情参考[这里](https://www.zhihu.com/question/47736315/answer/236284413)