Quaternion Conspectus - HackMD

<style> .reveal { font-size: 24px; } </style> # Quaternion Conspectus A summary on Quaternion rotation by: [Liber Normous](https://hackmd.io/@libernormous) --- # Intro - How do you rotate about arbitrary axis? - How do you rotate about a vector? - Quaternion is the answer! ---- Notation recap Point P based on $$ ^AP_A = (a,b,c) $$ Vector P based on coordinate {A} $$ ^A\mathbf{p}_p=\begin{bmatrix} a\\ b\\ c \end{bmatrix}\\ ^A\mathbf{p}_p=a \hat{x} + b\hat{y} + c\hat{z} $$ --- # Rotation transformation matrix ## On Z-axis \begin{bmatrix} \cos \theta & -\sin\theta & 0 \\ \sin \theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} ---- ## On X-axis \begin{bmatrix} 1 &0 &0 \\ 0 &\cos \theta &-\sin\theta \\ 0 &\sin \theta &\cos \theta \\ \end{bmatrix} ---- ## On Y-axis \begin{bmatrix} \cos \theta &0 &\sin\theta \\ 0 &1 &0 \\ -\sin\theta &0 &\cos \theta \\ \end{bmatrix} ---- ## On XYZ-axis $$ A\mathbf{p}'_p= \begin{bmatrix} \cos \theta & -\sin\theta & 0 \\ \sin \theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos \theta &0 &\sin\theta \\ 0 &1 &0 \\ -\sin\theta &0 &\cos \theta \\ \end{bmatrix} \begin{bmatrix} 1 &0 &0 \\ 0 &\cos \theta &-\sin\theta \\ 0 &\sin \theta &\cos \theta \\ \end{bmatrix} A\mathbf{p}_p $$ --- # Quaternion notation This is the form of quaternion expression $q = q_0 + q_1i + q_2j + q_3k$ and its conjugate is $q^* = q_0 - q_1i - q_2j - q_3k$ ---- Vector $p_A=[x,y,z]^T$ can be described using **pure** quaternion as $\mathring{A} = 0 + xi + yj + zk$ pure quaternion notation only comprises imaginary part, no real part ---- Let's define $\mathring{q} = a + bi + cj + ck$ as unit-vector quaternion describing a rotation axis from coordinate {B} to {A}. Quaternion describe the amount of rotation (angle) and the unit vector as rotation axis. It is different with euler rotation, which describe rotation angle based on x, y, and z axis (3 angles). Euler rotaion is rotated besed on sequences, for example XYZ, XYX, ZYX, and so on. Quaternion rotation is rotated only once with an angle on an axis which can be arbitary axis. The axis must be described as a unit vector. ---- So if you want to rotate a coordinate {A} to coordinate {B} then you must find the rotation axis. For quaternion you must define the rotation axis in one unit vector. ---- Then, let's define $\theta$ as the angle of rotation on unit vector $[v_x,v_y,v_z]^T$ as rotation axis. The quaternion element of $^A\mathring{q}_B$ can be calculated as $$ \left( \begin{matrix} a\\ b\\ c\\ d \end{matrix} \right) = \left( \begin{matrix} \cos{{\theta}\over{2}}\\ v_x \sin{{\theta}\over{2}}\\ v_y \sin{{\theta}\over{2}}\\ v_z \sin{{\theta}\over{2}} \end{matrix} \right) $$ ---- and become $$ \mathring{q} = \cos{{\theta}\over{2}} + v_x \sin{{\theta}\over{2}}i + v_y \sin{{\theta}\over{2}}j + v_z \sin{{\theta}\over{2}}k $$ ---- or in Prof. Peter Corke's book it becomes > [[3]](https://link.springer.com/book/10.1007%2F978-3-642-20144-8) page 36 : > Rotation of $\theta$ about the unit vector $\hat{n}$ > $$ \mathring{q} = \cos{{\theta}\over{2}} + \sin{{\theta}\over{2}}\hat{n} $$ and the norm (magnitude) must be equals to $1$ or $|\mathring{q}| =\sqrt{a^2 + b^2 + c^2 + d^2} = 1$ --- # Quaternion rotation matrix To rotate $q_A$ according to $\mathring{q}$ $q' = \mathring{q} \ q_A \ \mathring{q}^*$ ---- or in short $$ R(\mathring{q})= \begin{bmatrix} a^2+b^2-c^2-d^2 &2(bc - ad) &2(ac + bd)\\ 2(ad + bc) &a^2-b^2+c^2-d^2 &2(cd - ab)\\ 2(bd - ac) &2(ab + cd) &a^2-b^2-c^2+d^2 \end{bmatrix} $$ ---- So to rotate frame {B} to frame {A} can be performed with $$ ^AP_P = {^A}R_{B\mathring{q}}\ ^BP_P $$ --- # Angle-Axis format to Quaternion and vice versa ---- ## Angle-axis to quaternion format Let's define $\theta$ as the angle of rotation on unit vector $[v_x,v_y,v_z]^T$ as rotation axis. Then the quaternion format is: $$ \left( \begin{matrix} a\\ b\\ c\\ d \end{matrix} \right) = \left( \begin{matrix} \cos{{\theta}\over{2}}\\ v_x \sin{{\theta}\over{2}}\\ v_y \sin{{\theta}\over{2}}\\ v_z \sin{{\theta}\over{2}} \end{matrix} \right) $$ ---- ## Quaternion to angle-axis format Then we convert format quaternion format $\mathring{q} = a + bi + cj + ck$ using to angle-axis $(\theta, v_x, v_y, v_z)$ [[6]](https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm) $$ \left( \begin{matrix} \theta\\ v_x\\ v_y\\ v_z \end{matrix} \right) = \left( \begin{matrix} 2\cos^{-1}(a)\\ \frac{v_x}{\sqrt{1-b^2}}\\ \frac{v_x}{\sqrt{1-c^2}}\\ \frac{v_x}{\sqrt{1-d^2}} \end{matrix} \right) $$ --- # Converting RPY (Euler XYZ) to quaternion Source : [[4]](https://math.stackexchange.com/questions/2975109/how-to-convert-euler-angles-to-quaternions-and-get-the-same-euler-angles-back-fr) [[5]](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) [[6]](https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm) ---- NOTE! RPY is also commonly described in Tait Bryan angles (in terms of flight dynamics) [[5]](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) : * **Bank** ($\phi$) = Roll = Rotation about X-axis * **Attitude** ($\theta$) = Pitch = Rotation about Y-axis * **Heading** ($\psi$) = Yaw = Rotation about Z-axis ---- How do we find it? 1. First, roll around the world $x$ axis. The quaternion for this is $q_{x,\phi} = \cos\frac\phi2 + \left(\sin\frac\phi2\right)\mathrm i.$ 2. Second, pitch around the world $y$ axis. The quaternion is $q_{y,\theta} = \cos\frac\theta2 + \left(\sin\frac\theta2\right)\mathrm j.$ 3. Third, yaw around the world $z$ axis. The quaternion is $q_{z,\psi} = \cos\frac\psi2 + \left(\sin\frac\psi2\right)\mathrm k.$ ---- A rotation that is done in steps like this is modeled by multiplying the quaternions. The quaternion for the first rotation goes on the right. If you want a different order of rotations, rearrange the order of multiplication of the individual axis rotation quaternions accordingly. ---- Multiplying all these together, and recalling that $i^2 = j^2 = k^2 = 1,$ that $ij = k = -ji,$ that $jk = i = -kj,$ and that $ki = j = -ik,$ ---- $$ \begin{align} &q_{z,\psi} \cdot q_{y,\theta} \cdot q_{x,\phi} \\ = &\left(\cos\frac\psi2 + \left(\sin\frac\psi2\right)\mathrm k\right) \left(\cos\frac\theta2 + \left(\sin\frac\theta2\right)\mathrm j\right) \left(\cos\frac\phi2 + \left(\sin\frac\phi2\right)\mathrm i\right) \\ = &\left(\cos\frac\psi2+ \left(\sin\frac\psi2\right)\mathrm k\right) \\ &\left(\cos\frac\phi2\cos\frac\theta2+ \left(\sin\frac\phi2\cos\frac\theta2\right)\mathrm i + \left(\cos\frac\phi2\sin\frac\theta2\right)\mathrm j - \left(\sin\frac\phi2\sin\frac\theta2\right)\mathrm k\right) \\ = &\cos\frac\phi2\cos\frac\theta2\cos\frac\psi2+ \sin\frac\phi2\sin\frac\theta2\sin\frac\psi2 \\ &+ \left(\sin\frac\phi2\cos\frac\theta2\cos\frac\psi2- \cos\frac\phi2\sin\frac\theta2\sin\frac\psi2\right)\mathrm i\\ &+ \left(\cos\frac\phi2\sin\frac\theta2\cos\frac\psi2 + \sin\frac\phi2\cos\frac\theta2\sin\frac\psi2\right)\mathrm j\\ &+ \left(\cos\frac\phi2\cos\frac\theta2\sin\frac\psi2- \sin\frac\phi2\sin\frac\theta2\cos\frac\psi2\right)\mathrm k. \end{align} $$ ---- So   $$\left( \begin{matrix} a\\ b\\ c\\ d \end{matrix} \right)= \left( \begin{matrix} \cos\frac\phi2\cos\frac\theta2\cos\frac\psi2+ \sin\frac\phi2\sin\frac\theta2\sin\frac\psi2 \\ \sin\frac\phi2\cos\frac\theta2\cos\frac\psi2- \cos\frac\phi2\sin\frac\theta2\sin\frac\psi2 \\ \cos\frac\phi2\sin\frac\theta2\cos\frac\psi2 + \sin\frac\phi2\cos\frac\theta2\sin\frac\psi2 \\ \cos\frac\phi2\cos\frac\theta2\sin\frac\psi2- \sin\frac\phi2\sin\frac\theta2\cos\frac\psi2 \end{matrix} \right) $$ ---- CODE: ```python=1 def euler_to_quaternion(yaw, pitch, roll): qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) return [qx, qy, qz, qw] ``` --- # Converting quaternion to RPY (Euler XYZ) To convert a quaternion to Euler angles, we use facts such as $$ \begin{align} &\left(\cos\frac\phi2\cos\frac\theta2\cos\frac\psi2 + \sin\frac\phi2\sin\frac\theta2\sin\frac\psi2\right) \\ &\left(\sin\frac\phi2\cos\frac\theta2\cos\frac\psi2- \cos\frac\phi2\sin\frac\theta2\sin\frac\psi2\right)\\ &+ \left(\cos\frac\phi2\sin\frac\theta2\cos\frac\psi2+ \sin\frac\phi2\cos\frac\theta2\sin\frac\psi2\right)\\ &\left(\cos\frac\phi2\cos\frac\theta2\sin\frac\psi2 - \sin\frac\phi2\sin\frac\theta2\cos\frac\psi2\right) = \frac12 \sin\phi \cos\theta. \end{align} $$ and $$ \begin{align} &\left(\sin\frac\phi2\cos\frac\theta2\cos\frac\psi2- \cos\frac\phi2\sin\frac\theta2\sin\frac\psi2\right)^2\\ &+ \left(\cos\frac\phi2\sin\frac\theta2\cos\frac\psi2 + \sin\frac\phi2\cos\frac\theta2\sin\frac\psi2\right)^2= \frac12(1 - \cos\phi\cos\theta). \end{align} $$ ---- so in short! $$ \left( \begin{matrix} \phi\\ \theta\\ \psi\\ \end{matrix} \right)= \left( \begin{matrix} \text{atan2}(2(ab+cd),1-2(b^2+c^2))\\ \text{asin}(2(ac-db))\\ \text{atan2}(2(ad+bc),1-2(c^2+d^2)) \end{matrix} \right) $$ You will find another form in internet. If you do, please notice the euler rotation order. In this note I use XYZ euler rotation ---- CODE: ```python=1 def quaternion_to_euler(q): (x, y, z, w) = (q[0], q[1], q[2], q[3]) t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) roll = math.atan2(t0, t1) t2 = +2.0 * (w * y - z * x) t2 = +1.0 if t2 > +1.0 else t2 t2 = -1.0 if t2 < -1.0 else t2 pitch = math.asin(t2) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (y * y + z * z) yaw = math.atan2(t3, t4) return [yaw, pitch, roll] ``` --- # Example There is a vector $^A\mathbf{p}_P = [2,0,0]^T$ based on coordinate $\{A\}$. There is a vector $^A\mathbf{p}_R = [0,0,1]^T$. Rotate vector $^A\mathbf{p}_P$ $180^\circ$ to vector $^A\mathbf{p}_R$. 1. Turn each vector to their quaternion $$ q_A = 2i\\ q_R = k $$ ---- 2. Check if the magnitude of $|q_R|=1$, so it's correct $$ |q_R| = \sqrt{1^2} = 1 $$ ---- 3. Get the parameters $$ \left( \begin{matrix} a\\ b\\ c\\ d \end{matrix} \right) = \left( \begin{matrix} \cos{{\theta}\over{2}}\\ v_x \sin{{\theta}\over{2}}\\ v_y \sin{{\theta}\over{2}}\\ v_z \sin{{\theta}\over{2}} \end{matrix} \right)= \left( \begin{matrix} 0\\ 0\\ 0\\ -1 \end{matrix} \right) $$ ---- 4. Get the quaternion rotation matrix $$ R(\mathring{q})= \begin{bmatrix} -1 &0 &0\\ 0 &-1 &0\\ 0 &0 &1 \end{bmatrix} $$ ---- 5. Check using rotation matrix $$ ^A\mathbf{R}_B = \begin{bmatrix} \cos \theta & -\sin\theta & 0 \\ \sin \theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} -1 &0 &0\\ 0 &-1 &0\\ 0 &0 &1 \end{bmatrix} $$ 6. Same! ---- For example: If we have a roatation sequence of XYZ or RPY $\mathbf{R} = \mathbf{R_x}(\theta_r)\mathbf{R_y}(\theta_p)\mathbf{R_z}(\theta_y)=\mathbf{R_x}(0.1)\mathbf{R_y}(0.2)\mathbf{R_z}(0.3)$. Then, the rotatation matrix is $$ R = \left( \begin{matrix} 0.9363 &-0.2896 &0.1987 \\ 0.3130 &0.9447 &0.0978 \\ -0.1593 &0.1538 &0.9752 \end{matrix} \right) $$ The angle and rotation that describe matrix $\mathbf{R}$ is $\theta=0.3816$ and $\mathbf{v} = (0.3379, 0.4807, 0.8092)^T$ Try to proof it! --- # Table of content - Auto-generated Table of Content [ToC] --- # References Source: [[1] weizmann.ac.il quaternion-tutorial-2-0-1.pdf](https://www.weizmann.ac.il/sci-tea/benari/sites/sci-tea.benari/files/uploads/softwareAndLearningMaterials/quaternion-tutorial-2-0-1.pdf) [[2] chrobotics.com understanding quaternions](http://www.chrobotics.com/library/understanding-quaternions) [[3] Robotics Vision and Control by Prof Peter Corke](https://link.springer.com/book/10.1007%2F978-3-642-20144-8) [[4] math.stackexchange.com how to convert euler angles to quaternions](https://math.stackexchange.com/questions/2975109/how-to-convert-euler-angles-to-quaternions-and-get-the-same-euler-angles-back-fr) [[5] wikipedia.org Conversion between quaternions and Euler angles](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) [[6] euclideanspace.com quaternionToAngle](https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm) --- ###### tags: `robot`