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
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.
Steps to setup:
Forbidden modules: sympy.solvers
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
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
[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
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
“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
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 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
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 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.
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]$.
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.
solve
from hw08 for the Newton's method.
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:
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)
eqs2gb
:
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)
The output of the print(gb)
command is
[Poly(1.0*y**2 - 0.5, y, domain='RR'), Poly(x + y, x, y, domain='ZZ')]