~~NOTOC~~
===== Homework 08 - Inverse Kinematics by GB Computation =====

The goal of this task is to solve the inverse kinematic task for a general 6R mechanism using a general Groebner Basis computation. This consists of the six main elements

  - Provide mechanism parameters ($\sin \alpha_i$, $\cos \alpha_i$, $a_i$, $d_i$) as  rational numbers satisfying identities on cosines and sines.
  - Provide the end effector pose as a rational matrix with rotation matrix satisfying the rotation identities (exactly).
  - Formulate the algebraic equations describing the inverse kinematics task.
  - Find a Groebner basis of the formulated IKT equations w.r.t. lexicographic monomial ordering (via Maple).
  - Recover real solutions for sines and cosines from a Groebner basis using back-substitution.
  - Recover the joint angles from the computed sines and cosines.

Groebner Basis computation is done in exact arithmetics over rational numbers and therefore input must be provided in rational numbers. At the same time, the input must satisfy all identities on rotations, otherwise the systems would have no solution.


=== Task ===

Steps to setup:
  - Install [[https://download.cvut.cz/maple-2021-for-students/ | Maple 2021 for students]] and add the path to the binary **maple** (for Linux, Mac OS) or **cmaple** (for Windows) into the PATH variable (so that Maple can be run from terminal by command ''maple'' (Linux, Mac OS) or ''cmaple'' (Windows)).
  - Study the [[https://cw.fel.cvut.cz/b211/_media/courses/pro/labs/hw08.pdf|materials from Lab 11]].
  - Download the project [[https://cw.fel.cvut.cz/b211/_media/courses/pro/labs/hw08.zip | hw08.zip]] and implement the functions described below.

**a.** Implement function ''rat_approx(n, tol)'' returning a rational approximation of the given number ''n''.

I/O specifications for ''rat_approx'':
  - ''n'': float number
  - ''tol'' : positive float number
  - **Return value**: ''Fraction'' number ''f'' such that
$$|f - n| < tol$$

**b.** Implement function ''exact_cs(angle, tol)'' returning a rational point on the unit circle, which is close to a given point defined by ''angle''.

I/O specifications for ''exact_cs'':
  - ''angle'': float number from the interval $[-\pi; \pi]$
  - ''tol'' : positive float number
  - **Return value**: list ''[c, s]'' of ''Fraction'' numbers, such that 
$$c^2 + s^2 = 1 \textrm{ (exactly) } \quad \& \quad \exists k \in \mathbb{Z}: |\mathrm{atan2}(s, c) - angle + 2k\pi| < tol$$

**c.** Implement function ''exact_rot(q, tol)'' returning an exact rotation close to the one given by quaternion ''q''.

I/O specifications for ''exact_rot'':
  - ''q'' : 1x4 vector of float numbers (1D ''np.array'') of (approximately) unit norm
  - ''tol'' : positive float number
  - **Return value**: 3x3 matrix ''r'' of type ''np.array'' with ''Fraction'' numbers inside, such that 
$$ r^\top r = \mathrm{I}, \;\; \det{r} = 1 \textrm{ (exactly) } \quad \& \quad ||r - R(q)||_{\mathrm{F}} < tol $$
where $||\cdot||_{\mathrm{F}}$ denotes the Frobenius norm.

**d.** Implement function ''rational_mechanism(mechanism, tol)'' which converts the input mechanism to the rational one.

I/O specifications for ''rational_mechanism'':
  - ''mechanism'': dictionary with 24 keys ''"theta1 offset"'', $\dots$, ''"theta6 offset"'', ''"d1"'', $\dots$, ''"d6"'', ''"a1"'', $\dots$, ''"a6"'', ''"alpha1"'', $\dots$, ''"alpha6"''. The values are float numbers.
  - ''tol'': positive float number
  - **Return value**: dictionary with 30 keys ''"theta1 offset"'', $\dots$, ''"theta6 offset"'', ''"d1"'', $\dots$, ''"d6"'', ''"a1"'', $\dots$, ''"a6"'', ''"cos alpha1"'', ''"sin alpha1"'', $\dots$, ''"cos alpha6"'', ''"sin alpha6"''. The values for the keys ''"theta1 offset"'', $\dots$, ''"theta6 offset"'' remain unchanged. The values for all the other keys are ''Fraction'' numbers and must be converted from the input values according to the previously implemented functions ''rat_approx'' and ''exact_cs''.

**e.** Implement function ''rational_pose(pose, tol)'' which converts the input pose to the rational one.

I/O specifications for ''rational_pose'':
  - ''pose'': dictionary with 2 keys ''"q"'' and ''"t"'', whose values are the (approximately) unit quaternion (1x4 matrix of type ''np.array'' with float numbers) and the translation (1x3 matrix of type ''np.array'' with float numbers) of the end effector, respectively.
  - ''tol'': positive float number
  - **Return value**: 4х4 homogeneous transformation matrix of type ''np.array'' with ''Fraction'' numbers inside which is the approximation of the input pose. Must be computed according to the previously implemented functions ''rat_approx'' and ''exact_rot''.

**f.** Implement function ''ikt_eqs(mechanism, pose, tol)'' which returns the polynomial equations for the rationalized inverse kinematic task.

I/O specifications for ''ikt_eqs'':
  - ''mechanism'': dictionary with 24 keys ''"theta1 offset"'', $\dots$, ''"theta6 offset"'', ''"d1"'', $\dots$, ''"d6"'', ''"a1"'', $\dots$, ''"a6"'', ''"alpha1"'', $\dots$, ''"alpha6"''. The values are float numbers.
  - ''pose'': dictionary with 2 keys ''"q"'' and ''"t"'', whose values are the (approximately) unit quaternion (1x4 matrix of type ''np.array'' with float numbers) and the translation (1x3 matrix of type ''np.array'' with float numbers) of the end effector, respectively.
  - ''tol'': positive float number
  - **Return value**: list of ''Poly'' objects with rational coefficients in variables ''c1'', ''s1'', $\dots$, ''c6'', ''s6'' describing the polynomial equations of IKT for the rationalized mechanism and pose.

**g.** Implement function ''solve_ikt_gb_lex(gb)'' which returns the list of real solutions to a lexicographic Groebner basis of the instance of IKT. It is forbidden to use the function **solve** from sympy library.

I/O specifications for ''solve_ikt_gb_lex'':
  - ''gb'': list of ''Poly'' objects with float coefficients describing the reduced lexicographic (the order of variables is ''c1'' > ''s1'' > $\dots$ > ''c6'' > ''s6'') groebner basis of some instance of IKT. The order of polynomials in ''gb'' is the following: ''gb[0]'' is a polynomial in ''s6'' only, ''gb[1]'' is a polynomial in ''c6'' and ''s6'', $\dots$, ''gb[11]'' is a polynomial in ''c1'' and ''s6''.
  - **Return value**: list of real solutions to the given polynomial system ''gb''. Every solution is a list of float numbers corresponding to ''c1'', ''s1'', $\dots$, ''c6'', ''s6''.
   
=== Upload ===

Upload a zip archive ''hw08.zip'' containing
  - ''hw08.json'' - json file, containing the real solutions for the mechanism and the pose specified for you in BRUTE. Consider the tolerance $tol = 10^{-5}$ as an input argument for ''ikt_eqs''. (If the automatic evaluation shows $-1$ as an error, this means that you have the wrong number of solutions.)
  - ''hw08.py'' - python script containing the implemented functions ''rat_approx'', ''exact_cs'', ''exact_rot'', ''rational_mechanism'', ''rational_pose'', ''ikt_eqs'', ''solve_ikt_gb_lex''

**Creating** ''hw08.json'':

The value stored in ''hw08.json'' will be a list of all real joint solutions for the mechanism and the pose specified for you in BRUTE. Every solution must be represented as a dictionary. **Every angle must belong to the interval $[-\pi, \pi]$**. An example looks as follows:

<code>
real_sols = [{"theta1": 1.1, "theta2": 1.2, "theta3": 1.3, "theta4": 1.4, "theta5": 1.5, "theta6": 1.6}, 
             {"theta1": 2.1, "theta2": 2.2, "theta3": 2.3, "theta4": 2.4, "theta5": 2.5, "theta6": 2.6}]
import json
with open("hw08.json", "w") as outfile:
    json.dump(real_sols, outfile)
</code>

**Example of how to use the function ''get_ikt_gb_lex''**:
<code>
from sympy import symbols
c1, s6 = symbols('c1, s6')
eqs = [Poly(s6**2 + c1 + 1), Poly(s6 + c1 + 1)]
gb = get_ikt_gb_lex(eqs)
print(gb)
</code>

The output of the ''print(gb)'' command is
<code>
[Poly(1.0*s6**2 - 1.0*s6, s6, domain='RR'), Poly(1.0*s6 + 1.0*c1 + 1.0, s6, c1, domain='RR')]
</code>

**The function ''get_ikt_gb_lex'' works only locally on your computer, don't try to upload the code with it to the upload system, it will fail**.