=Paper=
{{Paper
|id=Vol-3754/paper13
|storemode=property
|title=Towards trajectory planning for a 6-degree-of-freedom robot manipulator considering the orientation of the end-effector using computer algebra
|pdfUrl=https://ceur-ws.org/Vol-3754/paper13.pdf
|volume=Vol-3754
|authors=Takumu Okazaki,Akira Terui,Masahiko Mikawa
|dblpUrl=https://dblp.org/rec/conf/sycss/OkazakiTM24
}}
==Towards trajectory planning for a 6-degree-of-freedom robot manipulator considering the orientation of the end-effector using computer algebra==
https://ceur-ws.org/Vol-3754/paper13.pdf
Towards Trajectory Planning for a 6-Degree-of-Freedom
Robot Manipulator Considering the Orientation of the
End-effector Using Computer Algebra⋆
Takumu Okazaki1 , Akira Terui2,∗ and Masahiko Mikawa3
1
Master’s Program in Mathematics, Graduate School of Science and Technology, University of Tsukuba, Tsukuba
305-8571, Japan
2
Institute of Pure and Applied Sciences, University of Tsukuba, Tsukuba 305-8571, Japan
3
Institute of Library, Information and Media Science, University of Tsukuba, Tsukuba 305-8550, Japan
Abstract
We consider the trajectory planning of a 6-Degree-of-Freedom (DOF) robot manipulator using computer
algebra, with controlling the orientation of the end-effector. As a first step towards the objective, we
present a solution to the inverse kinematics problem of the manipulator such that the orientation of
the end-effector remains constant using computer algebra.
Keywords
Trajectory planning, Inverse kinematic problem, Robot manipulator, Gröbner basis
1. Introduction
This paper discusses the trajectory planning of a 6-Degree-of-Freedom (DOF) robot manipulator.
A manipulator is a robot resembling a human hand and comprises links that function as a
human arm and joints that function as human joints. Each link is connected for movement
relative to each other by a joint. The first link is connected to the ground, and the last link
called end-effector, contains the hand, which can be moved freely. In this paper, we consider a
manipulator called “myCobot 280" [1] (hereafter called “myCobot”) that has six joints connected
in series that can only rotate around a certain axis. Note that each joint has one degree of
freedom. Therefore, myCobot has at most six degrees of freedom.
The inverse kinematics problem is a problem of determining the joint arrangement when the
end-effector is placed in a specified direction on a certain coordinate in space. The trajectory
planning problem of the manipulator is an inverse kinematics problem in which the position
of the end-effector is expanded from a single coordinate to a trajectory. In other words, it can
be regarded as a problem to find the displacement of the joint when the end-effector of the
manipulator moves on a given trajectory from the initial position to the final position.
In computer algebra, methods for the inverse kinematics problem of a 6-DOF robot manipulator
have been proposed for more than 30 years ([2], [3]). Furthermore, several methods have
been proposed to solve the inverse kinematics problem of a manipulator using Gröbner basis
computation ([4], [5], [6], [7]). Among them, two of the present authors have proposed methods
for solving the inverse kinematic problem ([8], [9]) and trajectory planning problem ([10]) of
a 3-DOF manipulator using computer algebra. In more detail, we have proposed a method
for solving the inverse kinematic problem efficiently with the use of Comprehensive Gröbner
SCSS 2024: 10th International Symposium on Symbolic Computation in Software Science, August 28–30, 2024,
Tokyo, Japan
⋆
This work was partially supported by JKA and its promotion funds from KEIRIN RACE.
∗
Corresponding author.
Envelope-Open s2320132@u.tsukuba.ac.jp (T. Okazaki); terui@math.tsukuba.ac.jp (A. Terui); mikawa@slis.tsukuba.ac.jp
(M. Mikawa)
GLOBE https://researchmap.jp/aterui (A. Terui); https://mikawalab.org/ (M. Mikawa)
Orcid 0000-0003-0846-3643 (A. Terui); 0000-0002-2193-3198 (M. Mikawa)
© 2024 Copyright for this paper by its authors. Use permitted under Creative Commons License Attribution 4.0 International
(CC BY 4.0).
CEUR
ceur-ws.org
Workshop ISSN 1613-0073
Proceedings
84
6𝑥 7𝑥
6 6 𝑧7 𝑦
𝑑6 𝑦 7𝑧
𝜃6
6 7
joint6
end-effector 4 5
𝑑5 𝑥 𝑧
4 5
𝑑4 𝑦 4
𝑧 𝑦 5
𝑥
𝜃4 joint5
4 5
joint4
3
𝑎3 𝑥
𝜃5
3𝑦 3
𝑧
𝜃3
3
joint3
2𝑥
𝑎2
2
𝑦 2𝑧
𝜃2
2
joint2 1
𝑧
𝑑1
1𝑦 1
𝑥
joint1
1
𝜃1
Figure 1: The schematic diagram and the local coordinate systems of each joint in myCobot.
Systems (CGS) and certifying the existence of a solution to the inverse kinematic problem using
the CGS-QE, or the quantifier elimination (QE) based on the CGS computation.
In this paper, we consider the trajectory planning of a 6-DOF robot manipulator while
controlling the orientation of the end-effector using computer algebra. As a first step towards the
objective, we present a solution to the inverse kinematics problem such that the end-effector’s
orientation remains constant. More precisely, we present a solution to the inverse kinematics
problem under the condition that the end-effector’s local coordinate system overlaps the global
coordinate system by translation.
The paper is organized as follows. In Section 2, the coordinate system and the notation of the
manipulator are introduced. In Section 3, the analytical solution of general inverse kinematics
problems is first described, followed by the solution in myCobot. In Section 4, future initiatives
are described.
2. Preliminaries
2.1. Coordinate systems
For each joint in myCobot, the coordinate system is defined as follows (see Figure 1). Let
each joint be numbered as joint 𝑖 from the base to the end-effector in increasing order, and the
end-effector be numbered as joint 7. Let 𝑖 be the coordinate system of joint 𝑖. Here, 1 is the
reference (global) coordinate system, while the other coordinate systems are local. The axes
of each coordinate system are defined as follows: the 𝑖 𝑧 axis in the direction of the joint axis
(rotational axis in the case of a revolute joint), 𝑖 𝑥 axis in the direction of the common normal of
the 𝑖 𝑧 and 𝑖+1 𝑧 axes, and 𝑖 𝑦 axis to be a right-handed system.
2.2. Notation
For the index 𝑖, the matrices are denoted by 𝐴𝑖 , and vectors are denoted by 𝑖+1 𝑥𝑛 , in which
subscripts are used to distinguish points, and superscripts denote the local coordinate system to
which they refer (vectors with no superscripts are referenced in 1 ). If the vector is referenced
with respect to a different coordinate system, it is enclosed within brackets, and a separate
85
superscript is added outside the brackets. (e.g. 𝑖 [𝑖+1 𝑥𝑛 ]). Scalars are expressed in lowercase
variables, and subscripts, such as 𝑎𝑖 , are used where necessary. In 𝑖 , the origin is denoted by 𝑂𝑖
and the unit vectors are denoted by 𝑖 𝑒𝑥 , 𝑖 𝑒𝑦 and 𝑖 𝑒𝑧 . In vector 𝑣, the 𝑖-th component is denoted by
𝑣[𝑖].
2.3. The Denavit-Hartenberg convention
The ‘Denavit-Hartenberg parameters’ [11] are used as a transformation method between coor-
dinate systems. The following parameters are used in the transformations: 𝑎𝑖 as the length of
the common normal between the 𝑖 𝑧 and 𝑖+1 𝑧 axes, 𝛼𝑖 as the angle from the 𝑖 𝑧-axis towards the
𝑖+1 𝑧-axis around the 𝑖+1 𝑥-axis in the clockwise direction, 𝑑 as the length between the common
𝑖
normal 𝑎𝑖 and the origin of the coordinate system 𝑖, and 𝜃𝑖 as the angle between the common
normal 𝑎𝑖 and the 𝑖 𝑥 axis.
Let 𝐴𝑖 be the transformation matrix from 𝑖+1 and 𝑖 . Then, as the product of matrices
representing rotation and translation, 𝐴𝑖 is expressed by using the above parameters as
cos 𝜃𝑖 − sin 𝜃𝑖 cos 𝛼𝑖 sin 𝜃𝑖 sin 𝛼𝑖 𝑎𝑖 cos 𝜃𝑖
⎛ ⎞
sin 𝜃𝑖 cos 𝜃𝑖 cos 𝛼𝑖 − cos 𝜃𝑖 sin 𝛼𝑖 𝑎𝑖 sin 𝜃𝑖
𝐴𝑖 = ⎜ ⎟. (1)
⎜ 0 sin 𝛼𝑖 cos 𝛼𝑖 𝑑𝑖 ⎟
⎝ 0 0 0 1 ⎠
For expressing the transformation of the coordinate system, affine coordinates are used as
follows: if the coordinates of a point is expressed as 𝑡 (𝑖 𝑥, 𝑖 𝑦, 𝑖 𝑧) and also as 𝑡+1 (𝑖+1 𝑥, 𝑖+1 𝑦, 𝑖+1 𝑧),
they are denoted by 𝑖 𝑋 = 𝑡 (𝑖 𝑥, 𝑖 𝑦, 𝑖 𝑧, 1) and 𝑖+1 𝑋 = (𝑖+1 𝑥, 𝑖+1 𝑦, 𝑖+1 𝑧, 1), respectively, in which the
last coordinates represent translation. Then, by the definition of 𝐴𝑖 , we have 𝑖 𝑋 = 𝐴𝑖 𝑖+1 𝑋 and
𝑖+1 𝑋 = 𝐴−1 𝑖 𝑋, where
𝑖
cos 𝜃𝑖 sin 𝜃𝑖 0 −𝑎𝑖
⎛ ⎞
− sin 𝜃𝑖 cos 𝛼𝑖 cos 𝜃𝑖 cos 𝛼 𝑖 sin 𝛼 𝑖 −𝑑𝑖 sin 𝛼𝑖 ⎟
𝐴−1
𝑖 = ⎜ .
⎜ sin 𝜃𝑖 sin 𝛼𝑖 − cos 𝜃𝑖 sin 𝛼𝑖 cos 𝛼𝑖 −𝑑𝑖 cos 𝛼𝑖 ⎟
⎝ 0 0 0 1 ⎠
If 𝑛 + 1 coordinate systems are given, there exist 𝑛 coordinate transformations between
neighboring coordinate systems. Therefore, if the coordinates of a point are given as 7 𝑋 with
respect to 7 (the end-effector), the coordinates 1 𝑋 of the same point with respect to 1 (the global
coordinate system) is obtained by multiplying 𝐴𝑖 in sequence, such that
1 𝑋 = Aeq7 𝑋, Aeq = 𝐴1 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 . (2)
Note that the inverse transformation is given as Aeq−1 = 𝐴−1 −1 −1 −1 −1 −1
6 𝐴5 𝐴4 𝐴3 𝐴2 𝐴1 .
3. Solving the inverse kinematic problem
3.1. The forward kinematics
We first consider the forward kinematics problem of myCobot. Let 𝑝 = ⃖⃖⃖⃖⃖⃖⃖⃗ 𝑂1 𝑂7 be the vector
from the origin of 1 (position of the root of the manipulator) to the origin of 7 (position of the
end-effector), and let 𝑙 = 𝑡 (𝑙1 , 𝑙2 , 𝑙3 ) = 1 [7 𝑒𝑥 ], 𝑚 = 𝑡 (𝑚1 , 𝑚2 , 𝑚3 ) = 1 [7 𝑒𝑦 ], 𝑛 = 𝑡 (𝑛1 , 𝑛2 , 𝑛3 ) = 1 [7 𝑒𝑧 ] be
the vectors that are parallel to the unit vectors on the 𝑥, 𝑦, and 𝑧 axes of 7 , respectively. Then,
𝑝, 𝑙, 𝑚, 𝑛 is represented with respect to 1 as
𝑙 1 𝑚 0 𝑛 0 𝑝 0
⎛ 1⎞ ⎛ ⎞ ⎛ 1⎞ ⎛ ⎞ ⎛ 1⎞ ⎛ ⎞ ⎛ 1⎞ ⎛ ⎞
𝑙2 0 𝑚2 1 𝑛2 0 𝑝2 0
⎜ ⎟ = Aeq ⎜ ⎟ , ⎜ ⎟ = Aeq ⎜ ⎟ , ⎜ ⎟ = Aeq ⎜ ⎟ , ⎜ ⎟ = Aeq ⎜ ⎟ ,
𝑙
⎜ 3⎟ ⎜0 ⎟ 𝑚
⎜ 3⎟ ⎜0 ⎟ 𝑛
⎜ 3⎟ ⎜1 ⎟ 𝑝
⎜ 3⎟ ⎜0 ⎟
⎝0⎠ ⎝0 ⎠ ⎝0⎠ ⎝0 ⎠ ⎝0⎠ ⎝0 ⎠ ⎝1⎠ ⎝1 ⎠
86
therefore, each component of Aeq and Aeq−1 is obtained as
𝑙 𝑚1 𝑛1 𝑝1 𝑙 𝑙2 𝑙3 −(𝑙1 𝑝1 + 𝑙2 𝑝2 + 𝑙3 𝑝3 )
⎛1 ⎞ ⎛ 1 ⎞
𝑙2 𝑚2 𝑛2 𝑝2 −1 𝑚 1 𝑚 2 𝑚3 −(𝑚 1 𝑝1 + 𝑚2 𝑝2 + 𝑚3 𝑝3 )⎟
Aeq = ⎜ ⎟, Aeq = ⎜ . (3)
⎜𝑙3 𝑚3 𝑛3 𝑝3 ⎟ ⎜ 𝑛1 𝑛2 𝑛3 −(𝑛1 𝑝1 + 𝑛2 𝑝2 + 𝑛3 𝑝3 ) ⎟
⎝0 0 0 1 ⎠ ⎝0 0 0 1 ⎠
As seen in eq. (3), by putting the angles of the joints 𝜃1 , … , 𝜃6 into the transformation matrix 𝐴𝑖 ,
the position and orientation of the end-effector are obtained.
3.2. The inverse kinematic problem
To make use of eq. (3) in myCobot, we substitute some of the joint parameters 𝑎𝑖 , 𝛼𝑖 , 𝑑𝑖 , 𝜃𝑖 of
myCobot into the transformation matrix 𝐴𝑖 . As shown in the schematic diagram of myCobot in
Figure 1, where the squares represent the revolute joints and the angle of rotation 𝜃𝑖 is given by
taking a positive counterclockwise direction with respect to the 𝑖 𝑧 axis, the joint parameters are
given as
{𝛼1 , 𝛼2 , 𝛼3 , 𝛼4 , 𝛼5 , 𝛼6 } = {𝜋/2, 0, 0, 𝜋/2, 𝜋/2, 0}, 𝑎1 = 𝑑2 = 𝑑3 = 𝑎4 = 𝑎5 = 𝑎6 = 0. (4)
By substituting the joint parameters in eq. (4) into the transformation matrix 𝐴𝑖 in eq. (1),
Aeq is calculated. Then, by comparing the components of Aeq with the components of Aeq in
eq. (3), a system of 12 polynomial equations in the variables 𝑠𝑖 and 𝑐𝑖 (𝑖 = 1, … , 6) is obtained,
where 𝑠𝑖 = sin 𝜃𝑖 and 𝑐𝑖 = cos 𝜃𝑖 . Each 𝜃𝑖 can be obtained by selecting the appropriate equation(s)
from the above system and finding the solution. However, in the computation of Gröbner
basis, the coefficients of the equations may expand, which makes it difficult to find the solution.
Therefore, we focus on the structure of myCobot and solve its inverse kinematic problem from a
different perspective as follows.
3.3. Solving the inverse kinematic problem with a fixed orientation
Inverse kinematics problems for robot manipulators of a certain structure can be solved analyti-
cally [12, 13]. Pieper [13] has pointed out that when the end effector of a 6-DOF manipulator has
a spherical joint, the inverse kinematics problem can be separated into position and orientation
problems of the end-effector. He has further noted that when the rotational axes of three consec-
utive rotational joints of a 6-DOF manipulator intersect at a single point, the inverse kinematics
problem can also be separated into position and orientation problems of the end-effector.
Pieper’s argument suggests that if there exists a combination of three consecutive rotational
joints whose rotational axes intersect at a single point, it becomes possible to solve the inverse
kinematics problem analytically. However, unfortunately, in the case of myCobot, there are no
combinations of three consecutive rotational joints whose rotational axes intersect at a single
point, although there are combinations of two consecutive joints whose rotational axes intersect
at a single point. Therefore, following Pieper’s approach, we solve the inverse kinematics problem
by imposing constraints on the orientation of the end-effector.
For simplicity, we solve the inverse kinematic problem with the condition that the orientation
of the end-effector remains constant, i.e. the axis in 7 is parallel to the axis in 1 preserving the
same direction. Let 𝑙 = 𝑡 (1, 0, 0), 𝑚 = 𝑡 (0, 1, 0), 𝑛 = 𝑡 (0, 0, 1), then Aeq and Aeq−1 in eq. (3) become
as
1 0 0 𝑝1 1 0 0 −𝑝1
⎛ ⎞ ⎛ ⎞
0 1 0 𝑝2 0 1 0 −𝑝2
Aeq = ⎜ ⎟ , Aeq−1 = ⎜ ⎟.
⎜0 0 1 𝑝 3 ⎟ ⎜0 0 1 −𝑝3 ⎟
⎝0 0 0 1 ⎠ ⎝0 0 0 1 ⎠
Let 𝑃 be the intersection point of axes 4 𝑧 and 5 𝑧, expressed as
1 𝑃 = ⃖⃖⃖⃖⃖⃗
𝑂1 𝑃 = 𝑡 (𝑥, 𝑦, 𝑧, 1). (5)
87
Note that 𝑃 is the position of Joint 5. We first express sin 𝜃𝑖 and cos 𝜃𝑖 (𝑖 = 1, … , 6) with the
coordinate of the intersection 𝑃.
Let 7 𝑃 = ⃖⃖⃖⃖⃖⃗
𝑂7 𝑃, then 7 𝑃 is expressed in two ways as
0 −𝑑 sin 𝜃6 𝑥 𝑥 − 𝑝1
⎛ ⎞ ⎛ 5 ⎞ ⎛ ⎞ ⎛ ⎞
7 𝑃 = 𝐴−1 𝐴−1 ⎜ ⎟ = ⎜ 5 cos 𝜃6 ⎟ ,
0 −𝑑 7 𝑃 = Aeq−1 ⎜ ⎟ = ⎜𝑦 − 𝑝2 ⎟ .
𝑦
6 5
⎜0⎟ ⎜ −𝑑6 ⎟ ⎜ 𝑧 ⎟ ⎜ 𝑧 − 𝑝3 ⎟
⎝1⎠ ⎝ 1 ⎠ ⎝1 ⎠ ⎝ 1 ⎠
By equating two vectors 7 𝑃 above, we have
𝑝1 − 𝑥 𝑝2 − 𝑦
sin 𝜃6 = , cos 𝜃6 = . (6)
𝑑5 𝑑5
Let 𝑤4 be the unit vector in the direction of 4 𝑧-axis. Then, 𝑤4 and 7 [𝑤4 ] are expressed as
sin 𝜃1 cos 𝜃6 sin 𝜃5
⎛ ⎞ ⎛ ⎞
− cos 𝜃1 7 [𝑤 ] = 𝐴−1 𝐴−1 𝐴−1 ⎜ sin 𝜃6 sin 𝜃5 ⎟ .
−
𝑤4 = 𝐴1 𝐴2 𝐴3 ⎜ ⎟, 4 6 5 4 (7)
⎜ 0 ⎟ ⎜ − cos 𝜃5 ⎟
⎝ 0 ⎠ ⎝ 0 ⎠
By using 7 [𝑤4 ] above and Aeq, we obtain another expression for 𝑤4 as
𝑤4 = Aeq7 [𝑤4 ] = 𝑡 (cos 𝜃6 sin 𝜃5 , − sin 𝜃6 sin 𝜃5 , − cos 𝜃5 , 0). (8)
By comparing each component of 𝑤4 in eqs. (7) and (8), respectively, we have
𝑝1 − 𝑥 𝑝2 − 𝑦
cos 𝜃5 = 0, sin 𝜃5 = ±1, cos 𝜃1 = ± sin 𝜃6 = ± , sin 𝜃1 = ± cos 𝜃6 = ± . (9)
𝑑5 𝑑5
Next, 𝑃3 = ⃖⃖⃖⃖⃖⃗
𝑂1 𝑃 is expressed as
0 𝑑 sin 𝜃1 + 𝑎2 cos 𝜃1 cos 𝜃2 + 𝑎3 cos 𝜃1 (cos 𝜃2 cos 𝜃3 − sin 𝜃2 sin 𝜃3 )
⎛ ⎞ ⎛ 4 ⎞
0 −𝑑4 cos 𝜃1 + 𝑎2 sin 𝜃1 cos 𝜃2 + 𝑎3 sin 𝜃1 (cos 𝜃2 cos 𝜃3 − sin 𝜃2 sin 𝜃3 )
𝑃3 = 𝐴1 𝐴2 𝐴3 ⎜ ⎟ = ⎜ ⎟. (10)
⎜𝑑4 ⎟ ⎜ 𝑑1 + 𝑎2 sin 𝜃2 + 𝑎3 (sin 𝜃2 cos 𝜃3 + cos 𝜃2 sin 𝜃3 ) ⎟
⎝1⎠ ⎝ 1 ⎠
On the other hand, 𝑃3 is also expressed as 1 𝑃 = 𝑡 (𝑥, 𝑦, 𝑧, 1) as shown in eq. (5). Let 𝑄 =
1 𝑃[1]2 + 1 𝑃[2]2 + (1 𝑃[3] − 𝑑 )2 = 𝑃 [1]2 + 𝑃 [2]2 + (𝑃 [3] − 𝑑 )2 , then we have
1 3 3 3 1
𝑄 = 𝑥 2 + 𝑦 2 + (𝑧 − 𝑑1 )2 = 𝑎22 + 𝑎32 + 𝑑42 + 2𝑎2 𝑎3 cos 𝜃3 . (11)
Therefore,
𝑥 2 + 𝑦 2 + (𝑧 − 𝑑1 )2 − 𝑎22 − 𝑎32 − 𝑑42
cos 𝜃3 = , sin 𝜃3 = ±√1 − (cos 𝜃3 )2
2𝑎2 𝑎3
From the third component of 𝑃3 and the constraints,we have
𝑑1 + 𝑎2 sin 𝜃2 + 𝑎3 (sin 𝜃2 cos 𝜃3 + cos 𝜃2 sin 𝜃3 ) = 𝑧, (12)
(sin 𝜃2 )2 + (cos 𝜃2 )2 = 1. (13)
If cos 𝜃3 , sin 𝜃3 is obtained, cos 𝜃2 , sin 𝜃2 is easily found from eq. (12).
Finally to find cos 𝜃4 and sin 𝜃4 , let 𝑤5 be the unit vector in the direction of 5 𝑧-axis. Then, by
using 7 [𝑤5 ] expressed as
0 sin 𝜃6
⎛ ⎞ ⎛ ⎞
0
7 [𝑤 ] = 𝐴−1 𝐴−1 ⎜ ⎟ = ⎜ cos 𝜃6
5 6 5 ⎟,
⎜1 ⎟ ⎜ 0 ⎟
⎝0 ⎠ ⎝ 0 ⎠
88
we have the expression for 𝑤5 in two ways as
0 cos 𝜃1 sin(𝜃2 + 𝜃3 + 𝜃4 ) sin 𝜃6
⎛ ⎞ ⎛ ⎞ ⎛ ⎞
0 sin 𝜃1 sin(𝜃2 + 𝜃3 + 𝜃4 ) 7 cos 𝜃6
𝑤5 = 𝐴1 𝐴2 𝐴3 𝐴4 ⎜ ⎟ = ⎜ ⎟, 𝑤5 = Aeq [𝑤5 ] = ⎜ ⎟.
⎜1⎟ ⎜ − cos(𝜃2 + 𝜃3 + 𝜃4 ) ⎟ ⎜ 0 ⎟
⎝0 ⎠ ⎝ 0 ⎠ ⎝ 0 ⎠
Then, by comparing each component of 𝑤5 above, 𝜃4 is obtained as the one satisfying cos(𝜃2 +
𝜃3 + 𝜃4 ) = 0 and sin(𝜃2 + 𝜃3 + 𝜃4 ) = ±1, together with the use of the additivity theorem.
We could represent sin 𝜃𝑖 and cos 𝜃𝑖 (𝑖 = 1, … , 6) using the coordinates of the intersection 𝑃.
Next, we want to find 𝑥, 𝑦 and 𝑧.
First, comparing the third component of 7 𝑃, we have
−𝑑6 = 𝑧 − 𝑝3 . (14)
Next, from eq. (6) and the trigonometric identity, we have
𝑝1 − 𝑥 2 𝑝2 − 𝑦 2
( ) +( ) = 1. (15)
𝑑5 𝑑5
Finally, by equating the first and the third components in the vector in the right-most-hand
of eq. (10) with 𝑥 and 𝑧, respectively, we have
𝑑4 sin 𝜃1 + 𝑎2 cos 𝜃1 cos 𝜃2 + 𝑎3 cos 𝜃1 (cos 𝜃2 cos 𝜃3 − sin 𝜃2 sin 𝜃3 ) = 𝑥,
𝑑1 + 𝑎2 sin 𝜃2 + 𝑎3 (sin 𝜃2 cos 𝜃3 + cos 𝜃2 sin 𝜃3 ) = 𝑧.
𝑃 −𝑦 𝑝 −𝑥
Then, by substituting sin 𝜃1 = 2𝑑 and cos 𝜃1 = 1𝑑 in eq. (9) into the above equations, assuming
5 5
first that sin 𝜃5 = 1, we have the following system of equations in sin 𝜃2 , cos 𝜃2 , sin 𝜃3 , cos 𝜃3 :
𝑝2 − 𝑦 𝑝1 − 𝑥 𝑝1 − 𝑥
𝑑4 + 𝑎2 cos 𝜃2 + 𝑎3 (cos 𝜃2 cos 𝜃3 − sin 𝜃2 sin 𝜃3 ) = 𝑥,
𝑑5 𝑑5 𝑑5
𝑑1 + 𝑎2 sin 𝜃2 + 𝑎3 (sin 𝜃2 cos 𝜃3 + cos 𝜃2 sin 𝜃3 ) = 𝑧, (16)
(sin 𝜃2 )2 + (cos 𝜃2 )2 = 1,
(sin 𝜃3 )2 + (cos 𝜃3 )2 = 1,
in which the last two equations are added as trigonometric identities.
Then, the solution of the system in eq. (16) gives the value of cos 𝜃3 as
1
cos 𝜃3 = 2
(−𝑎22 𝑝12 − 𝑎32 𝑝12 + 𝑝12 𝑑12 + 𝑝22 𝑑42 + 2𝑎22 𝑝1 𝑥 + 2𝑎32 𝑝1 𝑥 − 2𝑝1 𝑑12 𝑥 − 2𝑝2 𝑑4 𝑑5 𝑥
2𝑎2 𝑎3 (𝑝1 − 𝑥)
− 𝑎22 𝑥 2 − 𝑎32 𝑥 2 + 𝑑12 𝑥 2 + 𝑑52 𝑥 2 − 2𝑝2 𝑑42 𝑦 + 2𝑑4 𝑑5 𝑥𝑦 + 𝑑42 𝑦 2 − 2𝑝12 𝑑1 𝑧 + 4𝑝1 𝑑1 𝑥𝑧 − 2𝑑1 𝑥 2 𝑧
+ 𝑝12 𝑧 2 − 2𝑝1 𝑥𝑧 2 +𝑥 2 𝑧 2 ) . (17)
By putting cos 𝜃3 in eq. (17) into the first equation in eq. (11) and multiplying both sides by
(𝑝1 − 𝑥)2 , we have
𝑑42 𝑝12 + 𝑑42 𝑝22 − 2(𝑑42 𝑝1 + 𝑑4 𝑑5 𝑝2 )𝑥 + (𝑑42 + 𝑑52 − 𝑝12 )𝑥 2 + 2𝑝1 𝑥 3 − 𝑥 4
− 2𝑑42 𝑝2 𝑦 + 2𝑑4 𝑑5 𝑥𝑦 + (𝑑42 − 𝑝12 )𝑦 2 + 2𝑝1 𝑥𝑦 2 − 𝑥 2 𝑦 2 = 0. (18)
𝑃 −𝑦 𝑝 −𝑥
In the case sin 𝜃5 = −1, perform the same calculation with sin 𝜃1 = 2𝑑 and cos 𝜃1 = 1𝑑 . In
5 5
this case, the sign in eqs. (17) and (18) changes in part, which gives the following equations.
89
1
cos 𝜃3 = (−𝑎22 𝑝12 − 𝑎32 𝑝12 + 𝑝12 𝑑12 + 𝑝22 𝑑42 + 2𝑎22 𝑝1 𝑥 + 2𝑎32 𝑝1 𝑥 − 2𝑝1 𝑑12 𝑥 + 2𝑝2 𝑑4 𝑑5 𝑥
2𝑎2 𝑎3 (𝑝1 − 𝑥)2
− 𝑎22 𝑥 2 − 𝑎32 𝑥 2 + 𝑑12 𝑥 2 + 𝑑52 𝑥 2 − 2𝑝2 𝑑42 𝑦 − 2𝑑4 𝑑5 𝑥𝑦 + 𝑑42 𝑦 2 − 2𝑝12 𝑑1 𝑧 + 4𝑝1 𝑑1 𝑥𝑧 − 2𝑑1 𝑥 2 𝑧
+ 𝑝12 𝑧 2 − 2𝑝1 𝑥𝑧 2 +𝑥 2 𝑧 2 ) , (19)
𝑑42 𝑝12 + 𝑑42 𝑝22 − 2(𝑑42 𝑝1 − 𝑑4 𝑑5 𝑝2 )𝑥 + (𝑑42 + 𝑑52 − 𝑝12 )𝑥 2 + 2𝑝1 𝑥 3 − 𝑥 4
− 2𝑑42 𝑝2 𝑦 − 2𝑑4 𝑑5 𝑥𝑦 + (𝑑42 − 𝑝12 )𝑦 2 + 2𝑝1 𝑥𝑦 2 − 𝑥 2 𝑦 2 = 0. (20)
Furthermore, eq. (18) or eq. (20) together with eqs. (14) and (15), we obtain a system of
polynomial equations in 𝑥, 𝑦, 𝑧. Solving the system (by using Gröbner basis computation, etc.)
gives the position of the intersection point 𝑃.
4. Concluding remarks
In this paper, we have proposed a solution for the inverse kinematic problem of a 6-DOF
manipulator under the condition that the orientation of the end-effector remains constant.
Our first task from here includes the verification of the solution with the CGS-QE method
and efficiently solving the system of polynomial equations by using CGS, as we have proposed in
the previous work. In addition, the orientation was specified this time for simplicity. However,
orientation is not always constant in real-world manipulators. It is therefore necessary to develop
the problem into an inverse kinematics problem for arbitrary orientations. There are several
conditions on the geometry of the 6-DOF manipulator to be analytically solvable [13], and a
method with computer algebra has been proposed for solving the inverse kinematic problem of
the 6-DOF manipulator [3]. We will look for better solutions with reference to these methods.
References
[1] Elephant Robotics, Inc., 2024, mycobot 280, URL: https://www.elephantrobotics.com/en/
mycobot-en/.
[2] M. P. Husty, Manfred L., H.-P. Schröcker, A new and efficient algorithm for the inverse
kinematics of a general serial 6R manipulator, Mehcanism and machine theory 42.1 (2007)
66–81. doi:10.1016/j.mechmachtheory.2006.02.001 .
[3] D. Manocha, J. Canny, Efficient inverse kinematics for general 6r manipulators, IEEE
Transactions on Robotics and Automation 10 (1994) 648–657. doi:10.1109/70.326569 .
[4] J.-C. Faugère, J.-P. Merlet, F. Rouillier, On solving the direct kinematics problem for parallel
robots, Research Report RR-5923, INRIA, 2006. URL: https://hal.inria.fr/inria-00072366.
[5] C. M. Kalker-Kalkman, An implementation of Buchbergers’ algorithm with applications to
robotics, Mech. Mach. Theory 28 (1993) 523–537. doi:10.1016/0094- 114X(93)90033- R .
[6] T. Uchida, J. McPhee, Triangularizing kinematic constraint equations using Gröbner
bases for real-time dynamic simulation, Multibody System Dynamics 25 (2011) 335–356.
doi:10.1007/s11044- 010- 9241- 8 .
[7] T. Uchida, J. McPhee, Using Gröbner bases to generate efficient kinematic solutions for
the dynamic simulation of multi-loop mechanisms, Mech. Mach. Theory 52 (2012) 144–157.
doi:10.1016/j.mechmachtheory.2012.01.015 .
[8] N. Horigome, A. Terui, M. Mikawa, A Design and an Implementation of an Inverse
Kinematics Computation in Robotics Using Gröbner Bases, in: A. M. Bigatti, J. Carette,
J. H. Davenport, M. Joswig, T. de Wolff (Eds.), Mathematical Software – ICMS 2020,
Springer International Publishing, Cham, 2020, pp. 3–13. doi:10.1007/978- 3- 030- 52200- 1_
1.
90
[9] S. Otaki, A. Terui, M. Mikawa, A Design and an Implementation of an Inverse Kinematics
Computation in Robotics Using Real Quantifier Elimination based on Comprehensive
Gröbner Systems, Preprint, 2021. doi:10.48550/arXiv.2111.00384 , arXiv:2111.00384.
[10] M. Yoshizawa, A. Terui, M. Mikawa, Inverse Kinematics and Path Planning of Manipulator
Using Real Quantifier Elimination Based on Comprehensive Gröbner Systems, in: Computer
Algebra in Scientific Computing: CASC 2023, volume 14139 of Lecture Notes in Computer
Science, Springer, 2023, pp. 393–419. doi:10.1007/978- 3- 031- 41724- 5_21 .
[11] B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and Control,
Springer, 2008. doi:10.1007/978- 1- 84628- 642- 1 .
[12] A. A. Brandstötter, Mathias, M. Hofbaur, An analytical solution of the inverse kinematics
problem of industrial serial manipulators with an ortho-parallel basis and a spherical wrist,
in: Proceedings of the Austrian Robotics Workshop 2014, 2014, pp. 7–11.
[13] D. L. Pieper, The kinematics of manipulators under computer control, Stanford University,
1969. URL: https://apps.dtic.mil/sti/citations/AD0680036, accessed 2024-08-05, Ph.D. Thesis.
91