Inverse Kinematic Controlof a Prosthetic arm

Project Report-Engi9940

B.M.Oscar De Silva201070588

Faculty of Engineering and Applied ScienceMemorial University of Newfoundland

Fall 2010

submitted to:Dr. George Mann

Abstract

Robotic prosthetics are devices worn by humans with limb amputations. Theseoperate in unstructured environments performing realtime tasks. Thus require kine-matic controllers to perform online joint trajectory planning. The project performsa comparative study on online trajectory generation methods available for roboticmanipulators. Its implemented with the objective of selecting a suitable kinematiccontrol strategy to address problems in upper extremity prosthetic applications. Un-der the study, a 7 DOF prosthetic model was simulated for a pick and place task,using 4 different kinematic control methods based on the CLIK algorithm. Thecontrol approaches were tested for redundancy resolution, joint limits managementand singularity management. Weighted pseudo inverse with weighting function cap-turing the joint limits, outperforms the other methods compared under this study.Drawbacks of this method were highlighted with improvements necessary for real-time hardware application.

KEY INDEXING TERMS: Inverse Kinematics, Pseudo Inverse, Upper Ex-tremity Prosthetics, Redundancy

2

CONTENTS i

Contents

1 Introduction 1

2 Theory 22.1 Closed Loop Inverse Kinematics Algorithm . . . . . . . . . . . . . . . . . . 32.2 Jacobian Transpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.3 Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.4 Task Priority Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . 42.5 Weighted Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3 Approach 53.1 Mathematical Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.2 Task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.3 Visualization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.4 Inverse Kinematics Controller . . . . . . . . . . . . . . . . . . . . . . . . . 8

4 Results 94.1 Jacobian Transpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.2 Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.3 Damped Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.4 Task Priority Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . 114.5 Weighted Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5 Conclusion 14

A Appendices iA.1 Simulink model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iA.2 Matlab Code-Mathematical model generation code . . . . . . . . . . . . . iA.3 Matlab Code-Inverse kinematics code . . . . . . . . . . . . . . . . . . . . . iiiA.4 Matlab Code-Weight calculator for pseudo inverse . . . . . . . . . . . . . . ivA.5 Matlab Code-Trajectory generation code . . . . . . . . . . . . . . . . . . . vA.6 Matlab Code-Forward kinematic model . . . . . . . . . . . . . . . . . . . . viA.7 Matlab Code-Differential kinematic model . . . . . . . . . . . . . . . . . . vii

1 INTRODUCTION 1

1 Introduction

Motion planning with kinematic control has been a widely discussed topic in roboticmanipulators. In most applications the controllers poses the luxury of preprocessingthe trajectory of the end effector and the joints. This is due to the prior availabilityof the information of the task performed and the environment the task is performed.Industrial robots performing repetitive tasks such as painting, assembly etc.. are mainapplication areas of this method. But as manipulators become complex with autonomoustask planning and operates in unstructured environments, realtime solutions to the inversekinematic problem is more desirable.

Upper extremity prosthetics or robotic limbs are such systems that operate in unstruc-tured environments with realtime task determination. Thus a suitable inverse kinematicscontroller should be in place to generate the required trajectories for autonomous or semiautonomous operations. This should be performed while overcoming the sub problems ofredundancy, singularity and joint limits. These subproblems will be discussed in detail inchapter2.

A variety of methods for Inverse kinematic control has been developed and well doc-umented [1] [2] [3]. Each has unique attributes with respect to addressing subproblemsand computational demand. Most of the methods are based on Closed loop Inverse Kine-matics(CLIK) Algorithm proposed by [1]. For general application of the CLIK algorithma generalized inversion of a matrix is required. A number of generalized approaches cor-responding to different optimizations of the solution are available in literature[4]. Theperformance of these methods base upon the application understudy and properties theyrequire.

So The project aims to perform a comparative study on 5 selected inverse kinematiccontrollers on a 7DOF prosthetic arm system. The candidates were assessed for addressingredundancy, joint limits and singularity with aim to select the suitable IK controller forprosthetic implementation.

The report is structured in to 4 main parts addressing the theory, the approach,results and the conclusion of the study. The theory section outlines the methods proposedby previous researches with a general discussion on different attributes of the methods.Details on the model development and simulation are presented in the chapter 3 withspecific details on applying the IK controllers. Chapter 4 and 5 performs a comparativestudy on the results with a selection of a suitable controller for prosthetic applicationsremarked with the improvements necessary for hardware application.

2 THEORY 2

2 Theory

A manipulation task for a robot is generally defined in terms of position and orientation ofthe end effector. The objective of an Inverse kinematic controller is to find suitable jointtrajectories qr(t) of a manipulator corresponding to the desired end effector positions andorientations ηee(t). The outputs of the inverse kinematic controller are the joint variablesqr(t) which would be used as reference values for the dynamic control law of the robot. Amapping function f(q) from joint variables qr(t)to end effector variables ηee(t)defines theforward kinematics of the problem.

ηee = f(q(t)) (1)

The inverse problem which finds the desired joint variables can be expressed as;

qd(t) = f−1(ηee(t)) (2)

It is possible to analytically find the solution through solving the set of equations forqr(t) using trigonometric relations. But as the number of DOF‘s in the system becomelarger, the number of possible solutions increases requiring an optimal selection betweenthe solutions. But at a differential level the relationship between joint variables and theend effector becomes more traceable. This relationship is captured in equation 3.

η̇ee(t) = J(q)q̇(t) (3)

Where the Jacobian J is defined as the matrix which transforms the joint velocitiesto end effector velocities.

J =

∂η1∂q1

∂η1∂q2

· · · ∂η1∂qn

∂η2∂q1

∂η2∂q2

· · · ∂η2∂qn

......

. . ....

∂ηn∂q1

∂ηn∂q2

· · · ∂ηn∂qn

(4)

We can write equation 4 in Infinitesimal joint movements ∆θ and end effector move-ments ∆x in ∆t time.

∆θ = J−1∆x (5)

Equation 6 forms the basis for solving inverse kinematics of manipulators using adifferential approach. In forming an IK solution there are certain subproblems whichshould be incorporated in the IK algorithm.

Redundancy Resolution Kinematic redundancy is characterized by the appearance ofmultiple solutions to the IK problem. An IK algorithm should perform an intu-itive selection among the available solutions. At a differential Inverse kinematicapproach, this selection is done between the neighboring solutions of the robotscurrent configuration.

Singular Value Decomposition Singular configurations are characterized by the robothaving an non invertible jacobian matrix for the certain configuration. Algorithmic

2 THEORY 3

singularities occur when such singularities occur due to the trajectories selected bythe controller. So a proper singular value decomposing strategy should be in placefor desired operation.

Joint limits management Joint limits constitute a physical constraint of the manipu-lator. A joint limit management strategy effectively overcomes the physical incom-patibility of solutions approached.

2.1 Closed Loop Inverse Kinematics Algorithm

The Jacobian matrix is a function of joint variables. So solving Equation 6 requiresliberalization of the jacobian about an operating point. This causes a drifting error in thecalculation of the desired joint variables. Closed loop inverse kinematic (CLIK) algorithmis a method where this error is accounted for and properly handled.

qcurrent = qprevious + J(qprevious)−1 ∗ (∆ηcurrent + β ∗ error) (6)

Let q(t) be the current joint variables of the robot and ηd is the desired end effectorposition. The CLIK algorithm can be formulated as;

for ηd(t) = starttotarget doη(t− 1) = comupteη(q(t− 1))∆ηd = ηd(t)− ηd(t− 1)error = ηd(t− 1)− η(t− 1)q(t) = q(t− 1) + J†(q(t− 1)) ∗ (∆ηd + β ∗ error)

end for

The core of the algorithm is finding the jacobian inverse J† for the operating pointq(t − 1). This is quite straight forward for a square matrix case. But for a general casea generalized matrix inversion should be performed to find the solution. In precedingsections the use of generalized matrix inversion and its variants will be discussed.

2.2 Jacobian Transpose

The Transpose of the Jacobian JT form a simple alternative for the generalized inverseJ†. Although this is not a general inversion of the matrix, the CLIK algorithm acceptsthis to produces solutions for simple systems. This method is considered unstable andsusceptible to algorithmic singularities.

2.3 Pseudo Inverse

Pseudo inverse is the simplest form of generalized inversion to a matrix. The solutionsgiven using this inversion corresponds to the minimization of the joint velocities in a leastsquares sense.

J†(q) = JT (q)(J(q)JT (q))−1 (7)

2 THEORY 4

Damping can be introduced to the mapping using equation 8. This handles invertingof ill conditioned matrices.

J†(q) = JT (q)(J(q) ∗ JT (q) + λ ∗ Inxn)−1 (8)

The methods discussed considers only the primary task of following the desired tra-jectories of the end effector. However in the physical setup some solutions given by thesystem may not be compatible. Even when the solutions are valid, it would not be theoptimum solution from the many solutions available due to redundancy. So additionalconstraints should be introduced to the problem. The additional constraints can be incor-porated based on two main methods. The task priority method and a Weighted pseudoinverse with designed weights.

2.4 Task Priority Inverse Kinematics

The pseudo inverse can be further generalized starting at equation 6. This takes the form;

∆qr = J†p(q)∆ηp + (In − J†p(q)JTp (q)) J†s (q)∆ηs (9)

The subscript p and s refers to primary and secondary. The method expects a sec-ondary referenceηs as additional constraint. The jacobian Js is the jacobian defined forthe secondary task.

2.5 Weighted Pseudo Inverse

A further modification to the inverse is obtained by introducing weights to the jacobian(equation 10). This allows modification to the solution.

J†(q) = W−1JT (q)(J(q)W−1JT (q) + λ ∗ I)−1 (10)

The weight matrix W can be designed so that it constraints movement of certainjoints. Its designed so that it introduces the joint limit constraints by selecting thediagonal elements of the weight matrix W to take the form defined by equations 11 and12.

∂H(q)

∂qi=

(qi,max − qi,max)2(2qi − qi,max − qi,min)

4(qi,max − qi)2(qi − qi,min)2(11)

Wi,i =

{1 + |∂H

∂qi| if ∆|∂H/∂qi| ≥ 0

1 if ∆|∂H/∂qi| < 0(12)

This ensures that movements are amplified in the joints in between closer to the middleof joint limits.

3 APPROACH 5

3 Approach

Figure 1: Top view of prosthetic model home position

The IK controllers discussed in Chapter 2 were implemented in a 7DOF model of anUpper extremity prosthetic. 7DOF‘s capture the missing degrees of freedoms of an aboveelbow amputee. The end effector operations(hand operations) were not captured in thisinitial model, only positioning of the end effector was studied.

The selected task was a pick and place operation. This was selected within theworkspace of the manipulator. A critical review of the IK controllers would requiredthem to be tested in number of different tasks but for this study a single pick and placewas considered sufficient for initial testing.

3.1 Mathematical Model

A 7 DOF forward kinematics model was developed following the Davient Hartinburg no-tation for robotic manipulators. Table 1 illustrates the DH notations used in formulatingthe mathematical model which has 8 DOF‘s. The first DOF which corresponds to shoul-der movement is fixed for the purpose of this study. The coordinate systems were markedfollowing the physical order of actuation that is evident in arms. Anthropometric datawith respect to arm lengths and joint limits were used in developing the mathematicalmodel.

T V Ree = T V R0 ∗ T 0ee (13)

3 APPROACH 6

Frame θi di ai αi1 θ1 -17.95 205.22 −90o

2 θ2 0 0 90o

3 θ3 0 0 −90o

4 θ4 365 0 90o

5 θ5 0 0 −90o

6 θ6 310 0 90o

7 θ7 0 0 −90o

8 θ8 0 -150 90o

Table 1: DH notations for prosthetic model

T V R0 =

−1 0 0 00 0 1 00 1 0 00 0 0 1

(14)

Joint Variable Physical movement lower limit upper limit

θ2 Shoulder Flexion-Extension −15o 90o

θ3 Shoulder Adduction-Abduction −90o 90o

θ4 Shoulder Internal-External Rotation −100o 10o

θ5 Elbow Flexion-Extension −10o 150o

θ6 Elbow Supination-Pronation −130o 90o

θ7 Wrist Flexion-Extension −10o 10o

θ8 Wrist Adduction-Abduction −20o 20o

Table 2: Joint limits for prosthetic model

The overall transformation matrix acts as a forward kinematic model for the system.This translates the joint variables to the end effector position and orientations. Thiswas also used to formulate the jacobian matrix using the definition of the jacobian asfound in variational calculus. This formulates a 3x7 velocity jacobian to the 7 DOFsystem. Because the Inverse kinematic algorithm attempts to track position only in theend effector, the angular velocity jacobian is not incorporated in the study. However thiscan be added to the study using the same approach used for velocity jacobian if required.

3.2 Task

The task performed is a pick and place task in this initial study of the inverse kinematicalgorithm. Way points were selected in the manipulators task space ensuring a physicalsingularity will not be reached in the process. Desired maximum accelerations of 40mm

s2

and Maximum end effector velocities 20mms

were set, so that each way point is followed inthe preset trajectory pattern. The initial configuration of the 7 joint variables was set to[20 −20 −30 30 10 −50 10

]and two cases of the same task was considered during

simulation2.

3 APPROACH 7

Waypoint X(mm) Y(mm) Z(mm)

WP1 -143 -159 714WP2 -255 -350 500WP3 -255 -200 500WP4 -145 -200 500WP5 -145 -350 500

Table 3: Waypoints selected for the task

−400 −350 −300 −250 −200 −150 −100−400

−350

−300

−250

−200

−150

−100

X(mm)

Y(m

m)

−400 −350 −300 −250 −200 −150 −100−400

−350

−300

−250

−200

−150

−100

X(mm)

Y(m

m)

start point start point

Figure 2: The two end effector trajectories fed to the IK controller. left-Case B, Right-Case A

Case A All intermediate end effector trajectories between the way points were generated.This formulates the ideal conditions for the algorithm where each way point isarrived taking small steps.

Case B Intermediate end effector trajectories between the waypoints 1 and 2 were notgenerated. This is to test the stability of the algorithm for large errors figure2.

3.3 Visualization

Matlab virtual reality toolbox was used to visualize the developed model. A 3D skeletonmodel was developed using VRealm Builder which was operated using a Matlab GraphicalUser Interface across a simulink Platform. The visualization was necessary to verify themodels visually.

The coordinate system of the Virtual reality platform was first translated to the basecoordinates of the prosthetic model using euler angles (14). Then suitable adjustmentsto the input joint angles to the system were made to match the positive joint rotationsand the home position of mathematical model to the visualization system.

3 APPROACH 8

3.4 Inverse Kinematics Controller

The CLIK algorithm discussed in Chapter 2 was implemented as a Simulink block. Theinitial conditions were set to a non singular configuration for desirable operation. Withinthe CLIK algorithm the jacobian inverse was changed based on the theory presented inthe methods discussed in Chapter 2. For the weighted pseudo inverse approach the weightmatrix was developed based on the joint conditions which are apparent in human armspresented in table 2.

4 RESULTS 9

4 Results

4.1 Jacobian Transpose

The jacobian transpose, although simple was able to fully solve Case B. But its instabilityis evident in Case A where it reaches a singularity and fails to converge to a solution.

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1

q2

q3

q4

q5

q6

q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 3: Joint tracking, limits conformance and tracking error-Jacobian Transpose-CaseA

4.2 Pseudo Inverse

Pseudo inverse corresponds to the solution with the minimum joint velocities. This map-ping resolves redundancies following the least velocity neighbor solutions. It was successfulin forming solutions to both cases, but figure 5 illustrates certain abrupt jumps in the solu-tion. These correspond to transferring between solutions due to algorithmic singularitiesof the method.

Further it was observed that the algorithm does not take in to account the jointlimits of the system. This leads to physically incompatible solutions to be selected by theapproach. So the Pseudo inverse method lacks joint limit management and is prone toalgorithmic singularities.

4.3 Damped Pseudo Inverse

Damped pseudo inverse mathematically introduces damping to the convergence of thealgorithm. The results indicate the reduction in abrupt changes in joint angles with time.So the damped method clearly improves the capabilities of the pseudo inverse method as

4 RESULTS 10

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 4: Jacobian Transpose-Case B

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 5: Joint tracking, limits conformance and tracking error-Pseudo Inverse-Case A

far as to managing joint limits are concerned. Although no improvement in joint limitsmanagement was made.

4 RESULTS 11

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 6: Pseudo Inverse-Case B

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 7: Joint tracking, limits conformance and tracking error-Damped Pseudo Inverse-Case A

4.4 Task Priority Inverse Kinematics

The qualitative study of the task priority algorithm was performed based on a previousmodel used to test IK algorithms. This was a underwater robotic manipulator system. the

4 RESULTS 12

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 8: Damped Pseudo Inverse-Case B

results of this better illustrates the attributes of a task priority approach to the problem.First a secondary task of station keeping (keeping the robot base at a predefined

point) was added to the algorithm. A task of sinusoidal motion in y axis was performed.The results portraits the systems adherence to the secondary station keeping task inimplementation of the process.

Then the amplitude of the sinusoidal movement in y direction was increased. Whenthe amplitude exceeds the levels which adhering to the secondary task is no longer possiblethe algorithm seeks solutions which considers the primary task only with less concern onthe secondary task.

This is not acceptable for a constraints as joint limits. Joint limits constitute a con-straint which should be strictly adhered to rather than being considered as a secondaryobjective. So a task priority method of inverse kinematics is not suitable to incorporatejoint limits but it proves to be of much use in introducing additional secondary constraintssuch as minimizing energy or following a natural posture.

4.5 Weighted Pseudo Inverse

A weighted pseudo inverse method of incorporating joint limits to the equation hasachieved good results for both the cases in terms of singularity management and in-corporating joint limits. As the joint limits itself introduce configurations of singularitythe method simultaneously achieves both requirements in a certain extent.

Although the method ensures the output solutions are within joint limits it cannotrecover from configurations which has already exceeded the joint limits. so certain condi-tions should be met;

1. There is sufficient redundancy in the system to achieve the task in the given joint

4 RESULTS 13

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 9: Joint tracking, limits conformance and tracking error-Weighted Pseudo Inverse-Case A

−50

0

50

100

Join

t var

iabl

es(d

eg)

q1q2q3q4q5q6q7

−1

0

1

Join

t lim

it

0 5 10 15 20 25 30 35 40 45 500

5

Err

or(m

m)

time(s)

Figure 10: Weighted Pseudo Inverse-Case B

limits.

2. The initial conditions does not exceed joint limits(realistic).

5 CONCLUSION 14

5 Conclusion

IK Method Tracking error Singularities Joint limits

Jacobian Transpose 1mm max in Case A un-stable in Case B

Singularities en-countered in CaseA

exceeds in Case Aand B

Pseudo Inverse 4mm max in Case Aand Case B

Singularities en-countered in CaseB

exceeds in Case Aand B

Damped Pseudo Inverse 4mm max in Case Aand Case B

Singularities en-countered in CaseB but dampedw.r.t. pseudoinverse method

exceeds in Case Aand B

Weighted Pseudo Inverse 1mm max in Case Aand Case B

Singularities notencountered

satisfied in bothCase A and B

Table 4: Summary of IK algorithm performances

The compared IK algorithms correspond to different solutions to the IK problem. Allmethods were successful in solving the IK problem for the given task for certain timeperiods. The first three methods does not apply joint limits to its solution strategybut performs reasonably well in solving joint variables for the task. Task priority andWeighted pseudo methods enable incorporation of additional constraints to the Inversekinematic problem. But a task priority method fails in meeting strict constraints suchas joint limits. This would have more application in incorporating secondary constraintssuch as desirable postures or energy minimization.

Weighted pseudo method delivers more desirable results in terms of redundancy res-olution and joint limit management. Abrupt jumps in the solutions, which were evidentin previous methods were hardly encountered in the Weighted method. It was also ablefulfill the joint limit constraint for both the cases for the full time scale under study.Although it was minimally prone to algorithmic singularities ,errors accounting to 3mm‘swere encountered at certain time steps. So with a suitable singular value managementstrategy to overcome singularity exposure, is recommended for Inverse kinematic controlof realtime prosthetic applications.

The main drawback in the results, approaching singular solutions, should be elimi-nated if this method is to be employed in actual hardware implementation. A suitablesingular value decomposition method can be included to the algorithm to improve results.Another method to manage singular values is by incorporating it as a secondary task tothe system. A secondary task is designed so that its desired values corresponds to nonsingular configurations. So such a hybrid inverse kinematic strategy would better serveas a suitable system for hardware implementation. This could be further improved forenergy minimization and self collision avoidance.

A simplification to the 7DOF model can be applied by kinematic decoupling at thewrist joint. 5 DOF model without the wrist to solve the inverse kinematic problem wouldreduce reaching singular configurations but would reduce its redundant degrees of freedom. The wrist orientation can be separately controlled based on the task at hand.

REFERENCES i

References

[1] P. Chiacchio, S. Chiaverini, L. Sciavicco, and B. Siciliano, “Closed-loop inverse kine-matics schemes for constrained redundant manipulators with task space augmentationand task priority strategy,” The International Journal of Robotics Research, vol. 10,no. 4, p. 410, 1991.

[2] G. Antonelli and A. Leonessa, “Underwater robots: motion and force control ofvehicle-manipulator systems,” IEEE CONTROL SYSTEMS MAGAZINE, 2008.

[3] B. Dariush, Y. Zhu, A. Arumbakkam, and K. Fujimura, “Constrained closed loopinverse kinematics,” in Robotics and Automation (ICRA), 2010 IEEE InternationalConference on, pp. 2499 –2506, May 2010.

[4] A. Ben-Israel and T. Greville, Generalized inverses: Theory and applications. SpringerVerlag, 2003.

A Appendices

A.1 Simulink model

Figure 11: Simulink model for testing IK controllers

A.2 Matlab Code-Mathematical model generation code

1 %Jacobian of Human arm

A APPENDICES ii

2

3 %Dynamic Equations for 6DOF Stanford Manipulator4 %Author: Oscar De Silva5 %22 nov 20106 clear;7 %include required symbols and the dh parameters8 syms g;9 syms t1 t2 t3 t4 t5 t6 t7 t8;

10

11 q=['t2'; 't3'; 't4'; 't5'; 't6'; 't7'; 't8'];12 Tlink(:,:,1)=dhtsym(t1,-17.95,205.22,-90);%dummy13 Tlink(:,:,1)=dhtsym(0,-17.95,205.22,-90);%theta,d,a,alpha14 Tlink(:,:,2)=dhtsym(t2,0,0,90);15 Tlink(:,:,3)=dhtsym(t3,0,0,-90);16 Tlink(:,:,4)=dhtsym(t4,365,0,90);17 Tlink(:,:,5)=dhtsym(t5,0,0,-90);18 Tlink(:,:,6)=dhtsym(t6,310,0,90);19 Tlink(:,:,7)=dhtsym(t7,0,0,-90);20 Tlink(:,:,8)=dhtsym(t8,0,-150,90);21

22 %in homogenous cordinates23 rcoglink(:,1)=[0; 0; 0;1];24 rcoglink(:,2)=[0; 0; 0;1];25 rcoglink(:,3)=[0; 0; 0;1];26 rcoglink(:,4)=[0; 0; 0;1];27 rcoglink(:,5)=[0; 0; 0;1];28 rcoglink(:,6)=[0; 0; 0;1];29 rcoglink(:,7)=[0; 0; 0;1];30 rcoglink(:,8)=[0; 0; 0;1];31

32 g1=[0 0 -g];33

34 fprintf('Initialization Complete...\n');35 %do not edit from here36 %calculating transformation matrices37 fprintf('calculating transformation matrices...\n');38 for i=1:size(Tlink,3)39 if i==140 Tbase(:,:,i)=Tlink(:,:,i);41 else42 Tbase(:,:,i)=simplify(Tbase(:,:,i-1)*Tlink(:,:,i));43 end44 end45

46 %Extracting rotation matrices47 fprintf('Extracting rotation matrices...\n');48 for i=1:size(Tlink,3)49 Rbase(:,:,i)=Tbase(1:3,1:3,i);50 end51

52 %position vectors of center of mass53

54 %z biased to overcome zworld(0) problem55 fprintf('Extracting joint axis...\n');56 for i=2:size(Tlink,3)

A APPENDICES iii

57 zworld(:,i)=Rbase(:,:,i-1)*[0 0 1]';58 end59 zworld(:,1)=[0 0 1]';60

61 fprintf('finding relative distances...\n');62 for j=1:size(Tlink,3)63 pend(:,1,size(Tlink,3)-j+1)=Tbase(:,:,size(Tlink,3)-j+1)*rcoglink...64 (:,size(Tlink,3)-j+1);%p biased to overcome pend(0) problem65 for i=2:size(Tlink,3)-j+166 pend(:,i,size(Tlink,3)-j+1)=Tbase(:,:,size(Tlink,3)-j+1)*rcoglink...67 (:,size(Tlink,3)-j+1)-Tbase(:,:,i-1)*[0;0;0;1];68 end69 end70 %verified71

72 fprintf('Calculating link Jacobians...\n');73 J(:,:,1)=[t1;0;0;0;0;0]; %dummy74 for j=1:size(Tlink,3)75 for i=1:j76 if isempty(symvar(Tlink(1:3,1:3,i)))77 J(:,i,j)=[zworld(:,i);0;0;0];78 else79 J(:,i,j)=[cross(zworld(:,i),pend(1:3,i,j));zworld(:,i)];80 end81 end82 end83 %verified84 fprintf('The jacobian matrix for end effector...\n');85 J(:,:,size(Tlink,3));86

87 Tbtovr=[-1 0 0 0;88 0 0 1 0;89 0 1 0 0;90 0 0 0 1];91

92 Teetovr=Tbtovr*Tbase(:,:,8);93

94 fprintf('Computing jacobian with first principles...\n');95 for i=1:size(Tlink,3)-196 Jv(:,i)=diff(Teetovr(1:3,4),q(i,:));97 end98 Jv

A.3 Matlab Code-Inverse kinematics code

1 function out=humanik2(in)2 %in=[0.5 0.5 0 30/180*pi 30/180*pi];3 Q=in(1:3);%next point4 qcurrent=in(4:10);5 w=in(11:17);6 K=1*eye(3);%do not damp in feeded waypoint method7 l=0.5;

A APPENDICES iv

8

9 %calculate JL weighting matrix10 W=eye(7);11 for i=1:712 W(i,i)=w(i);13 end14

15 %qcurrent=qcurrent/180*pi;16 %P is the target vector and Q is the tracking vector17 % xwaypoint=[2 0 0.6];18 % P(:,1)=odinfk2(qcurrent);19 % for i=1:3%linear trajectory grneration20 % Q(i,:)=linspace(P(i),xwaypoint(i),30);21 % end22 P=humanfk3(qcurrent);23 Jv=humanjacobianv(qcurrent);24

25 %Jvinv=inv(Jv);%for square case26 %Jvinv=Jv.';%simple transpose27 %Jvinv=Jv.'*inv(Jv*Jv.');%pseudo inverse28 %Jvinv=Jv.'*inv(Jv*Jv.'+lˆ2*eye(3));%damped pseudo inverse29 Jvinv=inv(W)*Jv.'*inv(Jv*inv(W)*Jv.'+lˆ2*eye(3));30 %weighted damped pseudo inverse31

32 dq=Jvinv*K*(Q-P);33 qcurrent=qcurrent+dq;34 P=humanfk3(qcurrent);35

36 e=Q-P;37 error=sqrt(dot(e,e));38 lim=0;39 out=[qcurrent; error; lim];

A.4 Matlab Code-Weight calculator for pseudo inverse

1 function out=pseudoweight(in)2 qcurrent=in(8:14);3 qprevious=in(1:7);4 qmin=[-15 -90 -100 -10 -130 -10 -20]';5 qmax=[90 90 10 150 90 10 20]';6

7 for i=1:78 H(i)=((qmax(i)-qmin(i))ˆ2)*(2*qcurrent(i)-qmax(i)-qmin(i))/...9 (4*(qmax(i)-qcurrent(i))ˆ2*(qcurrent(i)-qmin(i))ˆ2);

10 H2(i)=((qmax(i)-qmin(i))ˆ2)*(2*qprevious(i)-qmax(i)-qmin(i))/...11 (4*(qmax(i)-qprevious(i))ˆ2*(qprevious(i)-qmin(i))ˆ2);12 if (abs(H(i))-abs(H2(i)))≥013 W(i)=1+abs(H(i));14 else15 W(i)=1;16 end17 end

A APPENDICES v

18 W;19

20 out=[qcurrent; W'];

A.5 Matlab Code-Trajectory generation code

1 clear2 wp(1,:)=[-143 -159 714];3 wp(2,:)=[-255 -350 500];4 wp(3,:)=[-255 -200 500];5 wp(4,:)=[-145 -200 500];6 wp(5,:)=[-145 -350 500];7

8 umax=20;9 amax=40;

10 p=0;11 for j=1:size(wp,1)-112 clear a qu aD a2D th S13 q=wp(j+1,:)-wp(j,:);14 S=sqrt(dot(q,q));15 qu=q/sqrt(dot(q,q));16

17 t(1)=0;18 dt=0.01;19 a(1)=0;20 aD(1)=0;21 a2D(1)=amax;22 th=0;23 flag=0;24 i=1;25 while(flag==0)26 if i==1 && j==127 t(p+i)=0;28 else29 t(p+i)=t(p+i-1)+dt;30 end31 i=i+1;32 if aD(i-1)≥umax33 if (a2D(i-1)>0)34 th=a(i-1);35 end36 a2D(i)=0;37 else38 a2D(i)=amax;39 end40 if (a(i-1)≥(S-th))41 a2D(i)=-amax;42 end43 aD(i)=aD(i-1)+ a2D(i).*dt;44 a(i)=a(i-1)+ aD(i).*dt;45 if (a(i)>(S-th) && aD(i)≤0)46 flag=1;

A APPENDICES vi

47 end48

49 end50 t(p+i)=t(p+i-1)+dt;51 % traj=a.'*qu52 for k=1:size(a,2)53 traj(p+k,:)=wp(j,:)+a(k).'*qu;54 end55 p=p+size(a,2)56

57 end58 trajplan3=[t' traj(:,1:3)].'

A.6 Matlab Code-Forward kinematic model

1 function out=humanfk(in)2 t1=0;3 t2=in(1)*pi/180;4 t3=in(2)*pi/180;5 t4=in(3)*pi/180;6 t5=in(4)*pi/180;7 t6=in(5)*pi/180;8 t7=in(6)*pi/180;9 t8=in(7)*pi/180;

10

11

12 %ee13 out=[ 365*cos(t3)*sin(t1) - (10261*cos(t1))/50 -...14 310*sin(t5)*(cos(t4)*(sin(t1)*sin(t3) - cos(t1)*cos(t2)*cos(t3)) +...15 cos(t1)*sin(t2)*sin(t4)) + 150*sin(t8)*(cos(t6)*(sin(t4)*(sin(t1)*sin(t3)...16 - cos(t1)*cos(t2)*cos(t3)) - cos(t1)*cos(t4)*sin(t2)) +...17 sin(t6)*(cos(t5)*(cos(t4)*(sin(t1)*sin(t3) - cos(t1)*cos(t2)*cos(t3)) +...18 cos(t1)*sin(t2)*sin(t4)) + sin(t5)*(cos(t3)*sin(t1) +...19 cos(t1)*cos(t2)*sin(t3)))) + 310*cos(t5)*(cos(t3)*sin(t1) +...20 cos(t1)*cos(t2)*sin(t3)) +...21 150*cos(t8)*(cos(t7)*(sin(t6)*(sin(t4)*(sin(t1)*sin(t3) -...22 cos(t1)*cos(t2)*cos(t3)) - cos(t1)*cos(t4)*sin(t2)) -...23 cos(t6)*(cos(t5)*(cos(t4)*(sin(t1)*sin(t3) - cos(t1)*cos(t2)*cos(t3)) +...24 cos(t1)*sin(t2)*sin(t4)) + sin(t5)*(cos(t3)*sin(t1) +...25 cos(t1)*cos(t2)*sin(t3)))) + sin(t7)*(sin(t5)*(cos(t4)*(sin(t1)*sin(t3) -...26 cos(t1)*cos(t2)*cos(t3)) + cos(t1)*sin(t2)*sin(t4)) -...27 cos(t5)*(cos(t3)*sin(t1) + cos(t1)*cos(t2)*sin(t3)))) +...28 365*cos(t1)*cos(t2)*sin(t3);...29 (10261*sin(t1))/50 + 365*cos(t1)*cos(t3) -...30 310*sin(t5)*(cos(t4)*(cos(t1)*sin(t3) + cos(t2)*cos(t3)*sin(t1)) -...31 sin(t1)*sin(t2)*sin(t4)) + 310*cos(t5)*(cos(t1)*cos(t3) -...32 cos(t2)*sin(t1)*sin(t3)) +...33 150*sin(t8)*(cos(t6)*(sin(t4)*(cos(t1)*sin(t3) +...34 cos(t2)*cos(t3)*sin(t1)) + cos(t4)*sin(t1)*sin(t2)) +...35 sin(t6)*(cos(t5)*(cos(t4)*(cos(t1)*sin(t3) + cos(t2)*cos(t3)*sin(t1)) -...36 sin(t1)*sin(t2)*sin(t4)) + sin(t5)*(cos(t1)*cos(t3) -...37 cos(t2)*sin(t1)*sin(t3)))) +...

A APPENDICES vii

38 150*cos(t8)*(cos(t7)*(sin(t6)*(sin(t4)*(cos(t1)*sin(t3) +...39 cos(t2)*cos(t3)*sin(t1)) + cos(t4)*sin(t1)*sin(t2)) -...40 cos(t6)*(cos(t5)*(cos(t4)*(cos(t1)*sin(t3) + cos(t2)*cos(t3)*sin(t1)) -...41 sin(t1)*sin(t2)*sin(t4)) + sin(t5)*(cos(t1)*cos(t3) -...42 cos(t2)*sin(t1)*sin(t3)))) + sin(t7)*(sin(t5)*(cos(t4)*(cos(t1)*sin(t3)...43 + cos(t2)*cos(t3)*sin(t1)) - sin(t1)*sin(t2)*sin(t4)) -...44 cos(t5)*(cos(t1)*cos(t3) - cos(t2)*sin(t1)*sin(t3)))) -...45 365*cos(t2)*sin(t1)*sin(t3) ];46 %47

48 %wrist49 % out=[365*cos(t3)*sin(t1) - (10261*cos(t1))/50 -50 % 310*sin(t5)*(cos(t4)*(sin(t1)*sin(t3) - cos(t1)*cos(t2)*cos(t3)) +51 % cos(t1)*sin(t2)*sin(t4)) + 310*cos(t5)*(cos(t3)*sin(t1) +52 % cos(t1)*cos(t2)*sin(t3)) + 365*cos(t1)*cos(t2)*sin(t3);53 % 365*sin(t2)*sin(t3) + 310*sin(t5)*(cos(t2)*sin(t4) +54 % cos(t3)*cos(t4)*sin(t2)) + 310*cos(t5)*sin(t2)*sin(t3) - 359/20;55 % (10261*sin(t1))/50 + 365*cos(t1)*cos(t3) -56 % 310*sin(t5)*(cos(t4)*(cos(t1)*sin(t3) + cos(t2)*cos(t3)*sin(t1)) -57 % sin(t1)*sin(t2)*sin(t4)) + 310*cos(t5)*(cos(t1)*cos(t3) -58 % cos(t2)*sin(t1)*sin(t3)) - 365*cos(t2)*sin(t1)*sin(t3)];59

60 %elbow61

62 % out=[365*cos(t3)*sin(t1) - (10261*cos(t1))/50 +63 % 365*cos(t1)*cos(t2)*sin(t3);64 % 365*sin(t2)*sin(t3) - 359/20;65 % (10261*sin(t1))/50 + 365*cos(t1)*cos(t3) - 365*cos(t2)*sin(t1)*sin(t3)]

A.7 Matlab Code-Differential kinematic model

1 function out=humanjacobianv(in)2 t2=in(1)*pi/180;3 t3=in(2)*pi/180;4 t4=in(3)*pi/180;5 t5=in(4)*pi/180;6 t6=in(5)*pi/180;7 t7=in(6)*pi/180;8 t8=in(7)*pi/180;9

10 out=[ 150*cos(t8)*(sin(t7)*(sin(t5)*(cos(t2)*sin(t4) +...11 cos(t3)*cos(t4)*sin(t2)) + cos(t5)*sin(t2)*sin(t3)) -...12 cos(t7)*(cos(t6)*(cos(t5)*(cos(t2)*sin(t4) + cos(t3)*cos(t4)*sin(t2)) -...13 sin(t2)*sin(t3)*sin(t5)) + sin(t6)*(cos(t2)*cos(t4) -...14 cos(t3)*sin(t2)*sin(t4)))) - 365*sin(t2)*sin(t3) +...15 150*sin(t8)*(sin(t6)*(cos(t5)*(cos(t2)*sin(t4) + cos(t3)*cos(t4)*sin(t2))...16 - sin(t2)*sin(t3)*sin(t5)) - cos(t6)*(cos(t2)*cos(t4) -...17 cos(t3)*sin(t2)*sin(t4))) - 310*sin(t5)*(cos(t2)*sin(t4) +...18 cos(t3)*cos(t4)*sin(t2)) - 310*cos(t5)*sin(t2)*sin(t3),...19 365*cos(t2)*cos(t3) - 150*cos(t8)*(sin(t7)*(cos(t2)*cos(t3)*cos(t5) -...20 cos(t2)*cos(t4)*sin(t3)*sin(t5)) +...21 cos(t7)*(cos(t6)*(cos(t2)*cos(t3)*sin(t5) +...

A APPENDICES viii

22 cos(t2)*cos(t4)*cos(t5)*sin(t3)) - cos(t2)*sin(t3)*sin(t4)*sin(t6))) +...23 150*sin(t8)*(sin(t6)*(cos(t2)*cos(t3)*sin(t5) +...24 cos(t2)*cos(t4)*cos(t5)*sin(t3)) + cos(t2)*cos(t6)*sin(t3)*sin(t4)) +...25 310*cos(t2)*cos(t3)*cos(t5) - 310*cos(t2)*cos(t4)*sin(t3)*sin(t5),...26 150*cos(t8)*(cos(t7)*(sin(t6)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...27 - cos(t5)*cos(t6)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4))) +...28 sin(t5)*sin(t7)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4))) -...29 310*sin(t5)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4)) +...30 150*sin(t8)*(cos(t6)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4)) +...31 cos(t5)*sin(t6)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4))),...32 150*cos(t8)*(sin(t7)*(cos(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...33 + cos(t2)*sin(t3)*sin(t5)) + cos(t6)*cos(t7)*(sin(t5)*(sin(t2)*sin(t4) -...34 cos(t2)*cos(t3)*cos(t4)) - cos(t2)*cos(t5)*sin(t3))) -...35 310*cos(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4)) -...36 310*cos(t2)*sin(t3)*sin(t5) -...37 150*sin(t6)*sin(t8)*(sin(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...38 - cos(t2)*cos(t5)*sin(t3)),...39 150*sin(t8)*(cos(t6)*(cos(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...40 + cos(t2)*sin(t3)*sin(t5)) + sin(t6)*(cos(t4)*sin(t2) +...41 cos(t2)*cos(t3)*sin(t4))) +...42 150*cos(t7)*cos(t8)*(sin(t6)*(cos(t5)*(sin(t2)*sin(t4) -...43 cos(t2)*cos(t3)*cos(t4)) + cos(t2)*sin(t3)*sin(t5)) -...44 cos(t6)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4))),...45 150*cos(t8)*(cos(t7)*(sin(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...46 - cos(t2)*cos(t5)*sin(t3)) + sin(t7)*(cos(t6)*(cos(t5)*(sin(t2)*sin(t4) -...47 cos(t2)*cos(t3)*cos(t4)) + cos(t2)*sin(t3)*sin(t5)) +...48 sin(t6)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4)))),...49 150*cos(t8)*(sin(t6)*(cos(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...50 + cos(t2)*sin(t3)*sin(t5)) - cos(t6)*(cos(t4)*sin(t2) +...51 cos(t2)*cos(t3)*sin(t4))) -...52 150*sin(t8)*(sin(t7)*(sin(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4))...53 - cos(t2)*cos(t5)*sin(t3)) - cos(t7)*(cos(t6)*(cos(t5)*(sin(t2)*sin(t4) -...54 cos(t2)*cos(t3)*cos(t4)) + cos(t2)*sin(t3)*sin(t5)) +...55 sin(t6)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4))));...56 365*cos(t2)*sin(t3) + 150*cos(t8)*(sin(t7)*(sin(t5)*(sin(t2)*sin(t4) -...57 cos(t2)*cos(t3)*cos(t4)) - cos(t2)*cos(t5)*sin(t3)) -...58 cos(t7)*(cos(t6)*(cos(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4)) +...59 cos(t2)*sin(t3)*sin(t5)) + sin(t6)*(cos(t4)*sin(t2) +...60 cos(t2)*cos(t3)*sin(t4)))) +...61 150*sin(t8)*(sin(t6)*(cos(t5)*(sin(t2)*sin(t4) -...62 cos(t2)*cos(t3)*cos(t4)) + cos(t2)*sin(t3)*sin(t5)) -...63 cos(t6)*(cos(t4)*sin(t2) + cos(t2)*cos(t3)*sin(t4))) -...64 310*sin(t5)*(sin(t2)*sin(t4) - cos(t2)*cos(t3)*cos(t4)) +...65 310*cos(t2)*cos(t5)*sin(t3), 365*cos(t3)*sin(t2) +...66 150*sin(t8)*(sin(t6)*(cos(t3)*sin(t2)*sin(t5) +...67 cos(t4)*cos(t5)*sin(t2)*sin(t3)) + cos(t6)*sin(t2)*sin(t3)*sin(t4)) -...68 150*cos(t8)*(cos(t7)*(cos(t6)*(cos(t3)*sin(t2)*sin(t5) +...69 cos(t4)*cos(t5)*sin(t2)*sin(t3)) - sin(t2)*sin(t3)*sin(t4)*sin(t6)) +...70 sin(t7)*(cos(t3)*cos(t5)*sin(t2) - cos(t4)*sin(t2)*sin(t3)*sin(t5))) +...71 310*cos(t3)*cos(t5)*sin(t2) - 310*cos(t4)*sin(t2)*sin(t3)*sin(t5),...72 310*sin(t5)*(cos(t2)*cos(t4) - cos(t3)*sin(t2)*sin(t4)) -...73 150*cos(t8)*(cos(t7)*(sin(t6)*(cos(t2)*sin(t4) +...74 cos(t3)*cos(t4)*sin(t2)) - cos(t5)*cos(t6)*(cos(t2)*cos(t4) -...75 cos(t3)*sin(t2)*sin(t4))) + sin(t5)*sin(t7)*(cos(t2)*cos(t4) -...76 cos(t3)*sin(t2)*sin(t4))) - 150*sin(t8)*(cos(t6)*(cos(t2)*sin(t4) +...

A APPENDICES ix

77 cos(t3)*cos(t4)*sin(t2)) + cos(t5)*sin(t6)*(cos(t2)*cos(t4) -...78 cos(t3)*sin(t2)*sin(t4))), 310*cos(t5)*(cos(t2)*sin(t4) +...79 cos(t3)*cos(t4)*sin(t2)) -...80 150*cos(t8)*(sin(t7)*(cos(t5)*(cos(t2)*sin(t4) +...81 cos(t3)*cos(t4)*sin(t2)) - sin(t2)*sin(t3)*sin(t5)) +...82 cos(t6)*cos(t7)*(sin(t5)*(cos(t2)*sin(t4) + cos(t3)*cos(t4)*sin(t2)) +...83 cos(t5)*sin(t2)*sin(t3))) - 310*sin(t2)*sin(t3)*sin(t5) +...84 150*sin(t6)*sin(t8)*(sin(t5)*(cos(t2)*sin(t4) + cos(t3)*cos(t4)*sin(t2))...85 + cos(t5)*sin(t2)*sin(t3)), -...86 150*sin(t8)*(cos(t6)*(cos(t5)*(cos(t2)*sin(t4) +...87 cos(t3)*cos(t4)*sin(t2)) - sin(t2)*sin(t3)*sin(t5)) +...88 sin(t6)*(cos(t2)*cos(t4) - cos(t3)*sin(t2)*sin(t4))) -...89 150*cos(t7)*cos(t8)*(sin(t6)*(cos(t5)*(cos(t2)*sin(t4) +...90 cos(t3)*cos(t4)*sin(t2)) - sin(t2)*sin(t3)*sin(t5)) -...91 cos(t6)*(cos(t2)*cos(t4) - cos(t3)*sin(t2)*sin(t4))),...92 (-150)*cos(t8)*(cos(t7)*(sin(t5)*(cos(t2)*sin(t4) +...93 cos(t3)*cos(t4)*sin(t2)) + cos(t5)*sin(t2)*sin(t3)) +...94 sin(t7)*(cos(t6)*(cos(t5)*(cos(t2)*sin(t4) + cos(t3)*cos(t4)*sin(t2)) -...95 sin(t2)*sin(t3)*sin(t5)) + sin(t6)*(cos(t2)*cos(t4) -...96 cos(t3)*sin(t2)*sin(t4)))),...97 150*sin(t8)*(sin(t7)*(sin(t5)*(cos(t2)*sin(t4) +...98 cos(t3)*cos(t4)*sin(t2)) + cos(t5)*sin(t2)*sin(t3)) -...99 cos(t7)*(cos(t6)*(cos(t5)*(cos(t2)*sin(t4) + cos(t3)*cos(t4)*sin(t2)) -...

100 sin(t2)*sin(t3)*sin(t5)) + sin(t6)*(cos(t2)*cos(t4) -...101 cos(t3)*sin(t2)*sin(t4)))) -...102 150*cos(t8)*(sin(t6)*(cos(t5)*(cos(t2)*sin(t4) +...103 cos(t3)*cos(t4)*sin(t2)) - sin(t2)*sin(t3)*sin(t5)) -...104 cos(t6)*(cos(t2)*cos(t4) - cos(t3)*sin(t2)*sin(t4)));...105 0,

150*cos(t8)*(cos(t7)*(cos(t6)*(sin(t3)*sin(t5) - cos(t3)*cos(t4)*cos(t5)) + cos(t3)*sin(t4)*sin(t6)) + sin(t7)*(cos(t5)*sin(t3) + cos(t3)*cos(t4)*sin(t5))) -310*cos(t5)*sin(t3) - 150*sin(t8)*(sin(t6)*(sin(t3)*sin(t5) - cos(t3)*cos(t4)*cos(t5)) -cos(t3)*cos(t6)*sin(t4)) - 365*sin(t3) - 310*cos(t3)*cos(t4)*sin(t5),150*cos(t8)*(cos(t7)*(cos(t4)*sin(t3)*sin(t6) + cos(t5)*cos(t6)*sin(t3)*sin(t4)) -sin(t3)*sin(t4)*sin(t5)*sin(t7)) + 150*sin(t8)*(cos(t4)*cos(t6)*sin(t3) -cos(t5)*sin(t3)*sin(t4)*sin(t6)) + 310*sin(t3)*sin(t4)*sin(t5),150*cos(t8)*(sin(t7)*(cos(t3)*sin(t5) + cos(t4)*cos(t5)*sin(t3)) - cos(t6)*cos(t7)*(cos(t3)*cos(t5) -cos(t4)*sin(t3)*sin(t5))) - 310*cos(t3)*sin(t5) + 150*sin(t6)*sin(t8)*(cos(t3)*cos(t5) -cos(t4)*sin(t3)*sin(t5)) - 310*cos(t4)*cos(t5)*sin(t3),150*sin(t8)*(cos(t6)*(cos(t3)*sin(t5) + cos(t4)*cos(t5)*sin(t3)) - sin(t3)*sin(t4)*sin(t6)) + 150*cos(t7)*cos(t8)*(sin(t6)*(cos(t3)*sin(t5) + cos(t4)*cos(t5)*sin(t3)) + cos(t6)*sin(t3)*sin(t4)),150*cos(t8)*(sin(t7)*(cos(t6)*(cos(t3)*sin(t5) + cos(t4)*cos(t5)*sin(t3)) -sin(t3)*sin(t4)*sin(t6)) - cos(t7)*(cos(t3)*cos(t5) - cos(t4)*sin(t3)*sin(t5))),150*cos(t8)*(sin(t6)*(cos(t3)*sin(t5) + cos(t4)*cos(t5)*sin(t3)) + cos(t6)*sin(t3)*sin(t4)) + 150*sin(t8)*(cos(t7)*(cos(t6)*(cos(t3)*sin(t5) + cos(t4)*cos(t5)*sin(t3)) -sin(t3)*sin(t4)*sin(t6)) + sin(t7)*(cos(t3)*cos(t5) - cos(t4)*sin(t3)*sin(t5)))];