~~NOTOC~~

===== Homework 09 - Inverse Kinematics by Gröbner Basis Computation =====

==== Overview ====

The goal of this task is to solve the inverse kinematic task for a general 6R mechanism using a general Gröbner 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 the identities on cosines and sines (exactly).
  - Provide an end-effector pose as a rational matrix with rotation matrix satisfying the rotation identities (exactly).
  - Formulate the algebraic equations describing the inverse kinematic task.
  - Find a Gröbner basis of the formulated IKT equations w.r.t. lexicographic monomial ordering (using Maple).
  - Recover real solutions for sines and cosines from a Gröbner basis using back-substitution, convert them to joint angles and apply Newton's method to refine the obtained angles.

Gröbner 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 the algebraic constraints exactly, otherwise the solutions of the formulated polynomial system couldn't be computed by Gröbner basis method.


==== Setup ====

Steps to setup:
  - Install [[https://download.cvut.cz/maple-2024/ | Maple]] and figure out the path to the binary **maple** (for Linux, Mac OS) or **cmaple** (for Windows).
  - Study the {{courses:pkr:labs:hw09.pdf|solution to the inverse kinematics of 6R manupulator by Gröbner basis computation}}.
  - Download the project {{courses:pkr:labs:hw09.zip | hw09.zip}}, modify the ''eqs2gb'' method by adding the path to the Maple binary and implement the functions described below.

==== Task ====

<note warning>
**Allowed libraries**: numpy, sympy, fractions, os

**Forbidden modules**: sympy.solvers
</note>

**a.** Implement function ''rational_approx(n, tol)'' that returns a rational approximation of a given number ''n''.

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

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

I/O specifications for ''rational_cs'':
  - ''angle'': float number from the interval $(-\pi; \pi]$
  - ''tol'' : positive float tolerance
  - **Return value**: list ''[c, s]'' of ''Fraction'' approximations of $\cos\theta$ and $\sin\theta$, such that 
$$c^2 + s^2 = 1 \textrm{ (exactly) } \quad \& \quad |c - \cos\theta| < tol, \;\; |s - \sin\theta| < tol $$

**c.** Implement function ''rational_rot(q, tol)'' that returns a rational rotation close to the one given by quaternion ''q''.

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

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

I/O specifications for ''rational_mechanism'':
  - ''mechanism'': dictionary with 4 keys ''"theta offset"'', ''"d"'', ''"a"'', ''"alpha"''.
  - ''tol'': positive float tolerance
  - **Return value**: dictionary with 5 keys ''"theta offset"'', ''"d"'', ''"a"'', ''"sin alpha"'', ''"cos alpha"''. The value for the key ''"theta offset"'' remains unchanged. The values for all the other keys are lists of ''Fraction'' numbers.

**e.** Implement function ''rational_pose(pose, tol)'' that 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 (1D ''np.ndarray'' of 4 float numbers) and the translation (1D ''np.ndarray'' of 3 float numbers) of the end effector, respectively.
  - ''tol'': positive float tolerance
  - **Return value**: 4х4 homogeneous transformation matrix of type ''np.ndarray'' with ''Fraction'' numbers inside that is the approximation of the input pose.

**f.** Implement function ''ikt_eqs(mechanism, pose, tol)'' that returns polynomials of degree at most $3$ (as in {{courses:pkr:labs:hw09.pdf|IKT by GB Computation}}, slide 5) that describe the rationalized inverse kinematic task.

I/O specifications for ''ikt_eqs'':
  - ''mechanism'': as in ''rational_mechanism''.
  - ''pose'': as in ''rational_pose''.
  - ''tol'': positive float tolerance
  - **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_gb(gb, mechanism, pose, niter, tol)'' that combines back-substitution with the Newton's method to find all real solutions to a lexicographic Gröbner basis of the instance of IKT. The Newton's method is used to refine the solutions obtained by back-substitution.

I/O specifications for ''solve_gb'':
  - ''gb'': list of ''Poly'' objects with float coefficients describing the reduced lexicographic (the order of variables is ''c1'' > ''s1'' > $\dots$ > ''c6'' > ''s6'') Gröbner basis of some instance of IKT. The order of polynomials in ''gb'' is as in {{courses:pkr:labs:hw09.pdf|IKT by GB Computation}}, slide 17.
  - ''mechanism'': as in ''ikt_eqs''.
  - ''pose'': as in ''ikt_eqs''.
  - ''niter'': integer that defines the number of iterations in the Newton's method.
  - ''tol'': positive float tolerance.
  - **Return value**: list of real solutions associated with the given polynomial system ''gb''. Every solution is a list of float numbers corresponding to $\theta_1$, $\dots$, $\theta_6$. **Every angle must belong to the interval $(-\pi, \pi]$**.

<note>
Use ''numpy.roots'' method to compute the roots of the univariate polynomial, since implementing it via companion matrix without any further improvement may not give suitable results for some polynomials with large coefficients.
</note>

<note tip>

Use function ''solve'' from [[courses:pkr:labs:hw08|hw08]] for the Newton's method.

</note>

==== Upload ====

Upload a zip archive ''hw09.zip'' containing
  - ''hw09.json'' - json file, containing the real solutions for the mechanism and the pose specified for you in BRUTE. (If the automatic evaluation shows $-1$ as the error, you have the wrong number of solutions.)
  - ''hw09.py'' - python script containing the implemented functions ''rational_approx'', ''rational_cs'', ''rational_rot'', ''rational_mechanism'', ''rational_pose'', ''ikt_eqs'', ''solve_gb''

**Creating** ''hw09.json'':

The value stored in ''hw09.json'' must 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 list. **Every angle must belong to the interval $(-\pi, \pi]$**. An example looks as follows:

<code>
real_sols = [[1.1, 1.2, 1.3, 1.4, 1.5, 1.6], 
             [2.1, 2.2, 2.3, 2.4, 2.5, 2.6]]
import json
with open("hw09.json", "w") as outfile:
    json.dump(real_sols, outfile)
</code>

<note tip>

**Example of how to use ''eqs2gb''**:
<code>
from sympy import symbols
x, y = symbols('x, y')
eqs = [poly(x**2 + y**2 - 1), poly(x + y)]
gb = eqs2gb(eqs, [x, y], "lex")
print(gb)
</code>

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

</note>