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

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

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 all identities on rotations, otherwise the systems would have no solution.

Steps to setup:

1. Install Maple 2022 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 the command maple (Linux, Mac OS) or cmaple (Windows)).
2. Study the solution to IKT by GB computation.
3. Download the project hw07.zip and implement the functions described below.

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

I/O specifications for rat_approx:

1. n: float number
2. tol : positive float number
3. 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:

1. angle: float number from the interval $(-\pi; \pi]$
2. tol : positive float number
3. Return value: list [c, s] of Fraction approximations of $\cos\theta$ and $\sin\theta$, such that

$$c^2 + s^2 = 1 \textrm{ (exactly) }$$

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:

1. q : 1D np.ndarray of 4 float numbers of (approximately) unit norm
2. tol : positive float number
3. Return value: 3×3 matrix r of type np.ndarray 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:

1. mechanism: dictionary with 4 keys “theta offset”, “d”, “a”, “alpha”.
2. tol: positive float number
3. 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) which converts the input pose to the rational one.

I/O specifications for rational_pose:

1. 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.
2. tol: positive float number
3. 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) which returns the polynomial equations for the rationalized inverse kinematic task.

I/O specifications for ikt_eqs:

1. mechanism: dictionary with 4 keys “theta offset”, “d”, “a”, “alpha”.
2. 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.
3. tol: positive float number
4. 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 Gröbner basis of the instance of IKT.

I/O specifications for solve_ikt_gb_lex:

1. 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 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.
2. 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 a zip archive hw07.zip containing

1. hw07.json - json file, containing the real solutions for the mechanism and the pose specified for you in BRUTE. Consider the tolerance $tol = 10^{-5}$. (If the automatic evaluation shows $-1$ as the error, this means that you have the wrong number of solutions.)
2. hw07.py - python script containing the implemented functions rat_approx, rational_cs, rational_rot, rational_mechanism, rational_pose, ikt_eqs, solve_ikt_gb_lex

Creating hw07.json:

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

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("hw07.json", "w") as outfile:
    json.dump(real_sols, outfile)

from sympy import symbols
x, y = symbols('x, y')
eqs = [poly(x*y - 1), poly(x**2 - y)]
gb = eqs2gb(eqs, [x, y], "lex")
print(gb)

[Poly(1.0*y**3 - 1.0, y, domain='RR'), Poly(1.0*x - 1.0*y**2, x, y, domain='RR')]