~~NOTOC~~

===== Homework 06 - Rotation parameterization by quaternions =====

=== Task 6a ===
Verify that formula (7.112) in [[https://cw.fel.cvut.cz/b211/_media/courses/pro/pro-lecture-2021.pdf|PRO-Lecture.pdf]] for quaternion composition holds true. It is already present in the script in Maple, you just need to rewrite it in Python using ''sympy'' package.

Create a function ''check_7112(theta1_half, theta2_half, v_theta, v_phi, u_theta, u_phi)'' which takes two rotation half-angles ''theta1_half'' and ''theta2_half'' and also parameters ''v_theta, v_phi, u_theta, u_phi'' for defining two normalized rotation axes $(~v = [v_1, v_2, v_3]^T, u = [u_1, u_2, u_3]^T$) and returns ''RR21-R21'' following the example in the script. In order to express the coordinates of $v$ and $u$ explicitly, it is convenient to use the trigonometric parametrization of the unit sphere.

Input/Output specifications:
  * ''theta1_half'' : angle associated to the first rotation ($\frac{\theta_1}{2}$) (''sympy'' symbol)
  * ''theta2_half'' : angle associated to the second rotation ($\frac{\theta_2}{2}$) (''sympy'' symbol)
  * ''v_theta, v_phi'' : angles in the trigonometric parametrization of the unit sphere, which define $v$ (''sympy'' symbols)
  * ''u_theta, u_phi'' : angles in the trigonometric parametrization of the unit sphere, which define $u$ (''sympy'' symbols)
  * **Return value** : 3x3 ''sympy'' matrix. It is expected to have zeros inside (following the example in the script).

Inside ''check_7112'' define $v$ and $u$ as follows:
<code>
def check_7112(theta1_half, theta2_half, v_theta, v_phi, u_theta, u_phi):
    s1 = sympy.sin(theta1_half)
    c1 = sympy.cos(theta1_half)
    s2 = sympy.sin(theta2_half)
    c2 = sympy.cos(theta2_half)

    v1 = sympy.cos(v_theta) * sympy.sin(v_phi)
    v2 = sympy.sin(v_theta) * sympy.sin(v_phi)
    v3 = sympy.cos(v_phi)
    u1 = sympy.cos(u_theta) * sympy.sin(u_phi)
    u2 = sympy.sin(u_theta) * sympy.sin(u_phi)
    u3 = sympy.cos(u_phi)
    
    v = sympy.Matrix([v1, v2, v3])
    u = sympy.Matrix([u1, u2, u3])
</code>

Also, create function ''R_from_theta_half_axis(theta, v)'' that takes as input rotation half-angle ''theta_half'' (''sympy'' symbol) and axis ''v'' (''sympy'' Matrix, 3x1, already normalized) and **returns** rotation matrix $R$ (''sympy'' Matrix).

=== Task 6b ===
Verify that formula (7.116) in [[https://cw.fel.cvut.cz/b211/_media/courses/pro/pro-lecture-2021.pdf|PRO-Lecture.pdf]] for quaternion composition holds true:
  * compose ''q1'' from ''c1'' and ''s1v1''
  * compose ''q2'' from ''c2'' and ''s2v2''
  * compose ''q21'' from ''c21'' and ''s21v21'' 
  * check that $q_2 q_1 = q_{21}$ 

Create a function ''check_7116(theta1_half, theta2_half, v_theta, v_phi, u_theta, u_phi)'' which takes two rotation half-angles ''theta1_half'' and ''theta2_half'' and also parameters ''v_theta, v_phi, u_theta, u_phi'' for defining two normalized rotation axes ($v = [v_1, v_2, v_3]^T, u = [u_1, u_2, u_3]^T$) and returns ''q2q1 - q21'' (4x1, ''sympy'' Matrix, expected to be zeros).

Create function ''R_from_q(q)'' that takes as input quaternion (''sympy'' Matrix, 4x1) and **returns** rotation matrix $R$ (''sympy'' Matrix, 3x3).
Create function ''q_from_R(R)'' that takes as input rotation matrix $R$ (''sympy'' Matrix, 3x3) and **returns** quaternion (''sympy'' Matrix, 4x1).

=== Task 6c ===
Verify the formula 7.116 with the given rotation. You can use ''q1.subs()'' to get numbers
     - Find the quaternion representation $q_1$ and matrix representation $R_1$ for the rotation by $\frac{\pi}{2}$ around x-axis.
     - Find the quaternion representation $q_2$ and matrix representation $R_2$ of the rotation by $\frac{\pi}{2}$ around y-axis.
     - Construct the quaternion representations $q_{21}$ of the rotation $q_{21} = q_2 \circ q_1$ using the composition of quaternions (7.116).
     - Find the rotation matrix $R_{21}$ = $R_2 R_1$ by matrix multiplication.
     - Construct the rotation matrix from $q_{21}$ and compare it to $R_{21}$.
     - Construct the quaternion from the rotation matrix $R_{21}$ and compare it to $q_{21}$. 

Implement the solution in a single file ''hw06.py''. The file must contain the ''check_7112'', ''R_from_theta_half_axis'', ''check_7116'', ''R_from_q'', ''q_from_R'' functions, such that it can be imported (by an automatic evaluation) as

<code>
import hw06
res = hw06.check_7112(theta1_half, theta2_half, v_theta, v_phi, u_theta, u_phi)
R = hw06.R_from_theta_half_axis(theta_half, v)
res = hw06.check_7116(theta1_half, theta2_half, v_theta, v_phi, u_theta, u_phi)
R = R_from_q(q)
q = q_from_R(R)
</code>
=== Upload ===
Create an empty dictionary in Python:

<code>
solution = {}
</code>

The keys for this dictionary will be:
  * ''"q1"'' (4x1), ''"q2"'' (4x1), ''"q21"'' (4x1) containing the values for quaternions from task 6c
  * ''"R1"'' (3x3), ''"R2"'' (3x3), ''"R21"'' (3x3) containing the values for rotation matrices from task 6c

Finally, save ''solution'' to ''hw06.json'':

<code>
import json
with open("hw06.json", "w") as outfile:
    json.dump(solution, outfile)
</code>
Upload via the [[https://cw.felk.cvut.cz/upload/|course ware]] the zip archive ''hw06.zip'' containing 
  - ''hw06.py'' Python script (functions) solving the assignment. 
  - ''hw06.pdf'' short description of the solution
  - ''hw06.json''