A Design and an Implementation of an Inverse Kinematics Computation in Robotics Using Gröbner Bases

Abstract

The solution and a portable implementation of the inverse kinematics computation of a 3 degree-of-freedom (DOF) robot manipulator using Gröbner bases are presented. The main system was written Python with computer algebra system SymPy. Gröbner bases are computed with computer algebra system Risa/Asir, called from Python via OpenXM infrastructure for communicating mathematical software systems. For solving a system of algebraic equations, several solvers (both symbolic and numerical) are used from Python, and their performance has been compared. Experimental results with different solvers for solving a system of algebraic equations are shown.

1 Introduction

In robotics, inverse kinematics computation is one of the central topics in motion planning [17]. In the field of computer algebra, methods of the inverse kinematics computation using Gröbner bases have been proposed ([2, 7, 22, 23], and the references therein). After formulating the forward kinematics problem using the Denavit–Hartenberg convention, the inverse kinematics problem is derived as a system of algebraic equations by conversion of trigonometric expressions to polynomials. Then, the system is triangularized by computing the Gröbner basis with respect to the lexicographic ordering and solved by appropriate solvers.

Since methods using Gröbner bases solve the inverse kinematics problem directly, these methods have advantages that one can verify if the given inverse kinematics problem is solvable (with the certification of the solution if needed), and if it is solvable, one can obtain the configuration of parameters for the desired motion of the robot directly, before the actual motion. On the other hand, the computational cost of methods using Gröbner bases tends to be high compared to that of numerical methods, thus it is desired to decrease computational cost for effective computation of solving the inverse kinematics problem using Gröbner bases. Furthermore, for the use of these methods in robotics simulators such as the Robot Operating System (ROS) [9], an implementation that can easily be integrated with these simulators is needed.

In this paper, we present the solution and a portable implementation of the inverse kinematics computation of a 3 degree-of-freedom (DOF) robot manipulator using Gröbner bases. For portable implementation and rapid development, the main program is developed in Python with computer algebra system SymPy [12]. Gröbner bases are computed effectively using computer algebra system Risa/Asir [14], connected to Python with OpenXM infrastructure for communicating mathematical software systems [10]. Then, the system of algebraic equations is solved using an appropriate solver called within Python. As the main focus of our paper, several solvers for solving a system of algebraic equations have been compared: an exact solver included in SymPy, a multivariate numerical solver using the secant method, and a univariate numerical solver with successive substitutions.

The rest of the paper is organized as follows. In Sect. 2, the method of inverse kinematics computation of a 3 DOF manipulator using Gröbner bases is explained. In Sect. 3, the description of the proposed system for solving the inverse kinematics problem is presented. In Sect. 4, the result of experiments is presented.

2 Inverse Kinematics of a 3 Degree-of-Freedom (DOF) Robot Manipulator

In this paper, an example of 3 degree-of-freedom (DOF) manipulator has been built using LEGO^® MINDSTORMS^® EV3 Education¹ (henceforth abbreviated to EV3) (Fig. 1). An EV3 set has a computer (which is called “EV3 Intelligent Brick”), servo motors and sensors (gyro, ultrasonic, color and touch sensors), along with bricks used for constructing building blocks of robots. While a GUI-based programming environment for controlling the robot is available, several programming languages such as Python, Ruby, C, and Java can also be used on the top of other programming environments.

Fig. 1.

A 3 DOF manipulator built with EV3.

We first solve the forward kinematics problem. Components of the manipulator are defined as shown in Fig. 2. Segments (links) are called Segment i ($i=1,2,3,4$) from the one fixed on the ground towards the end effector, and a joint connecting Segment i and $i+1$ is called Joint i. For Joint i, the coordinate system $\mathrm {\Sigma }_i$, with the $x_i$, $y_i$ and $z_i$ axes and the origin at Joint i, is defined according to the Denavit–Hartenberg convention [18] (Fig. 2). Note that, since the present coordinate system is right-handed, the positive axis pointing upwards and downwards is denoted by “$\odot$” and “$\otimes$”, respectively. Furthermore, let $\mathrm {\Sigma }_0$ be the coordinate system satisfying that the origin is placed at the perpendicular foot from the origin of $\mathrm {\Sigma }_1$, and the direction of axes $x_0$, $y_0$ and $z_0$ are the same as that of axes $x_1$, $y_1$ and $z_1$, respectively. Also, let Joint 5 be the end effector, and $\mathrm {\Sigma }_5$ be the coordinate system with the origin placed at the position of Joint 5 and that the axes $x_5$, $y_5$ and $z_5$ have the same direction as the axes $x_4$, $y_4$ and $z_4$, respectively.

Let $a_i$ be the distance between axes $z_{i-1}$ and $z_i$, $\alpha _i$ the angle between axes $z_{i-1}$ and $z_i$ with respect to $x_i$ axis, $d_i$ the distance between axes $x_{i-1}$ and $x_i$, and $\theta _i$ be the angle between axes $x_{i-1}$ and $x_i$ with respect to $z_i$ axis. Then, the coordinate transformation matrix ${}^{i-1}T_i$ from the coordinate system $\mathrm {\Sigma }_{i-1}$ to $\mathrm {\Sigma }_i$ is expressed as

$$\begin{aligned} {}^{i-1} T_i&= \begin{pmatrix} 1 &{} 0 &{} 0 &{} a_i \\ 0 &{} 1 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 &{} 0 \\ 0 &{} 0 &{} 0 &{} 1 \end{pmatrix} \begin{pmatrix} 1 &{} 0 &{} 0 &{} 0 \\ 0 &{} \cos \alpha _i &{} - \sin \alpha _i &{} 0 \\ 0 &{} \sin \alpha _i &{} \cos \alpha _i &{} 0 \\ 0 &{} 0 &{} 0 &{} 1 \end{pmatrix} \begin{pmatrix} 1 &{} 0 &{} 0 &{} 0 \\ 0 &{} 1 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 &{} d_i \\ 0 &{} 0 &{} 0 &{} 1 \end{pmatrix} \begin{pmatrix} \cos \theta _i &{} -\sin \theta _i &{} 0 &{} 0 \\ \sin \theta _i &{} \cos \theta _i &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 &{} 0 \\ 0 &{} 0 &{} 0 &{} 1 \end{pmatrix} \\&= \begin{pmatrix} \cos \theta _i &{} -\sin \theta _i &{} 0 &{} a_i \\ \cos \alpha _i \sin \theta _i &{} \cos \alpha _i \cos \theta _i &{} -\sin \alpha _i &{} -d_i \sin \alpha _i \\ \sin \alpha _i \sin \theta _i &{} \sin \alpha _i \cos \theta _i &{} \cos \alpha _i &{} d_i \cos \alpha _i \\ 0 &{} 0 &{} 0 &{} 1 \end{pmatrix} . \end{aligned}$$

Since the joint parameters $a_i$, $\alpha _i$, $d_i$ and $\theta _i$ for EV3 are given as shown in Table 1 (note that the unit of $a_i$ and $d_i$ is centimeters) and the transformation matrix T from the coordinate system $\mathrm {\Sigma }_5$ to $\mathrm {\Sigma }_0$ is calculated as $T={}^{0}T_1{}^{1}T_2{}^{2}T_3{}^{3}T_4{}^{4}T_5$, the position ${}^t(x,y,z)$ of the end effector with respect to the coordinate system $\mathrm {\Sigma }_0$ is expressed as

$$\begin{aligned} \begin{pmatrix} x \\ y \\ z \end{pmatrix} = \begin{pmatrix} 8 (2 \cos (\theta _2 + \pi /4) + 2 \cos (\theta _2 + \theta _3 + \pi /4) + \sqrt{2}) \cos \theta _1 \\ 8 (2 \cos (\theta _2 + \pi /4 ) + 2 \cos (\theta _2 + \theta _3 + \pi /4)+ \sqrt{2} )\sin \theta _1 \\ 16 \sin (\theta _2 + \theta _3 + \pi /4 ) + 8 + 8 \sqrt{2} \end{pmatrix} . \end{aligned}$$

(1)

Fig. 2.

Components and the coordinate systems of the manipulator.

Table 1. Joint parameters for EV3.

Next, for solving the inverse kinematics problem, we solve Eq. (1) with respect to $\theta _1$, $\theta _2$, $\theta _3$. With the expansion of trigonometric functions using the trigonometric addition formulas and transformation of trigonometric functions defined as

$$ c_i=\cos \theta _i,\quad s_i=\sin \theta _i, $$

subject to $c_1^2+s_i^2=1$, Eq. (1) is transferred to a system of algebraic equations:

$$\begin{aligned} \begin{aligned} f_1&= 8 \sqrt{2} c_1 (c_2 + c_3 (c_2 -s_2) -s_2 -s_3(c_2+s_2)+1 ) - x = 0, \\ f_2&= 8 \sqrt{2} s_1 (c_2 + c_3 (c_2 - s_2) -s_2 -s_3(c_2 + s_2) +1) - y = 0, \\ f_3&= 8 \sqrt{2} (c_2 + c_3 (c_2 +s_2) + s_2 + s_3 (c_2 -s_2)+1) + 8 -z = 0, \\ f_4&= s_1^2 + c_1^2 -1 = 0, \quad f_5 = s_2^2 + c_2^2 -1 = 0, \quad f_6 = s_3^2 + c_3^2 -1 = 0. \end{aligned} \end{aligned}$$

(2)

Then, by putting the coordinate ${}^t(x,y,z)$ into Eq. (2) and computing Gröbner basis of an ideal $\langle f_1,\dots ,f_6\rangle$ with respect to the lexicographic (lex) ordering, a “triangularized” system of equations is obtained. By solving the triangularized system of equations, configuration of the joint angles $\theta _1,\theta _2,\theta _3$ are obtained.

Remark 1

We see that the ideal $\langle f_1,\dots ,f_6\rangle$ is zero-dimensional for generic values of x, y, z, as follows. By computing the comprehensive Gröbner system [8] of $\langle f_1,\dots ,f_6\rangle$ with parameters x, y, z and variables $c_1,s_1,c_2,s_2,c_3,s_3$ in $\mathbb {R}[x,y,z,c_1,s_1,c_2,s_2,c_3,s_3]$ with respect to lex order with $c_1\succ s_1\succ c_2\succ s_2\succ c_3\succ s_3$, we have $\{g_1,g_2,g_3,g_4,g_5,g_6\}$ as the Gröbner basis for the generic case². In the generic case, we have $\text {LM}(g_1)=s_3^4$, $\text {LM}(g_2)=c_3$, $\text {LM}(g_3)=s_2$, $\text {LM}(g_4)=c_2$, $\text {LM}(g_5)=s_1$, $\text {LM}(g_6)=c_1$, where $\text {LM}(g)$ denotes the leading monomial of g. This shows that the ideal is zero-dimensional for the generic case [1].

3 Implementation

We have implemented a system for the inverse kinematics computation of the manipulator in SymPy [12] on the top of Python, connecting with the computer algebra system Risa/Asir [14] via OpenXM infrastructure for communicating mathematical software systems [10].

Python (and SymPy) has been chosen for rapid development including the use of the library for solving algebraic equations, and interoperability with the Robot Operating System (ROS) [9] for embedding our present implementation as a simulation environment or an inverse kinematics solver in the future.

OpenXM (which stands for “Open message eXchange protocol for Mathematics”) consists of definitions of protocols and data formats for communication and/or interchange of mathematical information among mathematical software systems. It also includes implementation of interface for various mathematical softwares including Risa/Asir, Kan/sm1 [20], Maple [11], Mathematica [26], MixedVol [3], NTL [16], PHC Pack [25] and TiGERS [4].

Risa/Asir is used for effective computation of Gröbner bases. After receiving input polynomials from Python/SymPy, it first computes the Gröbner basis with respect to the graded reverse lexicographic (grlex) ordering. Then, it converts the basis to the one with respect to lex ordering (with a modular FGLM algorithm [14]) before returning the final result to the host program. Risa/Asir can be called from Python easily using ctypes library [15] with the OpenXM interface library for Risa/Asir.

4 Experiments

We have tested our implementation for inverse kinematics computation for randomly given points within the feasible region.

For solving a system of algebraic equations, the following solvers have been used:

• 1. a built-in exact solver in SymPy (sympy.solvers.solvers.solve),

• 2. a numerical solver in Python’s npmath library [6] (mpmath.findroot) using multivariate secant method³ (called by sympy.solvers.solvers.nsolve function in SymPy package),

• 3. a numerical solver in Python’s NumPy package [24] (numpy.roots) solving univariate algebraic equations with successive substitutions.

For each solver, 10 sets of experiments were conducted with 100 random points given in each set of experiments (thus 1000 random points were given in total).

The computing environment is as follows.

• Host environment. Intel Core i5-7360U 2.3GHz, RAM 8GB, macOS 10.15.1, Parallels Desktop Lite 1.4.0.

• Guest environment. RAM 2GB, Linux 4.15.0, Python 2.7.12, SymPy 1.4, mpmath 1.1.0, numpy 1.11.0, OpenXM 1.3.3-1, Asir 20191101.

Remark 2

As shown in Remark 1, if the given point is within the feasible region, the system of algebraic equations $g_1=\cdots =g_6=0$ has real solution(s) and can be solved rigorously, where $\{g_1,\ldots ,g_6\}$ is the Gröbner basis of the ideal $\langle f_1,\ldots ,f_6\rangle$ with respect to lex order.

Remark 3

For the exact computation of Gröbner bases, the coordinates of the sample points are given as rational numbers with the magnitude of the denominator is less than 100.

4.1 The Result with an Exact Solver (solve)

Table 2 shows the result of experiments with the exact solver (sympy.solvers.solvers.solve) [19]. For a system of algebraic equations, the solver computes a Gröbner basis with lex order, solve univariate equations and substitute the roots in the other equations to obtain the other coordinate⁴.

In each test, $T_\text {GB}$ is the average of computing time of Gröbner basis, $T_\text {Solve}$ is the average of computing time for solving the system of algebraic equations, $T_\text {Total}$ is the average of total computing time for inverse kinematics computation, and ‘Error’ is the average of absolute error of the position of the end effector with the configuration of joint angles $\theta _1,\theta _2,\theta _3$, computed by solving the inverse kinematics problem, from the randomly given position. Note that $T_\text {Total}$ includes the time for synchronizing received data of Gröbner basis from Risa/Asir ($\simeq$ 1.5 s) and the time for evaluation of Error. The bottom row ‘Average’ shows the average of the values in each column of the 10 test sets.

In all the test cases, the system of algebraic equations has been rigorously solved with finding appropriate real roots, although it took much time for finding the roots. For finding joint angles $\theta _1,\theta _2,\theta _3$, the solutions $s_1,c_1,s_2,c_2,s_3,c_3$ have been approximated by double precision floating-point numbers for efficient use of $\arctan$ function. We see that, though the approximation of the solution, the position of the end effector has been computed with high precision, compared to the length of the segments (Table 1).

Table 2. A result of the inverse kinematics computation with the exact solver (nsolve).

4.2 The Result with the Multivariate Numerical Solver (nsolve)

Table 3 shows the result of experiments with the multivariate numerical solver (mpmath.findroot) [5]. The columns ‘Test’, $T_\text {GB}$, $T_\text {Solve}$, $T_\text {Total}$, ‘Error’, and the bottom row ‘Average’ are the same as those in Table 2. In these sets of experiments, we have observed that the method did not converge in many cases, so the column ‘#Fail’ shows the number of tests in which the method did not converge in each test set. Note that the data in $T_\text {GB}$, $T_\text {Solve}$, $T_\text {Total}$, ‘Error’ and ‘Average’ have been taken for the tests in which the method has successfully converged. The number ‘(564)’ in the ‘Average’ row and the ‘#Fail’ column shows the total number of tests in which the method did not converge⁵.

The result shows that the method did not converge in approximately half of the test cases, while, in the cases that the method converged, the method is more efficient than the exact root-finding method. It also shows that, in the cases that the method converged, the magnitude of the absolute error of the solution is approximately 10 times larger than those in the case of the exact method, which is sufficiently small for practical use⁶.

Table 3. A result of the inverse kinematics computation with the multivariate numerical solver (nsolve).

Table 4. The result of the inverse kinematics computation with the univariate numerical solver (roots) with successive substitutions.

4.3 The Result with the Univariate Numerical Solver (roots) with successive substitutions

Table 4 shows the result of experiments with the univariate numerical solver (numpy.roots) [21]. The columns ‘Test’, $T_\text {GB}$, $T_\text {Solve}$, $T_\text {Total}$, ‘Error’, and the bottom row ‘Average’ are the same as those in Tables 2 and 3. The result shows that the method successfully converged and found appropriate solutions with sufficiently small errors in all the tests.

5 Concluding Remarks

In this paper, we have presented a portable implementation of the inverse kinematics computation of a 3 DOF robot manipulator using Gröbner bases. The implementation is made on the top of Python and SymPy with using Risa/Asir for efficient computation of Gröbner bases and symbolic and/or numerical solvers called within Python for solving a system of algebraic equations. Risa/Asir can easily be called from Python via OpenXM infrastructure.

The experiments have shown the following features of solvers used in solving the system of algebraic equations used in the present computation:

• 1. Symbolic solver can be used for inverse kinematics computation with high accuracy with the certification of real roots, although the computing time increases.

• 2. Multivariate numerical solver is often unstable, although it can be used to solve the inverse kinematics problem with high efficiency and accuracy in stable cases.

• 3. Univariate numerical solver with successive substitutions is stable with high efficiency and accuracy for all the tests in the present paper.

Thus, we see that univariate numerical solver with successive substitutions is effective for solving the inverse kinematics problem in the present case, although certification of real roots may be needed.

For future research, we need to improve implementation for calling Risa/Asir from Python via OpenXM in a more sophisticated way for more efficient computation.⁷ Furthermore, we intend to extend our implementation for embedding our solver in robotics simulators such as ROS and/or controlling the actual EV3 manipulators including the one we have built in the present paper.