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

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 Groebner basis of the formulated IKT equations w.r.t. lexicographic monomial ordering (via Maple).
5. Recover real solutions for sines and cosines from a Groebner basis using back-substitution.
6. 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.

Steps to setup:

1. Install 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)).
2. Study the materials from Lab 11.
3. Download the project 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:

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

1. angle: float number from the interval $[-\pi; \pi]$
2. tol : positive float number
3. 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:

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

1. mechanism: dictionary with 24 keys “theta1 offset”, $\dots$, “theta6 offset”, “d1”, $\dots$, “d6”, “a1”, $\dots$, “a6”, “alpha1”, $\dots$, “alpha6”. The values are float numbers.
2. tol: positive float number
3. 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:

1. pose: dictionary with 2 keys “q” and “t”, whose values are the (approximately) unit quaternion (1×4 matrix of type np.array with float numbers) and the translation (1×3 matrix of type np.array with float numbers) of the end effector, respectively.
2. tol: positive float number
3. 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:

1. mechanism: dictionary with 24 keys “theta1 offset”, $\dots$, “theta6 offset”, “d1”, $\dots$, “d6”, “a1”, $\dots$, “a6”, “alpha1”, $\dots$, “alpha6”. The values are float numbers.
2. pose: dictionary with 2 keys “q” and “t”, whose values are the (approximately) unit quaternion (1×4 matrix of type np.array with float numbers) and the translation (1×3 matrix of type np.array with 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 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:

1. 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.
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 hw08.zip containing

1. 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.)
2. 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:

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)

Example of how to use the function get_ikt_gb_lex:

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)

The output of the print(gb) command is

[Poly(1.0*s6**2 - 1.0*s6, s6, domain='RR'), Poly(1.0*s6 + 1.0*c1 + 1.0, s6, c1, domain='RR')]

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.