D

Journal of Control Science and Engineering 2 (2014) 7-15

DAVID PUBLISHING

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism Vincent Olunloyo1, Oyewusi Ibidapo-Obe1, David Olowookere2 and Michael Ayomoh1 1. Faculty of Engineering, University of Lagos, Lagos 100001 Nigeria 2. Department of Engineering Technology, Texas Southern University, Houston, TX 77004, USA

Abstract: This paper presents some initial solutions to the problem of accuracy and repeatability of the arm position placement in applied kinematics by solving the inverse kinematics problem of a serial jointed manipulator whose forward kinematics solution was earlier presented to solve the position placement problem of a mobile manipulator for Lunar Oxygen production. The problem herein is that of identifying a combination of joint angles to effectively position the end-effecter at a specified location in space. The reverse solution as presented in this paper is predicated on DH’s (Denavit-Hartenberg’s) technique for robot arm position analysis. The generalized solution for the 5-degrees of freedom DOF (degree of freedom) revolute joint variables which comprises 2-links and a spade-like 3-DOF end-effecter was obtained by solving a set of algebraic equations emerging from series of transformation matrices. The proposed solution herein has a high degree of accuracy and repeatability for workspace reachable domains where joint combination is analytic. Key words: Inverse kinematics, joint variables, DH’s, mobile manipulator, end-effecter.

1. Introduction The inverse or reverse kinematics problem for robot arm position placement is a fundamental challenge in everyday robotics. For serial linked manipulators as presented herein, the inverse problem is a perfect mimic and replication of the dexterity exhibited by the human arm in day to day problem solving. Basically, the inverse analysis could be seen as a phenomenon predicated on a backward chaining of a set of position control algorithms from the known end-effecter coordinate points to the unknown joint variables as a means of proffering realistic and feasible joint solutions. This scenario of problem demands a prior knowledge of the end-effecter’s position in space for the determination of a set of unknown joint variables capable of repositioning the end-effecter at its initial position. Corresponding author: Michael Ayomoh, Ph.D., research field: robotics, mechatronics, systems automation and modeling. E-mail: [email protected].

This paper presents algorithmic solutions to the problem of accuracy and repeatability of the arm position placement in applied kinematics by solving the inverse kinematics problem of a serial jointed manipulator whose forward kinematics solution was earlier presented to solve the position placement problem of a mobile manipulator for Lunar Oxygen production. The problem herein is that of identifying a combination of joint angles to effectively position the end-effecter at a desired location in space. The reverse solution as presented in this paper is premised on analytical schemes initiated with the DH’s technique. An inverse kinematics problem of a priori unknown 6R-Robot from the representation point of view was looked at in Ref. [1] while Ref. [2] presented a comparative study of several inverse kinematics methods for serial manipulators. Their comparison was based on the evaluation of a short distance approaching the goal point and on their computational

8

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

complexity. An integrated navigation scheme of the HVFF (hybrid virtual force field) and DH (Denavit-Hartenberg) kinematic convention for navigation and control of a mobile manipulator with emphasis on the forward kinematics of the arm was proposed in Ref. [3]. The HVFF scheme was first introduced in Refs. [4-7]. An algorithm for the tele-operation of mobile-manipulator systems with a focus on ease of use for the operator was proposed in Ref. [8]. A method based on geometric algebra for computing the solutions to the IKP (inverse kinematics problem) of the 6R robot manipulators with offset wrist was presented in Ref. [9], while Ref. [10] proposed a memetic DE (differential evolution) to improve the convergence behaviour of the standard DE scheme for two non-redundant robot manipulators. The double quaternions and Dixon resultant to solve the inverse kinematics analysis of the general 6R robot was introduced in Ref. [11], while Ref. [12] presented a real-time human motion capture system based on silhouette contour analysis and real-time inverse kinematics. In Ref. [13], a novel 3-DOF (degree of freedom) uncoupled spatial parallel manipulator was presented. Firstly, they addressed the geometrical structure and mobility analysis of the manipulator followed by the analysis of the forward and inverse kinematic problems. In Ref. [14], the kinematic analysis of a new TPM (translational parallel manipulator) was described, while Ref. [15] aimed at providing a mathematical and theoretical foundation for the design of the configuration and kinematic analysis of a novel humanoid robot. In Ref. [16], a novel method for redundancy parameterization and extremely fast inverse kinematic solutions for a 7-DOF anthropomorphic manipulators and animation characters was described. The generated data are classified into various inverse kinematic solution manifolds. Also, Ref. [17] developed an inverse kinematic solution for a reconfigurable robot system, and addressed the formulation of a generic numerical inverse kinematic model. While Ref. [18]

developed a model for the kinematic analysis of a parallel manipulator. The model includes the inverse and forward kinematics modeling. A network inversion as a method for solving inverse problems by using a multilayer neural network was proposed in Ref. [19], while Ref. [20] proposed a biomimetic approach for solving the problem of redundancy resolution. In Ref. [21], an analytical solution for the inverse kinematics of a 5-DOF manipulator was proposed while Ref. [22] presented an adaptive learning strategy using an ANN (artificial neural network) to control the motion of a 6-DOF manipulator robot and to overcome the inverse kinematics problem. In Ref. [23], a novel control approach based on a recurrent neural network was presented. This control strategy learns to control the robot arm online by solving the inverse kinematic problem in its control region. While Ref. [24] proposed an enhanced neural network model by developing the bees algorithm for effective optimization on the one hand, and in a second case considered the design of a hierarchical PID (proportional integral derivative) controller for a flexible single-link robot manipulator. In Ref. [25], a unique approach used to solve the human-like arm inverse kinematics, allowing the control system to generate smooth trajectories for each joint of the humanoid robot was introduced, while Ref. [26] presented closed-form solutions of IKMs (inverse kinematic models) of the anthropomorphic biped robot HYDROïD which has eight active DOF per leg.

2. DH Kinematic Structure of the Proposed Machine A 3D view of the proposed autonomous machine is as presented in Fig. 1. The arm is characterized with 5-DOF. One DOF each for the two links, and three DOF at the end-effecter. The inverse kinematic analysis is limited to the arm component of the mobile manipulator as shown in Fig. 1. Table 1 as presented below is an extract from Fig. 2.

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

nx ox ax d x  n o y a y d y  R   y nz oz az d z    0 0 1   0 Let the product matrix of the arm be expressed as:

It contains the DH parameters of the joint variables for the proposed mobile manipulator. The product matrix is given as: 0

5

T5 



i 1

T i  0 T1 1T 2 2 T 3 3 T 4 4 T 5

(1)

i 1

where,

5

T5  i 1Ti  0T1 1T2 2T3 3T4 4T5

0

C 1  S 1  S C 1 0 T1   1  0 0  0  0 C  2  S  2  S C 2 1 T2   2  0 0  0  0 C  3  S 3 3   0   0 C  4  3T   S  4 4  0   0 2T

C  5  S 4 T5   5  0   0

0 a1C 1  0 a1 S 1  1 0   0 1  0 a 2 C 2  0 a 2 S 2  1 0   0 1 

0 0 1

 S 3 C3

0

0 1

0  S 4 C4 0

0

0

0

0

 S 5 C 5

9

(2)

i 1

R  0 T 5 (See Appendix A) Joints 3,4,5

Joint 2

0 0  0  1 0 0  0  1

Joint 1

Fig. 1 Table 1

0 0  0 1 d5   0 0 1 Assuming the generalized DH matrix is expressed 0 0

in terms of R then:

3D view of the proposed autonomous machine. DH link parameters. a

α

θ

D

1 2 3 4

a1 a2 0 0

0 0 -π/2 -π/2

θ1 θ2 θ3 θ4

0 0 0 0

5

0

0

θ5

d5 d5

z3

a2

a1 z0

θ1

θ2 x0

y0

Fig. 2

θ4 θ3

z1

Coordinate frames for the robot arm.

y3

x4 z4

x3

y4

x5

x2

x1 y1

z2

θ5

y2

z5

y5

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

10

3. Inverse Kinematics Analysis

where,

Solving for the joint variables θ1, θ2, θ3, θ4 and θ5 From Appendix A,

 4  cos

1

 5  sin

(

( a z )

(3)

sz ) sin  4

(4)

also, 1

 represents a phase shift If   c 1  c

then Using trigonometric identities,

c1   2 2 2 2  sin1  Pxy  A5 a2  a1 2Pxy A5[sin]   2a1R   

R

Solving for the joint variables θ1, θ2 and θ3 requires some form of product matrix simplification hence, R1 = 1T0R and 5

T5  

i 1

T i  1T 2 2 T 3 3 T 4 4 T 5 (5)

p xy2  ( A5 ) 2  2 p xy A5 cos( )

  tan 1[

Hence, T 0 R  T 5 (See Appendix B). Following from Appendix B,

1  c1   xy

(16)

(17)

1

 2.  sin 1[

Pxy cos(1   xy )  A5 sin(1   xy ) a2

a x cos  1  a y sin  1  Axy cos(  1   xy ) (6) and

 a x sin 1  a y cos 1   Axy sin(1   xy ) (7) Let A  xy

( a x  a y ) ,  xy  tan 1 2

2

a y (8) ax

Also from Appendix B

Py sin 1  Px cos1  a1  Pxy sin(1   xy )  a1 (9) and

Py cos  1  Px sin  1  Pxy cos(  1   xy ) (10) Let 2

2

Pxy  ( p x  p y ) ,

 xy   ( 

 xy  tan 1

Px Py

) and sin  4  

(11)

  (90  ( xy   xy )

(12)

xy

]

(18)



Let

A5 sin  ] p xy  A5 cos

(15)

Hence,

i 1

1

  (14)  

where,

3.1 First Inverse (Iteration One)

1

(13)

  A5 sin(1   xy )     2  (19)   d5   

 3   sin 1  

4. Simulation The proposed inverse kinematics models were validated by first running some virtual experimental trials of the forward kinematics model with the aid of Matlab Software, version 7.1, 2010. These trials however, premised on arbitrary combination of joint angles, generated specific DH matrices which contains the end-effecter’s position placement point in space (translational parameters) and the normal, orientational and approach vector matrices (rotational parameters) as seen in the respective homogeneous transformational matrices following each of Tables 2-5. With a desired or known end-effecter’s position as obtained from the forward solution, the task at hand is that of recovering the arbitrarily selected joint variables this time with the use of the proposed

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

Experiment one: results for joint variables.

Simulated experiments Joint variables Value (degrees)

1 2 3 4 5

30 40 50 35 25

Analysis Joint variables Value (degrees)

1 2 3 4 5

0 . 9580 0 . 2868   0 . 0052  0 . 8542  0 . 4967 0 . 1533    0 . 5198  0 . 8192 0 . 2424  0 0 0  Given parameters: d5 =5, a1 = 10, a2 =

3.142).

experiment shape-preserving analysis

50

45

40

35

30

25 0

0.5

1

1.5 2 2.5 linear frames

3

3.5

4

Fig. 3 Graph of joint variables against linear frame scale: experiment I. Table 3

Experiment two: results for joint variables.

Simulated experiments Joint variables Value (degrees)

1 2 3 4 5

27 43 44 35 15

Analysis Joint variables Value (degrees)

1 2 3 4 5

27.0004 42.9999 44.1594 34.9952 15.0067

0 .9687 0 .2333   0 .0854  0 .8281 0 .1992  0 .5240    0 .5540 0 .1485  0 .8192  0 0 0  Given parameters: d5 =5, a1 = 10, a2 = 3.142.

13 .4967  11 .3169   4 .0958   1  10 and pi =

45

29.9985 40.0020

experiment shape-preserving analysis

40

50.0034 35

34.9952 25.0028

      20 and pi = 16 . 9346 21 . 3102  4 . 0958 1

joint variables

Table 2

55

joint variables

inverse model. Tables 2-5 and their respective homogeneous matrices represent outputs from the forward kinematics experiments nomenclatured (virtual experiment). Also contained in the tables are joint variable results premised on the reverse kinematics model with the nomenclature (analysis) as seen on the tables. Table 2 represents the outputs of the first experiment whose joint variables are as contained in the table and DH parameters contained in the matrix underneath. Fig. 3 is a graphical view of the values for the joint variables from both the virtual experiment (forward kinematics) and inverse kinematics analysis. The two line plots for both joint values are seen to be lying right on each other which is an indication of high degree of exactness or accuracy of output. Table 3 represents outputs from the second experiment whose joint variables are also contained in the table and DH parameters contained in the following matrix. Fig. 4 is a graphical view of thevalues for the joint variables from both the virtual experiment (forward kinematics) and analysis (inverse kinematics). The line plots in both cases are seen to be lying right on each other, which is an indication of high degree of exactness.

11

30

25

20

15 0

0.5

1

1.5 2 2.5 linear frames

3

3.5

4

Fig. 4 Graph of joint variables against linear frame scale: experiment II.

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

12

Simulated experiments Joint variables Value (degrees)

Analysis Joint variables Value (degrees)

1 2 3 4 5

1 2 3 4 5

10 50 35 25

45

34.9995 10.0016 50.0129 34.9952

40

25.0028

Simulated experiments Joint variables Values (degrees)

Analysis Joint variables Values (degrees)

1 2 3 4 5

1 2 3 4 5

 0.4226   0.7424   0.5198  0 

40 42 32 28

0.9063  0.3462 0.2424 0

0  0.5736  0.8192 0

24.0001 40.0009 41.9571 32.0052 27.9978

35 30 25 20 15 10 0

0.5

1

1.5 2 2.5 linear frames

3

3.5

4

Fig. 5 Graph of joint variables against linear frame scale: experiment III.

Experiment four: results for joint variables.

24

experiment shape-preserving analysis

50

joint variables

35

0 .9330 0 .05000 15 .5125   0 .3563  0 .7764  0 . 2659  0 . 5714 9 .9499     0 .5198 0 .2424  0 .8192  4 .0958    0 0 0 1   θ1 = 35; θ2 = 10; θ3=50; θ4 = 35; θ5 = 25. Given d5 = 5, a1 = 10, a2 = 10 and pi = 3.142. Table 5

55

Experiment three: results for joint variables.

42

experiment shape-preserving analysis

40 38 36 joint variables

Table 4

34 32 30 28 26

15.2626  9.9389   4.0958   1 

Given parameters: d5 = 5, a1 = 10, a2 = 10 and pi = 3.142. Table 4 which represents outputs from the third trial also has its peripheral matrix attached beneath it. As in earlier cases, the corresponding Fig. 5 which is a plot from Table 4 adequately expresses high level of accuracy. Furthermore, Table 5 contains the joint values for both the virtual experiment (forward kinematics) and analysis. The corresponding Fig. 6 like the earlier graphs represents a situation of exactness and accuracy.

24 0

0.5

1

1.5 2 2.5 linear frames

3

3.5

4

Fig. 6 Graph of joint variables against linear frame scale: experiment IV.

5. Conclusions This paper has presented an applicative research on the inverse kinematics problem for the arm component of a proposed mobile manipulator. The reverse model or inverse kinematics analysis utilized is premised on the DH principle of interacting homogenous matrices. The proposed analytical scheme is quite effective with a high level of model accuracy. The level of accuracy recorded, i.e., degree of exactness in comparing the values for joint variables obtained from analysis and virtual experimentation is significantly high and consistent for all joints within the range of analyticity.

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

However, a hand-full of analytic joint combinations (as presented in the tables) were deployed to validate the model. Following that, the inverse kinematics problem is usually associated with multiple solutions for same end-effecter placement in space, the selection of an optimal joint angle combination could be a research challenge. This would be looked at in terms of likely energy utilization by the actuators while positioning the end-effecter in the desired point in space. Also, the total travel time, arm navigation path, workspace constraint and velocity variant would form part of the modeling variables for different joint motion types such as: purely prismatic joints, purely revolute jointly and mixed joints in a follow-up paper.

[9]

[10]

[11]

[12]

Reference [1]

[2]

[3]

[4]

[5]

[6]

[7]

[8]

Groh, F., Groh, K., and Verl, A. 2013. “On the inverse kinematics of an a priori unknown general 6R- Robot.” Robotica. 1 (1) : 1-9. Eba, I. D., and Opalka, M. 2013. “A Comparison of Jacobian-Based Methods of Inverse Kinematics for Serial Robot Manipulators.” Int. J. Appl. Math. Comput. Sci. 23(2) : 373-82. Ibidapo-Obe, O., Olowokere D., and Ayomoh, M. K. O.2011. “A Simulation Model for Mobile Manipulator Navigation System for Lunar Oxygen Production.” International Journal of Agile Manufacturing 11(2): 37-51. Olunloyo, V. O. S., Ayomoh, M. K. O., and Ibidapo-Obe, O. 2009. “A Path Planning Model for an Autonomous Vehicle in an Unstructured Obstacle Domain.” In Proceedings of the IASTED Conference on Robotics and Applications, 180-7. Olunloyo, V. O. S. and Ayomoh, M. K .O.2009. “Autonomous Mobile Robot Navigation Using Hybrid Virtual Force Field Concept.” European Journal of Scientific Research. 31(2): 204-28 Olunloyo, V. O .S, and Ayomoh, M. K. O. 2010. “A Hybrid Path Planning Model For Autonomous Mobile Vehicle Navigation.” In Proceedings of 25th International Conference of CAD/CAM Robotics & Factories of the Future, 1-11. Olunloyo, V. O. S., and Ayomoh, M. K. O. 2011. “An Efficient Path Planning Model in an Unstructured Obstacle Domain.” In Proceedings of IASTED International Conference on Robotics and Applications, 38-45. Frejek, M., and Nokleby, S. B. 2013. “A Methodology

[13]

[14]

[15]

[16]

[17]

[18]

[19]

[20]

13

for Tele- operating Mobile Manipulators with an Emphasis on Operator Ease of Use.” Robotica, 31 (3) : 331-44. Fu, Z., Yang, W. and Yang, Z. 2013. “Solution of Inverse Kinematics for 6R Robot Manipulators with Offset Wrist Based on Geometric Algebra.” Journal of Mechanisms and Robotics 5 (3) : 81-7. Uzcátegui, C. G., and Rojas, D. B., 2013. “A Memetic Differential Evolution Algorithm for the Inverse Kinematics Problem of Robot Manipulators.” International Journal of Mechatronics and Automation 3(2) : 118-31. Qiao, S., Liao, Q., Wei, S. and Su, H. 2010. “Inverse Kinematic Analysis of the General 6R Serial Manipulators Based on Double Quaternions.” Mechanism and Machine Theory, 45 (2): 193-9. Hoshino, R., Yonemoto, S., Arita, D., and Taniguchi, R. 2002. “Real-time Motion Capture System Based on Silhouette Contour Analysis and Inverse Kinematics.” In Proceedings of the 7th Korea-Japan joint Workshop On Computer Vision: Frontiers of Computer Vision, 157-63. Zhang, Y., Liu H. and X. Wu, 2009. “Kinematics Analysis of a novel parallel manipulator.” Mechanism and Machine Theory 44 (9): 1648-57. Ruggiu, M., 2008. “Kinematics Analysis of the CUR Translational Manipulator.” Mechanism and Machine Theory 43 (9): 1087-98. Man, C., Fan, X., Li C., and Zhao, Z. 2007. “Kinematics Analysis Based on Screw Theory of a Humanoid Robot.” Journal of China University of Mining and Technology, 17 (1): 49-52. Tarokh, M., Keerthi, K., and Lee, M. 2010. “Classification and Characterization of Inverse Kinematics Solutions for Anthropomorphic Manipulators.” Robotics and Autonomous Systems, 58 (1): 115-20. Chen, I., and Yang, G. 1998. “Numerical Inverse Kinematics for Modular Reconfigurable Robots.” In Proceedings of IEEE Conference on Robotics and Automation. Ogawa, T. and Kanada, H. 2010. “Solution for Ill-Posed Inverse Kinematics of Robot Arm by Network Invesion.” Journal of Robotics. Article ID 870923: 9 Artemiadis, P. K., Katsiaris, P. T. and Kyriakopoulos, K. J. 2010. “A Biomimetic, Approach to Inverse Kinematics for a Redundant Robot Arm.” Autonomous Robots 29(3-4): 293-308. Xu, D., Calderon, C. A. A., Gan, J. Q., Hu, H., and Tan, M. 2005. “An Analysis of the Inverse Kinematics for a 5-DOF Manipulator.”International Journal of Automation and Computing, 2: 114-24.

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

14

[21] Hasan, A. T., Hamouda, A. M. S., Ismail, N., and Al-Assadi, H. M. A. A. 2006. “An Adaptive-learning Algorithm to Solve the Inverse Kinematics Problem of a 6-DOF Serial Robot Manipulator.” Advances in Engineering Software 37 (7): 432-8. [22] Waegeman, T., and Schrauwen, B. 2011. “Towards Learning Inverse Kinematics with a Neural Network Based Tracking Controller.” Neural Information Processing, Vol. (7064): 441-8. [23] Fahmy, A. A., Kalyoncu, M., and Castellani, M. 2011. “Automatic design of control systems for robot manipulators using the bee algorithm.” In Proceedings of the Institution of Mechanical Engineers. [24] Cha, Y., Kim, K., Lee, J., Lee, J., Choi, M., Jeong, M.,

Kim, C., You, B., and Oh, S. 2011. “MAHRUM: A Mobile Humanoid Robot Platform Based on a Dual-network Control System and Coordinated Task Execution.” Robotics and Autonomous Systems, 59(6): 354-66. [25] Bertrand, S. Bruneau, O. Ouezdou, F.B. and Alfayad, S., 2012. “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-40. [26] Fang, H. C., Ong, S. K., and Nee, A. Y. C. 2012. “Interactive Robot Trajectory Planning and Simulation Using Augmented Reality.” Robotics and Computer-Integrated Manufacturing, 28 (2): 227-37.

Appendix A: product matrix

nx n R   y nz   0 n x  sin 

5

 (cos 

 sin  1  sin   sin 

3

2

3

 (cos  1  sin 

))  cos 

 (cos  1  sin 

n y  cos  4  cos  5  (cos  3  sin  1

2

4

 cos 

2

ax ay az

0

0

 cos 

2

 cos 

ox oy oz

5

     1 

dx dy dz

 sin  1 )  sin 

2

 (cos 

3

(1)

3

 (cos  1  cos 

 (cos  1  cos  2

 sin  1  sin 

2 2

)

(2)

 sin  1 ))

 (cos  1  sin  2  cos  2  sin  1 )  sin  3  (cos  1  cos  2

 sin  2 ))  sin  5  (cos  3  (cos  1  cos  2  sin  1  sin  2 )  sin  3  (cos  1  sin  2 (3)

 cos  2  sin  1 ))

n z   cos  5  sin  4

s x  cos  5  (cos  2  sin  3  cos  3  sin  2 )  cos  4  sin  5  (cos  2  cos  3  sin  2  sin  3 ) s y   cos  5  (cos  3  (cos  1  cos  2  sin  1  sin  2 )  sin  3  (cos  1  sin  2  cos  2  sin  1 ))

(4) (5)

(6)

 cos  4  sin  5  (cos  3  (cos  1  sin  2  cos  2  sin  1  sin  3  (cos  1  cos  2  sin  1  sin  2 )))

s z  sin  4  sin  5

a x   sin  4  (cos  3  (cos  1  cos  2  sin  1  sin  2 )  sin  3  (cos  1  sin  2  cos  2  sin  1 ))

(7) (8)

a y   sin  4  (cos  3  (cos  1  sin  2  cos  2  sin  1 )  sin  3  (cos  1  cos  2  sin  1  sin  2 ))

(9)

a z   cos  4

(10)

p x  a1  cos 1  a2  cos 1  cos  2  a2  sin 1  sin  2  d 5  sin  4  (cos  3  (cos 1  cos  2  sin 1  sin  2 )  sin  3  (cos 1  sin  2  cos  2  sin 1 ))

(11)

Inverse Kinematics Analysis of a Five Jointed Revolute Arm Mechanism

p

y

15

 a 1  sin  1  a 2  cos  1  sin  2  a 2  cos  2  sin  1  d 5  sin  4  (cos  3 

(12)

(cos  1  sin  2  cos  2  sin  1 )  sin  3  (cos  1  cos  2  sin  1  sin  2 )) p z   d 5  cos  4

(13)

Appendix B: inverse iteration matrix

R1  nx cos(1 )  ny sin(1 ), sx cos(1 )  s y sin(1 ), ax cos(1 )  a y sin(1 ) , px cos(1 )  a1  p y sin(1 ) ny cos(1 )  nx sin(1 ), s y cos(1 )  sx sin(1 ), a y cos(1 )  ax sin(1 ), p y cos(1 )  px sin(1 ) nz ,

sz ,

ax

0,

0,

0

(1)

pz 1

n x cos  1  n y sin  1  sin  5  (cos  2  sin  3  cos  3  sin  2 )  cos  4  cos  5  (cos  2  cos  3  sin  2  sin  3 ) n y  cos  4  cos  5  (cos  2  sin  3  cos  3  sin  2 )  sin  5  (cos  2  cos  3

(3)

 sin  2  sin  3 )

nz   cos5  sin  4

(4)

s x  cos  5  (cos  2  sin  3  cos  3  sin  2 )  cos  4  sin  5  (cos  2  cos  3

(5)

 sin  2  sin  3 ) s

y

  cos   sin 

3

5

 (cos 

 cos 

3

2

 cos 

 sin 

2

3

 sin 

2

 sin  3 )  cos 

(2)

4

 sin 

5

 (cos 

) s z  sin  4  sin  5

2

(6) (7)

a x   sin  4  (cos  2  cos  3  sin  2  sin  3 )

(8)

a y   sin  4  (cos  2  sin  3  cos  3  sin  2 )

(9)

a z   cos  4

(10)

p x  a 2  cos  2  d 5  sin  4  (cos  2  cos  3  sin  2  sin  3 ) p y  a 2  sin  2  d 5  sin  4  (cos  2  sin  3  cos  3  sin  2 )

pz  d5  cos4

(11) (12) (13)