Acta Polytechnica https://doi.org/10.14311/AP.2023.63.0326 Acta Polytechnica 63(5):326–346, 2023 © 2023 The Author(s). Licensed under a CC-BY 4.0 licence Published by the Czech Technical University in Prague GEOMETRIC APPROACH TO SOLVING INVERSE KINEMATICS OF SIX DOF ROBOT WITH SPHERICAL JOINTS Nacer Hadidia,∗, Mohamed Bouaziza, Chawki Mahfoudib, Mohamed Zaharuddinc a Ecole Nationale Polytechnique, Laboratoire de Genie Mecanique et devellopement, Department of Mechanical engineering, 10 Avenue Hassan Badi, 16200 El Harrach, Algeria b University Larbi Ben Mhidi, Campus of AinBeida, Department of Mechanical engineering, Route de Constantine, B.P. 358, 04000 Oum El Bouaghi, Algeria c Universiti Teknologi Malaysia, Faculty of Electrical Engineering, Department of mechatronics, 81310 Johor Bahru, Malaysia ∗ corresponding author: nacer.hadidi@g.enp.edu.dz Abstract. Inverse kinematics is a fundamental concept in robotics that plays a crucial role in a robot’s ability to perform tasks. In this contribution, we propose a novel geometric approach based on vector calculus to solve the inverse kinematics problem. The primary advantage of this approach originates from the solutions, which exhibit a linear form and uncoupled equations. To validate the effectiveness and correctness of our proposed method, we constructed a six-degrees-of-freedom robot. This robot is controlled by an Arduino Mega 2650 on which we have implemented the inverse kinematics algorithm. The validation process involved considering various desired trajectories of the end-effector, which were simulated in Matlab and then performed by the physical robot. Importantly, our findings confirm that the end-effector successfully tracks the predefined trajectories. Furthermore, we conducted a comparative analysis between Paul’s method and the results obtained from joint angles using our proposed approach. Interestingly, our study reveals a significant similarity between the two sets of results, reaffirming the accuracy and validity of the approach presented in this study. Keywords: Robotic, forward kinematics, inverse kinematics, geometric modelling, space geometry. 1. Introduction Due to the interesting and complex nature of field of robotics, several approaches to the research of robots have been taken. One of the challenging tasks is the kinematic modelling (Forward and Inverse Kinematics). Research into the treatment of the robot kinematics continues to spread over different types of robots. In the literature, some works have focused on the serial manipulator robots [1–5]. While others have dealt with parallel robots and hybrid robots’ structure [6, 7]. In addition, some research has focused on specific robot configurations, such as snake robots [8], wheeled robots, and quadruped robots [9, 10]. Forward kinematics (FK) does not pose significant difficulties because it involves a mathematically straight- forward multiplication of successive homogeneous matrices that lead the end-effector to a corresponding position and orientation [11–13]. However, inverse kinematics (IK) entails specifying a precise pose for the robot’s end-effector within its workspace, defined by its position and orientation in Euclidean space R3. In this context, finding a suitable set of joint configurations that enables the end-effector to reach the desired positions is the main challenge. In the literature, Inverse kinematics (IK) problem has been addressed by several authors using different approaches. Basically, these approaches can be categorised into analytical approaches, numerical approaches, and hybrid approaches, using different methods in order to achieve a smooth solution. Where the primary aim of these methods is to provide flexible mobility for the robot, which is dependent on the robot’s geometric structure and the number of degrees of freedom (DOF) [14, 15]. In Euclidian space R3, analytical methods provide closed-form solutions for the inverse kinematics under certain conditions: (1.) When three successive revolute joint axes intersect at the same point; (2.) When three consecutive revolute joint axes are parallel. Mathematically, analytical methods are represented by the following general matrix equation [16–19]: 0T1 1T2 2T3 3T4 4T5 5T6 = T , (1) 326 https://doi.org/10.14311/AP.2023.63.0326 https://creativecommons.org/licenses/by/4.0/ https://www.cvut.cz/en vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot where j−1Tj represents the homogeneous matrix between frames j and j − 1, and T is the matrix representing the desired position of the end-effector. However, solutions to these problems are typically derived through nonlinear and coupled equations that often involve matrix calculations. Several methods have been developed in the literature to address these challenges. Notable approaches include Pieper’s method [16], which is applicable to robots with three articulatory joints featuring concurrent axes or three prismatic joints. Raghavan and Roth [17] have formulated a system of 16 multivariate polynomials for closed-loop solutions, suitable for 6-revolute manipulators (6R). Their solutions are based on successive elimination techniques. Paul’s method [18] involves isolating and solving variables one by one, making it suitable for most industrial manipulator robots. Gan et al. [19] have derived an inverse kinematics (IK) solution for a 5-DOF robotic arm (Pioneer 2), which has similarities to Paul’s method. Other studies have investigated the IK problem of robots with redundancy, providing analytical solutions based on various assumptions related to the redundancy settings [20–22]. Numerous works have addressed the numerical approach to solve the Inverse Kinematics (IK) problem [1– 3, 23–27]. The majority of these solutions have been derived using different methods, including numerical algorithms [2], neural networks [24, 28], genetic algorithms [29], fuzzy logic, and others. Within this context, numerical algorithms have been used to ensure convergence and accuracy. However, it’s important to note that many of the currently available methods are plagued by high computational costs and the generation of unrealistic poses [14]. In addition, some robots with 7 degrees of freedom (7-DOF) have been addressed using numerical approaches, aiming to resolve redundancy issues in the Inverse Kinematics (IK) solutions. Among these, Y. Wenbin and S. Lei [30] have studied an iterative approach combining two techniques: (a) A technique based on “fixed joint angles”, (b) A technique involving a “two-phase computational optimisation algorithm using Weighted Least-Norm”. In addition, B. Ma et al. [31] have presented a hybrid IK solution for 7-DOF robots, based on two analytical techniques that consider redundant parameters to produce approximate solutions, followed by a numerical algorithm that minimises errors to achieve convergence of the solution. This paper introduces a novel approach based on the geometric description of a robot to solve the Inverse Kinematics (IK) problem. The approach is based on vector calculus and trigonometric relations and offers a significant advantage by providing solutions through simple and decoupled linear equations. This eliminates the need to solve the complex IK problem using matrices and numerical methods. To assess the feasibility and accuracy of this approach, we conducted Matlab simulations and an experimental study involving the design and construction of a 6-degree-of-freedom (6-DOF) robot. By instructing the robot’s end-effector to follow predefined trajectories, our approach successfully converted Cartesian coordinates into joint angle coordinates in both the simulation and the experiment. As a result, the robot was able to accurately track the predefined trajectories. We also performed a comparative analysis between our approach and Paul’s method for a similar robot arm, as presented in reference [32]. This comparison revealed significant consistency and similarity in the results, confirming the validity and reliability of our proposed approach. 2. Principle of the approach for inverse kinematics An angle between two given vectors −→ U and −→ V in R3 is defined by its magnitude and its conventional orientation (rotation sense). Mathematically, the magnitude is given by the cosine law. −→ U . −→ V = U.V cos(−→U , −→ V ) , (2) where U and V represent the magnitude of the vectors −→ U and −→ V , respectively. The rotation sense of the angle is described by applying the right hand rule between three vectors: −→ U and −→ V and −→ W . Where −→ W is the cross product of −→ U and −→ V or any other orthonormal vector to the plane formed by −→ U and −→ V (Plane-uv), as illustrated in Figure 1. A helpful relation between the cross product and the determinant can be defined as follows: −→ U ∧ −→ V = U.V sin(−→U , −→ V )−→nW . (3) By multiplying both sides of Equation (3) by the same vector −→nW and dividing by the scalar UV , we obtain a new equality in Equation (4): 1 UV (−→U ∧ −→ V ).−→nW = sin(−→U , −→ V ).(−→nW )2 . (4) 327 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica Figure 1. Right hand rule and vector order. Equation (4) leads to the equality below: sin(−→U , −→ V ) = (−→nU ∧ −→nV .−→nW ) = det(−→nU , −→nV , −→nW ) , (5) where: • −→nU = −→ U U and −→nV = −→ V V are two unit vectors of −→ U and −→ V , respectively, and (−→nW )2 = 1, • det(−→nU , −→nV , −→nW ) = (−→nU ∧ −→nV ).−→nW = nwx nwy nwz nux nuy nuz nvx nvy nvz  is the determinant of the three vector (or mixed product), which can also be developed as folows: det(−→nU , −→nV , −→nW ) = nwx(nuy.nvz − nvy.nuz)− nwy(nux.nvz − nvx.nuz)+ nwz(nux.nvy − nvx.nuy) . An important property of the determinant regarding its sign is obtained by swapping two columns [33]. det(−→W, −→ U , −→ V ) = (−1) det(−→W, −→ V , −→ U ) . This implies that the sign of the determinant changes with variations in angular orientation. The magnitude and direction sense of each joint angle can be determined by substituting these two previous parameters of Equation (2) and Equation (5) as arguments into the mathematical function atan2(x, y), see Appendix A.1. β = atan2(det(−→nU , −→nV , −→nW ), cos(−→U , −→ V )) . (6) 3. Robot structure and geometry Figure 2 shows a right upper limb with 6-DOF of a biped robot distributed as follows: 3-DOF at the shoulder, 1-DOF at the elbow, and 2-DOF at the wrist. These components are interconnected by three geometric links: the arm (La), the forearm (Lf ), and the hand (Lh). Their lengths are summarised in Table 1. Links Symbol Value [m] Arm La 0.32 Forearm Lf 0.25 Hand Lh 0.12 Table 1. Robot link lengths. 328 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot (a). Image of manufactured robot. z0 y0 x0 y1 x1 z1 x2 z2 y2 y3 x3 z3 z4 y4 x4 y5 x5 z5 z6 y6 x6 z7 y7 x7 La Lf Lh (b). Robot’s kinematic chain. Figure 2. Right upper limb of a biped robot and frame assignment according to DH. 4. Mathematical modelling of the robot This section is primarily dedicated to developing the mathematical model that describes the Inverse Kinematics (IK) solutions. The approach outlined below provides the corresponding actuator rotations for each state of the end-effector. The following assumptions have been used in this work: • The shoulder, with its 3 degrees of freedom (3 DOF), has its axes of rotation intersecting at the same point. Consequently, the shoulder’s rotational movement can be treated as a single geometric point. • Similarly, the same assumption has been applied to the wrist, which has 2 DOF. Consequently, the wrist is also treated as a single geometric point. 4.1. Forward kinematics The matrix representing the desired position and orientation, denoted as T = [S N A P], of the end-effector, given in Equation (1) and obtained through successive multiplications of homogeneous transformation matrices, can be rewritten as Equation (7). [S N A P] = 0T1 1T2 2T3 3T4 4T5 5T6 6T7 =  sx nx ax Pdx sy ny ay Pdy sz nz az Pdz 0 0 0 1  , (7) where: S = (sx, sy, sz)T , N = (nx, ny, nz)T , A = (ax, ay, az)T are the cosine directions of the frame F7(x7, y7, z7) (orientation of the end-effector). P = (Pdx, Pdy, Pdz)T is the coordinate of the desired position of the end-effector and i−1Ti is the homogeneous transformation between two successive frames (i = 1, . . . , 7). Table 2 shows parameters obtained using the DH convention where αi is the rotational angle between zi axis and zi−1 axis around xi axis, qi = θi + q0i is the rotational angle between xi axis and xi−1 axis around zi axis where θi represents the effective rotation and q0i represents the initial frame rotation corresponding to the home position. Hence, the homogeneous transformation matrix between two successive frames can be obtained through Equation (8): 329 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica Frame αi Li qi di Joint range F0 – – – – – F1 0 0 π 2 + θ1 0 [ − π 2 , π ] F2 π 2 0 π 2 + θ2 0 [ − π 2 , −π ] F3 − π 2 0 − π 2 + θ3 La [−π, π] F4 − π 2 0 θ4 0 [ 0, 3π 4 ] F5 π 2 0 θ5 Lf [0, 2π] F6 − π 2 0 − π 2 + θ6 0 [ − 3π 4 , 3π 4 ] F7 0 Lh 0 0 – Table 2. DH parameters. i−1Ti =  cos(qi) − sin(qi) 0 Li cos(αi) sin(qi) cos(αi) cos(qi) − sin(αi) −di sin(αi) sin(αi) sin(qi) sin(αi) cos(qi) cos(αi) di cos(αi) 0 0 0 1  . (8) 4.2. Inverse kinematics solutions Solving the inverse kinematics involves determining the appropriate rotations of each robot’s articulation to bring the end-effector to the desired situation (position and orientation). Figure 3 represents a geometric parameterisation of the robot (geometric entities), which will be utilised in the inverse kinematics problem solving process. Y0 X0 Z0 O F6 F7 z7 x7 x6 y7 z6 y6 (a). Robot configuration with assignment of main frames. ௗܲ 0X 0Z 0Y ௘ܲ ௛ܲ ௙ܮ ௦ܲ ௪ܲ ௔ܮ ௛ܮ ܪ ௛௪ܮ ݎ ௛௘ܮ ଺ݖ ଵߙ ଶߙ Q plane Circle of La radius (b). Geometric entities. Figure 3. Geometric parameterisation of a right upper limb biped robot. In this study, the strategy for solving the inverse kinematics problem is divided into two phases: the first phase consists of determining the geometric entities that allow the determination of the vectors defining each joint. The second phase focuses on determining the angles of each joint, which are based on the results obtained in the first phase. 330 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot 4.2.1. Calculate the geometric entities of the robot Initially, certain geometric entities, including points, vectors, and planes, must be determined. To achieve this, the following strategy is adopted: • Determine the wrist position. • Determine the elbow position by the following procedure: ▷ Determine the Q-plane equation through the normal vector at the rotational joint q6 (z6-axis). Geomet- rically, the forearm link has to belong to the Q-plane because for any position and orientation of the end-effector, the z6-axis remains perpendicular to the forearm-link. ▷ Calculate the coordinate of the closest point Ph on Q-plane at the original frame F0. ▷ Determine the circle on the Q-plane in which the wrist poin Pw and the length of forearm (Lf ) are its centre and radius, respectively. ▷ Calculate the coordinate point of elbow (Pe). According to Figure 3, the frame F6 of the rotational joint q6 maintains the same orientation as the frame F7; therefore, the position of the wrist is determined by the following Equation (9), using the given vector, −→ S6 = (s6x, s6y, s6z)T , in th matrix (T) of the end-effector. −−→ OPw = −−→ OPd − Lh −→ S6 , (9) where: −−→ OPw = (Pwx, Pwy, Pwz)T and −−→ OPd = (Pdx, Pdy, Pdz)T are the coordinates of the wrist position and the desired position of the end-effector, respectively. From Figure 3, we derived the tetrahedron form, which consists of two right-angled triangles ∆(Ps, Ph, Pe) and ∆(Ps, Ph, Pw), which are defined as shown in Figure 4. ௘ܲ ௛ܲ ௔ܮ ௦ܲ ௪ܲ ௙ܮ r (a). Initial form. ௔ܮ ௪ܲܮ௙ ௘ܲ ௛ܲ ௦ܲ r ௛௘ܮ ௛௪ܮ ௘ܲ ௛௪ܮ ௛௘ܮ ܪ (b). Decomposed form. Figure 4. Tetrahedron form obtained from the derived geometry space of robot. Ps, Pe, Pw and Ph are the points designating: shoulder, elbow, wrist, and the closest point on Q-plane to the shoulder frame (F0), respectively. From Figure 4, the length between the origin and the wrist can be calculated by Equation (10). r = √ P 2 wx + P 2 wy + P 2 wz , (10) where Pwx, Pwy, Pwz are the coordinates of the vector (−−→ OPw) obtained in Equation (9). Q-plane equation obtained via the normal vector −→ A6 = (ax, ay, az)T of z6-axis of the frame F6 is: axx + ayy + azz − D = 0 , (11) where D = (axPwx + ayPwy + azPwz) is the parameter of the plane equation. From Equation (11), the closest distance, H, between the origin and Q-plane can be calculated as: H = |axPwx + ayPwy + azPwz|√ a2 x + a2 y + a2 z . (12) 331 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica And the corresponding coordinates of the closest point on Q-plane from the origin is given by: Phx = axD a2 x + a2 y + a2 z , Phy = ayD a2 x + a2 y + a2 z , (13) Phz = azD a2 x + a2 y + a2 z . From Figure 4, the length Lhe is defined as the distance between Ph (the closest point to the shoulder frame on Q-plane) and Pe (the elbow position). Similarly, Lhw is defined between points Ph and Pw (the wrist position). Consequently, using the results obtained in Equations (10) to (13), the two lengths, Lhe and Lhw, can be obtained by the following relationships: Lhe = √ L2 f − H2 , (14) Lhw = √ r2 − H2 . (15) Thus, based on Equations (14) and (15) and from the triangle ∆(Pe, Ph, Pw) shown in Figure 3, the angle α1 formed by the points (Pe, P̂w, Ph) can be calculated by the cosine law as: α1 = (Pe, P̂w, Ph) = a cos( L2 hw + L2 f − L2 he 2Lf Lhe ) . (16) In addition, the circle equation of radius Lf and centre point Pw on the Q-plane is defined in Equation (17). −−→ OPc = −−→ OPw + Lf cos(ϕ)−→S6 + Lf sin(ϕ)−→N6 , (17) where −−→ OPc = (Pcx, Pcy, Pcz)T and −−→ OPw = (Pwx, Pwy, Pwz)T are the coordinates of circle’s circumference and circle’s centre respectively. −→ S6 and −→ N6 are the unit vectors of x6-axis and y6-axis of frame F6, respectively, and ϕ = α1 + α2 is the angle between x6-axis and −−→ Vwe (the vector between the wrist and the elbow point). Additionally, the value α1 = (Pe, P̂w, Ph) is calculated by Equation (16) and the α2 magnitude is given by the cosine law: cos(α2) = −→ S6.−−→nwh | −→ S6|−−→nwh| , (18) where −−→nwh and |−−→nwh| are normalised vectors of −−→ Vwh and its modulus, respectively. Referring to Figure 3b, where the vectors −→ S6 and −−→ Vwh (wrist-closest point on the Q-plane to the shoulder frame) are both inside the Q-plane, the orientation of α2 can be determined by the determinant of these three vectors: −→ A6, −−→nwh and −→ S6 as shown in Equation (19): det α2 = det [−→ A6 −−→nwh −→ S6 ]T . (19) Hence, based on Equations (18) and (19), the angle α2 is given by the next equation: α2 = atan2(det α2 , cos(α2)) . (20) Then, the angle ϕ is calculated by the Equations (16) and (20). ϕ = α1 + α2 . (21) Finally substituting Equation (21) in Equation (17), the elbow position is determined by the coordinate’s vector (22): −→ Voe = (Pex, Pey, Pez)T . (22) After determining the necessary geometric entities for calculating the joint angles, we proceed to their identifications. 332 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot 4.2.2. Calculate the joint angles of the robot The geometric vectors needed to calculate the joint angles are obtained in Phase 1. Therefore, the joint angle equations are calculable (βi, i = 1, . . . , 6). Geometrically, each joint angle is described by three vectors (see Figure 5a-5f), two mathematical arguments (arg1, arg2) are expressed which are used in Equation (6) in the form atan2(arg1, arg2). 푛 푛 푛 ⃗ 푃 푦 푥 푧 훽 표 푛 ⃗ 휷ퟏ 푃 푃 푄 (a). β1. 푛 ⃗ 푃 푃 푦 푧 푃 표 훽 푛 ⃗ 푛 ⃗ 휷ퟐ 훾 푥 푄 (b). β2. 푥 푛 ⃗ 휷ퟑ 푃 푃 푦 푧 푛 ⃗ 표 푛 ⃗ 푉 ⃗ 훽 (c). β3. 푃 푃 푦 푧 푃 표 푉 ⃗ 휷ퟒ 푥 훽 푉 ⃗ (d). β4. 퐴⃗ 훽 푃 푃 푦 푥 푧 푛 ⃗ 푛 ⃗ 휷ퟓ 푉 ⃗ 푄 (e). β5. 퐴⃗ 푃 푃 푦 푥 푧 휷ퟔ 푃 푉 ⃗ 푆⃗ (f). β6. Figure 5. Vectorial description of robot’s joint angles. a. Joint angle β1: Figure 5a shows the vectorial description of the joint angle β1. The angle is defined by the normal vector obtained from the cross product of vectors −−→ Voz0 and −→ Voe. −→nq1 is the axis collinear with the rotational joint q2-axis. Furthermore, −→nq1 revolves in a plane parallel to plane Qxoy. Therefore, n1x = cos(−−→ Vox0 ,−−→nq1) |−−→nq1| and n1y = sin(−−→ Vox0 ,−−→nq1) |−−→nq1| , where: −−→ Vox0 and −−→ Voz0 : the unit vectors of x0-axis and z0-axis, respectively, of frame F0, −→nq1: the normalised and normal vector of Q1-plan obtained from cross product −−→ Voz0 ∧ −→ Voe, −→ Voe: the vector between the two points (origin-elbow), Qxoy: the plane defined by two unit vectors of axes, x0-axis and y0-axis. As the normal vector −→nq1 is normalised to a unit vector (|−→nq1| = 1), the equations can be written as: n1x = cos(−−→ Vox0 , −→nq1) and n1y = sin(−−→ Vox0 , −→nq1). Therefore, the joint angle is obtained by formula: β1 = atan2(n1y, n1x) . (23) b. Joint angle β2: Figure 5b shows the vectorial description of the joint angle β2. It is defined by two vectors,−−→ Voz0 and −→ Voe, where, −−→noz0 and −→noe are their normal vectors, respectively. 333 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica From Figure 5b, β2 can be written as: β2 = π 2 − γ2 , (24) where γ2 is expressed by the equation γ2 = atan2(det γ2 , cos(γ2)), where cos(γ2) = −−→ Voz0 .−→noe and det γ2 = det [−→nq1 −−→noz0 −→noe]T in which −−→noz0 = (0 0 1)T , −→noe = (nex ney nez)T and −→nq1 = (n1x n1y 0)T . c. Joint angle β3: The joint angle β3 is the angle between the two planes Q1-plane (defined by vectors −−→ Voz0 and −→ Voe) and Q2-plane (defined by the three points of the robot’s links: shoulder-elbow-wrist) as shown in Figure 5c, where −→nq1 and −→nq2 are their normal vectors, respectively. Thus, β3 can be further expressed as follows: β3 = atan2(det β3 , cos(β3)) , (25) where cos(β3) = −→nq1.−→nq2 and the determinant is: det β3 = det [−→noe −→nq1 −→nq2]T . d. Joint angle β4: The joint angle β4 is an angle between −→ Voe (arm) and −−→ Vew (forearm) as shown in Figure 5d. −→noe and −−→new are the normalised vectors of −→ Voe and −−→ Vew, respectively. The angle can be calculated by the formula below: β4 = atan2(det β4 , cos(β4)) , (26) where the cos(β4) = −→noe.−−→new and the determinant of β4 can be obtained by: det β4 = det [−→nq1 −→noe −−→new]T . e. Joint angle β5: The joint angle β5 is defined as the angle between the vectors −→nq2 and −→ A6 = (ax, ay, az)T of the z6-axis as shown in Figure 5e. So the arguments of β5 can be expressed by cos(β5) = −→nq2. −→ A6 and det β5 = det [−−→ Vwe −→nq2 −→ A6 ]T . Finally, β5 can be expressed by Equation (27): β5 = atan2(det β5 , cos(β5)) . (27) f. Joint angle β6: The joint angle β6 is the angle between the forearm and the hand of the robot, which is defined by the vectors −−→ Vew (elbow-wrist) and −−→ Vwd (wrist-desired position), respectively, see Figure 5f. Therefore, β6 is the angle between two vectors: −−→new = 1 | −−→ Vew| −−→ Vew and −→ S6 = (sx, sy, sz)T . Thus, the two arguments of β6 are: cos(β6) = −−→new. −→ S6 and det β6 = det [−→ A6 −−→new −→ S6 ]T . Consequently, β6 can be obtained by the following formula: β6 = atan2(det β6 , cos(β6)) . (28) 5. Simulations and results In this study, simulations have been performed to verify the performance of the proposed approach for solving the Inverse Kinematics (IK) problem. To achieve this, a program was developed using MATLAB based on the IK results presented in Equations (23) to (28) for the simulation case. Figure 6 illustrates a freely chosen predefined path for the end-effector. The robot should therefore be able to follow these trajectories accurately. Each part of the end-effector’s path has been characterised as an arc of a circle defined by a set of Cartesian coordinates. Only the path’s part 9→10 is a circular trajectory defined by its equation (see Appendix A.2). As mentioned in Table 3, the robot’s end-effector will start from the point with coordinates (0,-0.6,0) m, following the trajectories indicated by the following numbered points order: P0 → P1 → P2 → P3 → P4 → P5 → P6 → P7 → P8 → P9 → P10 → P11 The orientation of the end-effector is expressed by a given matrix as shown below: [S N A] =  0 1 0 −1 0 0 0 0 1  . 334 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot Figure 6. Predefined trajectories of end-effector’s path. Points Coordinates (x, y, z) [m] P0 (0.00 -0.60 0.00) P1 (0.00 -0.59 0.12) P2 (0.40 -0.45 0.12) P3 (0.00 -0.59 0.12) P4 (0.00 -0.60 0.00) P5 (0.23 -0.50 0.23) P6 (0.32 -0.01 0.32) P7 (0.23 -0.50 0.23) P8 (0.00 -0.60 0.00) P9 (0.00 -0.59 0.12) P10 (0.00 -0.59 0.12) P11 (0.00 -0.60 0.00) Table 3. Coordinate points of each limit path section. A simulation is used to assess the effectiveness of the current method. Specifically, the angles obtained for the six joint articulations using the current method are simulated using Forward Kinematics (FK) equations within the developed MATLAB program. The robot’s end-effector demonstrates the capability to accurately follow the prescribed trajectories, as shown in Figure 7. From Figure 7, it can be seen that the robot’s end-effector has followed exactly the predefined trajectory that is divided into ten paths as numbered above in Figure 6. 335 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica −0.2 −0.1 0 −0.5 −0.4 −0.3 −0.2 −0.1 0 0 0.05 0.1 z[m] x[m] y[ m ] (a). Forward P0 → P1, P8 → P9 (Returned P3 → P4, P10 → P11). −0.2 0 0.2 0.4 −0.5 −0.4 −0.3 −0.2 −0.1 0 0 0.050.1z[m]x[m] y[ m ] (b). Forward P1 → P2 (Returned P2 → P3). −0.2 0 0.2 0.4 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0 0.10.2 0.3 z[m] x[m] y[ m ] (c). Forward P4 → P5 → P6 (Returned P6 → P7 → P8). −0.2 0 0.2 −0.5 −0.4 −0.3 −0.2 −0.1 0 0 0.1 0.2 z[m] x[m] y[ m ] (d). Movement P9 → P10. Figure 7. MATLAB simulations of the robot’s end-effector following predefined trajectories. 6. Experiments In this study, experiments have been conducted to demonstrate the applicability of the proposed approach for solving the Inverse Kinematics (IK) problem. In order to achieve this objective, by exploring the results of IK solutions given in Section 4, an Arduino code has been developed and implemented into an Arduino board 2650. This board serves as an Input/output device between the computer and the robot’s actuators, as shows in the schematic circuit of the control and connection between devices in Figure 8. Tools and materials for the experiment: The design of the robot was created using SolidWorks software and its parts were 3D printed using a 3D printer. The robot is equipped with six stepper motors at the joint articulations, each controlled by a dedicated motor driver. The control of these motors is managed by an Arduino board Mega 2650, which is programmed accordingly. The setup also includes a Ramps 1.4 board and power supplies that provides both 12 V and 24 V. 336 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot Figure 8. Schematic circuit of control and connection between devices. Figure 8 shows a schematic diagram of the main devices used to control the robots and their corresponding wiring. As part of further validation, we deliberately instructed the robot to return along the same path in certain cases. This included cases like P1 → P2 and P2 → P3, as well as P5 → P6 and P6 → P7. In practice, the angle equations of the six joint articulations derived from the current method were directly used in the programmed Arduino code through the Mega2560 control board, after configuring the relevant robot parameters. Based on the experimental observation, it’s evident that the robot can successfully follow the predefined trajectories. Figure 9 displays various poses of the robot as it tracks the trajectories assigned to the end-effector. The six motors of the robot effectively respond to the signals generated by the Inverse Kinematics (IK) solution. Consequently, the IK solutions obtained are validated. However, it’s worth noting that in this case, no position control system is implemented on the joint articulations. As a result, slight fluctuations in position and orientation of the end-effector can be observed in some instances. Figure 10 illustrates the variations in the six articulations of the robot as it moves along the ten path segments. It can clearly be seen that the variations have regularities, notably in the case where the robot returned along the same path (P1→P2 with P2→P3, P0→P1 with P3→P4,. . . ). Also, in the case of the circular path (P9→P10), each of the six motors start and end at the same points. In fact, there is a compatibility between the angular variations and the parts of the trajectories as suggested in Figure 6, and further confirmation is deduced when the robot comes back on the same path. For example, in the case when the robot followed the path from P5 → P6 and retraced the same way defined in the graph via points P6 → P7. It is evident from the graph that the six joint angle variations exhibit perfect symmetry with respect to point P6. This indicates that the robot retraced the path with agreat precision. This proves the compatibility between variations of articulations and the predefined trajectories as suggested in Figure 6. 337 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica Home position Movement P1P2P3 Movement P4P5 and (P7P8) Movement P5P6P7 Movement P9P10 (a). Front view. Home position Movement P1P2P3 Movement P4P5 and (P7P8) Movement P5P6P7 Movement P9P10 (b). Side view.  Home position Movement P1P2P3 Movement P4P5 and (P7P8) Movement P5P6P7 Movement P9P10   (c). Top view. Figure 9. Different postures of the robot following predefined trajectories. Figure 10. Graphs of the variations of the robot’s joint angles. 7. Comparison and discussion Paul’s method is recognised as one of the most effective analytical approaches for solving the Inverse Kinematics (IK) problem in robotics. It starts with the Forward Kinematics (FK) model as given in Equation (7), which leads to a system of nonlinear and coupled equations. For example, the IK problem for a robot with a 6-DOF 338 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot configuration similar to ours has been studied and presented in [16] and [32] using Paul’s method. Here are the initial key results of the generated system of equations, as shown in Equations (29). S1.S2.S3.C4.C5.C6 + C1.C3.C4.C5.S6 − S1.C2.S4.C5.S6 + S1.S2.C3.S5.S6− C1.S3.S5.S6 + S1.S2.S3.S4.C6 + C1.C3.S4.C6 + S1.C2.C4.C6 = sx , S1.S2.S3.C4.C5.C6 + C1.C3.C4.C5.C6 − 21.C2.S4.C5.C6 + S1.S2.C3.S5.C6− C1.S3.S5.C6 − S1.S2.S3.S4.S6 − C1.C3.S4.S6 − S1.C2.C4.S6 = nx , − S1.S2.S3.C4.S5 − C1.C3.C4.S5 − S1.C2.S4.S5 + S1.S2.C3.C5 − C1.S3.C5 = ax , La.S1.S2.S3.S4 + La.C1.C3.S4 + La.S1.C2.S4 + Lf .S1.C2 = sy , C1.S2.S3.C4.C5.C6 + S1.C3.C4.C5.S6 + C1.C2.S4.C5.S6 − C1.S2.C3.S5.S6− S1.S3.S5.S6 − C1.S2.S3.S4.C6 + S1.C3.S4.C6 − C1.C2.C4.C6 = ny , − C1.S2.S3.C4.C5.C6 + S1.C3.C4.C5.C6 + C1.C2.S4.C5.C6 − C1.S2.C3.S5.C6− S1.S3.S5.C6 + C1.S2.S3.S4.S6 − S1.C3.S4.S6 + C1.C2.C4.S6 = ay , C1.S2.S3.C4.S5 − S1.C3.C4.S5 − C1.C2.S4.S5 − C1.S2.C3.C5 − S1.S3.C5 = sz , − La.C1.S2.S3.S4 + La.S1.C3.S4 − La.C1.C2.C4 − Lf .C1.C2 = nz , C2.S3.C4.C5.C6 + S2.S4.C5.S6 + C2.C3.S5.S6 + C2.S3.S4.C6 − S2.C4.C6 = az , C2.S3.C4.C5.C6 + S2.S4.C5.C6 + C2.C3.S5.C6 − C2.S3.S4.S6 + S2.C4.S6 = Pdx , − C2.C3.C4.S5 − S2.S4.S5 + C2.C3.C5 = Pdy , − La.C2.S3.S4 − La.S2.C4 − Lf .S2 = Pdz , (29) where Si and Ci are sin(θi) and cos(θi) for i = 1, . . . , 6, respectively. To obtain solutions for the articular space variables (θ1, θ2, θ3, θ4, θ5, θ6), Paul’s method relies on complex mathematical operations and rearrangements to isolate and identify these variables one by one [16]. How do we obtain the solutions when comparing the two methods and evaluating the Inverse Kinematics (IK) problem? We observe that the solutions of the present approach originate from simple linear equations. In contrast, Paul’s method obtains solutions from more complex equations. This distinction becomes evident when considering Equations (23) to (28) for the present approach and Equations (29) for Paul’s method. Figure 11 provides information on the errors in position for each point (defined by its coordinates in the Cartesian coordinate system: x, y, z) of the end-effector. By using the results of the end-effector configurations within the work space that are generated through the FK equations for both methods (Paul’s method and the current approach) and by calculating the corresponding errors in the coordinate position of the end-effector ( xerror, yerror, zerror) during the time for all paths, as shown in Figure 11. We find that the errors are exceptionally negligible, with the largest error value estimated to be in the order of 1×10−15 m. (a). x position error. (b). y position error. (c). z position error. Figure 11. Comparison of end-effector position between Paul’s method and current approach. Consequently, it is evident that the current approach has significant accuracy and advantage in obtaining the solutions to the IK problem. The elapsed time in Matlab was also taken into account for both methods as shown in Figure 12. From Figure 12, we can see the time elapsed by the two methods separately in each portion of the trajectory, and we can also deduce the total time elapsed during all the robot’s movements. The total elapsed time in Paul’s method and the vectorial approach is 1.59 s and 1.86 s, respectively. From these results, it can be concluded that Paul’s method is slightly quicker than the vectorial method. This 339 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica 0, 05 49 53 54 3 se c 0, 23 37 71 00 2 se c 0, 22 92 12 22 3 se c 0, 05 27 42 48 8 se c 0, 18 12 46 67 6 se c 0, 14 34 82 97 se c 0, 14 63 08 59 2 se c 0, 18 37 23 90 1 se c 0, 05 29 48 70 8 se c 0, 26 66 05 18 3 se c 0, 05 17 38 65 se c 0, 06 82 59 33 9 se c 0, 26 03 20 00 5 se c 0, 25 46 22 97 3 se c 0, 07 20 41 02 7 se c 0, 20 29 42 14 6 se c 0, 17 80 21 66 1 se c 0, 18 06 90 99 5 se c 0, 19 73 92 60 4 se c 0, 07 53 72 87 3 se c 0, 30 09 51 45 6 se c 0, 07 11 57 68 4 se c T R A J 0 -- 1 T R A J 1 -- 2 T R A J 2 -- 3 T R A J 3 -- 4 T R A J 4 -- 5 T R A J 5 -- 6 T R A J 6 -- 7 T R A J 7 -- 8 T R A J 8 -- 9 T R A J 9 -- 1 0 T R A J 1 0 -- 1 1 Paul's method Vectorial approach Figure 12. Elapsed time [sec] required for the robot’s IK solution tracking the path in Matlab. difference in time comes from the necessity of pre-calculating specific geometric parameters for each situation of the end-effector, which are utilised in the equations that provide the solutions for the joints (angles) of the robot. From the simulations and the experimental results, we can conclude that the present approach is a powerful asset for a geometric solution of the inverse kinematics problem. 8. Conclusion This study proposes an alternative approach to solving the inverse kinematics problem. The approach is based on vector calculus using spatial geometry and trigonometric relationships. The validity of the approach was confirmed on a 6-DOF robot. By utilising the geometrical lengths of the robot and the given matrix of the desired position along with certain geometrical relationships, all the joint angles were calculated. A major advantage of the proposed approach is that it treats the inverse kinematic problem separately for each joint angle, and the resulting equations have a linear form, which simplifies the mathematical model. Experimental studies were carried out on a designed and manufactured robot. In addition, simulations were performed to test and validate the effectiveness and correctness of the approach. The joint angles were obtained and utilised for various desired trajectories. In both scenarios (experimental and simulation), the end-effector successfully followed the desired trajectories, confirming the accuracy of the suggested approach. Given these advantages, the approach developed in this work for solving the inverse kinematics problem represents an alternative solution that could find widespread use in various robotic structures. List of symbols DH Denavit Hartemberg convention F K Forward Kinematics IK Inverse Kinematics DOF Degrees of freedom La, Lf andLh Geometrical lengths of the robot, namely: arm, forearm and hand [m] Fi(i = 1, . . . , 7) Reference Frame T = [S N A P ] Matrix (4×4) of the desired orientation and position of the End-effector −−→ OPw = (Pwx, Pwy, Pwz)T Vector of wrist-position −−→ OPd = (Pdx, Pdy, Pdz)T Vector of desired position (end-effectors) −→ Vse Vector defined by two points (shoulder-elbow) −−→ Vwh Vector defined by two points (wrist-h: the nearest point on the Q-plane to origin) −→nseand−−→nwh Normalised vectors of −→ Vse and −−→ Vwh, respectively 340 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot ∆(Ps, Ph,e ) Triangle of three points Q − plane Plane of normal vector −→nq det[−→A6 −−→nwh −→ S6] Determinant of three vectors (Pe, P̂w, Ph) Angle by three points in which the midpoint is its vertex SiandCifori = 1, . . . , 6 sin(θi) and cos(θi), respectively −→ U . −→ V Dot product of vectors −→ U and −→ V −→ U ∧ −→ V Cross product of vectors −→ U and −→ V References [1] H. Ren, P. Ben-Tzvi. Learning inverse kinematics and dynamics of a robotic manipulator using generative adversarial networks. Robotics and Autonomous Systems 124(8):379–386, 2020. https://doi.org/10.1016/j.robot.2019.103386 [2] M. Alebooyeh, R. J. Urbanic. Neural network model for identifying workspace, forward and inverse kinematics of the 7-DOF YuMi 14000 ABB collaborative robot. IFAC-PapersOnLine 52(10):176–181, 2019. https://doi.org/10.1016/j.ifacol.2019.10.019 [3] J. Li, H. Yu, N. Y. Shen, et al. A novel inverse kinematics method for 6-DOF robots with non-spherical wrist. Mechanism and Machine Theory 157:104180, 2021. https://doi.org/10.1016/j.mechmachtheory.2020.104180 [4] M. Toz. Chaos-based Vortex Search algorithm for solving inverse kinematics problem of serial robot manipulators with offset wrist. Applied Soft Computing 89(C):106074, 2020. https://doi.org/10.1016/j.asoc.2020.106074 [5] Z. Liao, G. Jiang, F. Zhao, et al. A novel solution of inverse kinematic for 6R robot manipulator with offset joint based on screw theory. International Journal of Advance Robotic Systems 17(3):1–12, 2020. https://doi.org/10.1177/1729881420925645 [6] J. S. Toquica, P. S. Oliveira, W. S. R. Souza, et al. An analytical and a Deep Learning model for solving the inverse kinematic problem of an industrial parallel robot. Computers & Industrial Engineering 151:106682, 2021. https://doi.org/10.1016/j.cie.2020.106682 [7] H. Ye, D. Wang, J. Wu, et al. Forward and inverse kinematics of a 5-DOF hybrid robot for composite material machining. Robotics and Computer-Integrated Manufacturing 65(1):101961, 2020. https://doi.org/10.1016/j.rcim.2020.101961 [8] O. M. Omisore, S. Han, L. Ren, et al. Deeply-learnt damped least-squares (DL-DLS) method for inverse kinematics of snake-like robots. Neural Networks 107(2):34–47, 2020. https://doi.org/10.1016/j.neunet.2018.06.018 [9] M. LiBretto, Y. Qiu, E. Kim, et al. Singularity-free solutions for inverse kinematics of degenerate mobile robots. Mechanism and Machine Theory 153(6):103988, 2020. https://doi.org/10.1016/j.mechmachtheory.2020.103988 [10] M. U. Atique, R. I. Sarker, A. R. Ahad. Development of an 8DOF quadruped robot and implementation of inverse kinematics using Denavit-Hartenberg convention. Helyion 4(12):e01053, 2018. https://doi.org/10.1016/j.heliyon.2018.e01053 [11] A. El-Sherbiny, M. A. Elhosseini, A. Y. Haikal. A comparative study of soft computing methods to solve inverse kinematics problem. Ain Shams Engineering Journal 9(4):2535–2548, 2018. https://doi.org/10.1016/j.asej.2017.08.001 [12] S. Starke, N. Hendrich, J. Zhang. A forward kinematics data structure for efficient evolutionary inverse kinematics. Mechanisms and Machine Science 50:560–568, 2017. https://doi.org/10.1007/978-3-319-60867-9_64 [13] Y. Lin, H. Min. Inverse kinematics of modular manipulator robot with shoulder offset based on geometric method mixed with analytical method algorithm. In IEEE International Conference on Cyber Technology in Automation, Control, and Intelligent Systems (CYBER), pp. 1198–1203. 2015. https://doi.org/10.1109/CYBER.2015.7288114 [14] A. Aristidou, J. Lasenby. FABRIK: A fast, iterative solver for the Inverse Kinematics problem. Graphical Models 73(5):243–260, 2011. https://doi.org/10.1016/j.gmod.2011.05.003 [15] W. Suleiman, F. Kanehiro, E. Yoshida. Infeasibility-free inverse kinematics method. In IEEE/SICE International Symposium on System Integration (SII), pp. 307–312. 2015. https://doi.org/10.1109/SII.2015.7404996 [16] W. Khalil, E. Dombre. Transformation matrix between vectors, frames and screws. In Modeling, identification and control of robots, pp. 13–34. Butterworth-Heinemann, London, UK, 2002. https://doi.org/10.1016/B978-190399666-9/50002-6 [17] M. Raghavan, B. Roth. Inverse kinematics of the general 6R manipulator and related linkages. Journal of Mechanical Design 115(3):502–508, 1993. https://doi.org/10.1115/1.2919218 [18] Y. Lou, P. Quan, H. Lin, et al. A closed-form solution for the inverse kinematics of the 2n-DOF hyper-redundant manipulator based on general spherical joint. Applied Sciences 11(3):1277–1285, 2021. https://doi.org/10.3390/app11031277 [19] J. Q. Gan, E. Oyama, E. M. Rosales, H. Hu. A complete analytical solution to the inverse kinematics of the Pioneer 2 robotic arm. Journal Robotica 23(1):123–129, 2005. https://doi.org/10.1017/S0263574704000529 341 https://doi.org/10.1016/j.robot.2019.103386 https://doi.org/10.1016/j.ifacol.2019.10.019 https://doi.org/10.1016/j.mechmachtheory.2020.104180 https://doi.org/10.1016/j.asoc.2020.106074 https://doi.org/10.1177/1729881420925645 https://doi.org/10.1016/j.cie.2020.106682 https://doi.org/10.1016/j.rcim.2020.101961 https://doi.org/10.1016/j.neunet.2018.06.018 https://doi.org/10.1016/j.mechmachtheory.2020.103988 https://doi.org/10.1016/j.heliyon.2018.e01053 https://doi.org/10.1016/j.asej.2017.08.001 https://doi.org/10.1007/978-3-319-60867-9_64 https://doi.org/10.1109/CYBER.2015.7288114 https://doi.org/10.1016/j.gmod.2011.05.003 https://doi.org/10.1109/SII.2015.7404996 https://doi.org/10.1016/B978-190399666-9/50002-6 https://doi.org/10.1115/1.2919218 https://doi.org/10.3390/app11031277 https://doi.org/10.1017/S0263574704000529 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica [20] B. Tondu. A closed-form inverse kinematic modelling of a 7R anthropomorphic upper limb based on a joint parametrization. In 6th IEEE-RAS International Conference on Humanoid Robots, pp. 390–397. 2006. https://doi.org/10.1109/ICHR.2006.321302 [21] S. Bertrand, O. Bruneau, F. B. Ouezdou, S. Alfayad. Closed-form solutions of inverse kinematic models for the control of a biped robot with 8 active degrees of freedom per leg. Mechanism and Machine Theory 49:117–140, 2012. https://doi.org/10.1016/j.mechmachtheory.2011.10.014 [22] L. Sardana, M. K. Sutar, P. M. Pathak. A geometric approach for inverse kinematics of a 4-link redundant In-Vivo robot for biopsy. Robotics and Autonomous Systems 61(12):1306–1313, 2013. https://doi.org/10.1016/j.robot.2013.09.001 [23] I. Sancaktar, B. Tuna, M. Ulutas. Inverse kinematics application on medical robot using adapted PSO method. Engineering Science and Technology, an International Journal 21(5):1006–1010, 2018. https://doi.org/10.1016/j.jestch.2018.06.011 [24] L. Sciavicco, B. Siciliano. A dynamic solution to the inverse kinematic problem for redundant manipulators. In IEEE International Conference on Robotics and Automation, vol. 4, pp. 1081–1087. 1987. https://doi.org/10.1109/ROBOT.1987.1087921 [25] G. Tevatia, S. Schaal. Inverse kinematics for humanoid robots. In IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), vol. 1, pp. 294–299. 2000. https://doi.org/10.1109/ROBOT.2000.844073 [26] R. Pérez-Rodríguez, A. Marcano-Cedeño, U. Costa, et al. Inverse kinematics of a 6 DoF human upper limb using ANFIS and ANN for anticipatory actuation in ADL-based physical neurorehabilitation. Expert Systems with Applications 39(10):9612–9622, 2012. https://doi.org/10.1016/j.eswa.2012.02.143 [27] F. Chapelle, P. Bidaud. A closed form for inverse kinematics approximation of general 6r manipulators using genetic programming. In IEEE International Conference on Robotics and Automation (Cat. No.01CH37164), vol. 4, pp. 3364–3369. 2001. https://doi.org/10.1109/ROBOT.2001.933137 [28] M. Jin, L. Qiang, B. Wang, H. Liu. An efficient and accurate inverse kinematics for 7-DOF redundant manipulators based on a hybrid of analytical and numerical method. IEEE Access 8:16316–16330, 2020. https://doi.org/10.1109/ACCESS.2020.2966768 [29] W. Yan, H. Lu-Bin, Y. Ting-Li. Inverse kinematics analysis of general 6R serial robot mechanism based on Groebner base. Frontiers of Mechanical Engineering in China 1:115–124, 2006. https://doi.org/10.1007/s11465-005-0022-7 [30] Y. Wenbin, S. Lei. An optimization method for inverse kinematics of a 7-DOF redundant manipulator. In 34th Chinese Control Conference (CCC), pp. 4472–4479. 2015. https://doi.org/10.1109/ChiCC.2015.7260331 [31] B. Ma, Z. Xie, Z. Jiang, H. Liu. Precise semi-analytical inverse kinematic solution for 7-DOF offset manipulator with arm angle optimization. Frontiers of Mechanical Engineering 16:435–450, 2021. https://doi.org/10.1007/s11465-021-0630-x [32] R. O’Flaherty, P. Vieira, M. Grey, et al. Kinematics and inverse kinematics for humanoid robot HUBO2+.GT-GOLEM-001. Technical report, Georgia Institute of Technology, 2013. [33] T. Lyche. A short review of linear algebra. In Numerical linear algebra and matrix factorizations, vol. 22, pp. 1–24. Springer, Chamonix, CH, 2020. https://doi.org/10.1007/978-3-030-36468-7_1 342 https://doi.org/10.1109/ICHR.2006.321302 https://doi.org/10.1016/j.mechmachtheory.2011.10.014 https://doi.org/10.1016/j.robot.2013.09.001 https://doi.org/10.1016/j.jestch.2018.06.011 https://doi.org/10.1109/ROBOT.1987.1087921 https://doi.org/10.1109/ROBOT.2000.844073 https://doi.org/10.1016/j.eswa.2012.02.143 https://doi.org/10.1109/ROBOT.2001.933137 https://doi.org/10.1109/ACCESS.2020.2966768 https://doi.org/10.1007/s11465-005-0022-7 https://doi.org/10.1109/ChiCC.2015.7260331 https://doi.org/10.1007/s11465-021-0630-x https://doi.org/10.1007/978-3-030-36468-7_1 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot A. Appendicies A.1. Trigonometric function atan2(y, x) atan2(y, x), it is a helpful trigonometric function utilized especially by the most computer languages. Because, it is summarizing the four quadrants of trigonometric circle and consequently, gives the corresponding angles of any quadrant. Basically, it is works with polar coordinates by converting the cartesian coordinates (x, y) to polar coordinates (r, θ) which mean that r = √ x2 + y2 and θ = atan2(y, x), x = r. cos θ and y = r. sin θ. θ = atan2(y, x) =  atan(y, x) if x > 0 atan(y/x) + π if x < 0 and y ≥ 0 atan(y/x) − π if x < 0 and y < 0 π/2 if x = 0 and y > 0 −π/2 if x = 0 and y < 0 undefined if x = 0 and y = 0 343 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica A.2. Path of the robot’s end-effector Table below contains the ten parts of path that tracked by the robot’s end-effector. Reverse Path Coordinates path Coordinates Remakes 0→1 Pdx = 0.0 3→4 Pdx = 0.0 Pdz increments Pdy = − √ 0.62 − P 2 dz 10→11 Pdy = − √ 0.62 − P 2 dz (decrements) by a constant Pdz = 0.0 : 0.02 : 0.12 (1→0) Pdz = 0.12 : −0.02 : 0.0 step of 0.02 m 1→2 Pdx = 0.0 : 0.02 : 0.4 2→3 Pdx = 0.4 : −0.02 : 0.0 The value 0.59 m in Pdy Pdy = − √ 0.592 − P 2 dx 2→1 Pdy = − √ 0.592 − P 2 dx represents the last value of Pdz = 0.12 Pdz = 0.12 path 0→1 4→5 Pdx = 0.6 sin α + 0.6 cos π 4 7→8 Pdx = 0.6 sin α + 0.6 cos π 4 π ≤ α < 3π/4 Pdy = −0.6 cos α 5→4 Pdy = −0.6 cos α With a step of: Pdz = 0.6 sin α + 0.6 sin π 4 Pdz = 0.6 sin α + 0.6 sin π 4 ∆α = π 60 rad 5→6 Pdx = Pdx + ∆Pdx 8→9 Pdx = Pdx − ∆Pdx Pdx starts from last value of path 4→5 until Pdx = 0.3 m. Pdy = −0.6 cos α (6→5) Pdy = −0.6 cos α same thing to Pdz. For Pdy: 3π 4 < α < π/2 Pdz = Pdz + ∆Pdz Pdz = Pdz − ∆Pdz and step: ∆α = π 60 rad 9→10  PCx = QOx + R cos(ϕ)SQx + R sin(ϕ)NQx PCy = QOy + R cos(ϕ)SQy + R sin(ϕ)NQy PCz = QOz + R cos(ϕ)SQz + R sin(ϕ)NQz Circularepath which is located on a plane of equation: 0.19x − 0.94y + 0.19z− R = 0.12 m: raidus of circle 0.365 = 0 Qo = (0.12, −0.58, 0.12): center coordinates of circle And normal vector: −→ SQ = (0.65, 0.00, −0.65)T −→n (0.19, −0.94, 0.19) −→ NQ = (0.65, 0.23, 0.65)T 0 ≤ ϕ ≤ 2π of step −→ SQ = (SQx, SQy, SQz)T and −→ NQ = (NQx, NQy, NQz)T are the direction ∆ϕ = π 60 radcosines of the frame axes belong the C-plane in the reference frame F0 344 vol. 63 no. 5/2023 Geometric approach to solving inverse kinematics of six DOF robot A.3. Inverse kinematics solutions using Paul’s method Equation (A1) represents the forward kinematics of the robot. It is a successive multiplication of differents homogeneous transformation matrices. 0T1 1T2 2T3 3T4 4T5 5T6 6T7 = [S N A P] =  sx nx ax Pdx sy ny ay Pdy sz nz az Pdz 0 0 0 1  . (A1) Calculate the inverse of Equation (A1): 7T6 6T5 5T4 4T3 3T2 2T1 1T0 =  s1 n1 a1 P1 s2 n2 a2 P2 s3 n3 a3 P3 0 0 0 1  , (A2) where: s1 = (a2.n3 − a3.n2) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , s2 = −(a2.s3 − a3.s2) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , s3 = (n2.s3 − n3.s2) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , n1 = −(ax.sz − az.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , n2 = (ax.sz − az.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , n3 = −(nx.sz − nz.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , a1 = (ax.nz − az.nx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , a2 = −(ax.sy − ay.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , a3 = (nx.sz − nz.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , P1 = −(Pdx.ay.nz − Pdx.az.ny − Pdy.ax.nz + Pdy.az.nx + Pdz.ax.ny − Pdz.ay.nx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , P2 = (Pdx.ay.sz − Pdx.az.sy − Pdy.ax.sz + Pdy.az.sx + Pdz.ax.sy − Pdz.ay.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) , P3 = −(Pdx.ny.sz − Pdx.nz.sy − Pdy.nx.sz + Pdy.nz.sx + Pdz.nx.sy − Pdz.ny.sx) (ax.ny.sz − ax.nz.sy − ay.nx.sz + ay.nz.sx + az.nx.sy − az.ny.sx) . The corresponding solutions of inverse kinematics are given bellow. A.3.1. The joint angle θ4 cos θ4 = (P1 + Lh)2 + P 2 2 + P 2 3 − L2 a − L2 f 2.La.Lf , θ4 = atan2( √ 1 − cos θ2 4, cos θ4) . A.3.2. The joint angle θ5 cos θ5 = −P3 La. sin θ4 , θ5 = atan2(sin θ5, √ 1 − cos θ2 5) . 345 N. Hadidi, M. Bouaziz, C. Mahfoudi, M. Zaharuddin Acta Polytechnica A.3.3. The joint angle θ6 sin θ6 = La. sin θ4. cos θ5.(Lh + P1) + (La. cos θ4 + Lf )P2 P 2 2 + (P1 + Lh)2 , cos θ6 = La. sin θ4. cos θ5.P2 − (La. cos θ4 + Lf )(Lh + P1) P 2 2 + (P1 + Lh)2 . A.3.4. The joint angle θ2 sin θ2 = −((− sin θ6.a1 − cos θ6.a2) cos θ5 + sin θ4. sin θ5.a3 + cos θ4(cos θ6.a1 − sin θ6.a2)) , θ2 = atan2(sin θ2, √ 1 − sin θ2 2) . A.3.5. The joint angle θ3 sin θ3 = cos θ4(cos θ5(sin θ6.a1 + cos θ6.a2) − sin θ5.a3) + sin θ4(cos θ6.a1 − sin θ6.a2) , cos θ3 = sin θ5(sin θ6.a1 + cos θ6.a2) + sin θ5.a3 , θ3 = atan2(sin θ3, cos θ3) . A.3.6. The joint angle θ1 sin θ1 = sin θ4(− cos θ5(sin θ6.s1 + cos θ6.s2) + sin θ5.s3) + cos θ4(cos θ6.s1 − sin θ6.s2) , cos θ1 = − sin θ4(− cos θ5(sin θ6.n1 + cos θ6.n2) + sin θ5.n3) + cos θ4(cos θ6.n1 − sin θ6.n2) , θ1 = atan2(sin θ1, cos θ1) . 346 Acta Polytechnica 63(5):326–346, 2023 1 Introduction 2 Principle of the approach for inverse kinematics 3 Robot structure and geometry 4 Mathematical modelling of the robot 4.1 Forward kinematics 4.2 Inverse kinematics solutions 4.2.1 Calculate the geometric entities of the robot 4.2.2 Calculate the joint angles of the robot 5 Simulations and results 6 Experiments 7 Comparison and discussion 8 Conclusion List of symbols References A Appendicies A.1 Trigonometric function atan2(y,x) A.2 Path of the robot's end-effector A.3 Inverse kinematics solutions using Paul's method A.3.1 The joint angle 4 A.3.2 The joint angle 5 A.3.3 The joint angle 6 A.3.4 The joint angle 2 A.3.5 The joint angle 3 A.3.6 The joint angle 1