Inverse Kinematic Control

of a Prosthetic arm
Project Report-Engi9940

B.M.Oscar De Silva
201070588
Faculty of Engineering and Applied Science
Memorial University of Newfoundland
Fall 2010

submitted to:
Dr. George Mann
 Abstract
Robotic prosthetics are devices worn by humans with limb amputations. These
operate in unstructured environments performing realtime tasks. Thus require kine-
matic controllers to perform online joint trajectory planning. The project performs
a comparative study on online trajectory generation methods available for robotic
manipulators. Its implemented with the objective of selecting a suitable kinematic
control 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. The
control approaches were tested for redundancy resolution, joint limits management
and 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 2
2.1 Closed Loop Inverse Kinematics Algorithm . . . . . . . . . . . . . . . . . . 3
2.2 Jacobian Transpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.3 Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.4 Task Priority Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . 4
2.5 Weighted Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3 Approach 5
3.1 Mathematical Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
3.2 Task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3.3 Visualization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.4 Inverse Kinematics Controller . . . . . . . . . . . . . . . . . . . . . . . . . 8

4 Results 9
4.1 Jacobian Transpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4.2 Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4.3 Damped Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4.4 Task Priority Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . 11
4.5 Weighted Pseudo Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5 Conclusion 14

A Appendices i
A.1 Simulink model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . i
A.2 Matlab Code-Mathematical model generation code . . . . . . . . . . . . . i
A.3 Matlab Code-Inverse kinematics code . . . . . . . . . . . . . . . . . . . . . iii
A.4 Matlab Code-Weight calculator for pseudo inverse . . . . . . . . . . . . . . iv
A.5 Matlab Code-Trajectory generation code . . . . . . . . . . . . . . . . . . . v
A.6 Matlab Code-Forward kinematic model . . . . . . . . . . . . . . . . . . . . vi
A.7 Matlab Code-Differential kinematic model . . . . . . . . . . . . . . . . . . vii
1 INTRODUCTION 1

1 Introduction
Motion planning with kinematic control has been a widely discussed topic in robotic
manipulators. In most applications the controllers poses the luxury of preprocessing
the trajectory of the end effector and the joints. This is due to the prior availability
of the information of the task performed and the environment the task is performed.
Industrial robots performing repetitive tasks such as painting, assembly etc.. are main
application areas of this method. But as manipulators become complex with autonomous
task planning and operates in unstructured environments, realtime solutions to the inverse
kinematic 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 kinematics
controller should be in place to generate the required trajectories for autonomous or semi
autonomous operations. This should be performed while overcoming the sub problems of
redundancy, singularity and joint limits. These subproblems will be discussed in detail in
chapter2.
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 subproblems
and 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 algorithm
a 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]. The
performance of these methods base upon the application understudy and properties they
require.
So The project aims to perform a comparative study on 5 selected inverse kinematic
controllers on a 7DOF prosthetic arm system. The candidates were assessed for addressing
redundancy, joint limits and singularity with aim to select the suitable IK controller for
prosthetic 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 proposed
by 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 with
specific details on applying the IK controllers. Chapter 4 and 5 performs a comparative
study on the results with a selection of a suitable controller for prosthetic applications
remarked 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 of
the end effector. The objective of an Inverse kinematic controller is to find suitable joint
trajectories qr (t) of a manipulator corresponding to the desired end effector positions and
orientations ηee (t). The outputs of the inverse kinematic controller are the joint variables
qr (t) which would be used as reference values for the dynamic control law of the robot. A
mapping function f (q) from joint variables qr (t)to end effector variables ηee (t)defines the
forward 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 for
qr (t) using trigonometric relations. But as the number of DOF‘s in the system become
larger, the number of possible solutions increases requiring an optimal selection between
the solutions. But at a differential level the relationship between joint variables and the
end 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 velocities
to end effector velocities.
 ∂η1 ∂η1 ∂η1 
∂q1 ∂q2
··· ∂qn
 ∂η2 ∂η2
··· ∂η2 
 ∂q1 ∂q2 ∂qn 
J = . .. ... ..  (4)
 .. . . 
∂ηn ∂ηn ∂ηn
∂q1 ∂q2
··· ∂qn

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 a
differential approach. In forming an IK solution there are certain subproblems which
should be incorporated in the IK algorithm.

Redundancy Resolution Kinematic redundancy is characterized by the appearance of

multiple solutions to the IK problem. An IK algorithm should perform an intu-
itive selection among the available solutions. At a differential Inverse kinematic
approach, this selection is done between the neighboring solutions of the robots
current configuration.

Singular Value Decomposition Singular configurations are characterized by the robot

having an non invertible jacobian matrix for the certain configuration. Algorithmic
2 THEORY 3

singularities occur when such singularities occur due to the trajectories selected by
the controller. So a proper singular value decomposing strategy should be in place
for 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 requires
liberalization of the jacobian about an operating point. This causes a drifting error in the
calculation of the desired joint variables. Closed loop inverse kinematic (CLIK) algorithm
is 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 effector
position. 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 point
q(t − 1). This is quite straight forward for a square matrix case. But for a general case
a generalized matrix inversion should be performed to find the solution. In preceding
sections the use of generalized matrix inversion and its variants will be discussed.

2.2 Jacobian Transpose

The Transpose of the Jacobian J T form a simple alternative for the generalized inverse
J † . Although this is not a general inversion of the matrix, the CLIK algorithm accepts
this to produces solutions for simple systems. This method is considered unstable and
susceptible to algorithmic singularities.

2.3 Pseudo Inverse

Pseudo inverse is the simplest form of generalized inversion to a matrix. The solutions
given using this inversion corresponds to the minimization of the joint velocities in a least
squares sense.

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

2 THEORY 4

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

J † (q) = J T (q)(J(q) ∗ J T (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 the
system may not be compatible. Even when the solutions are valid, it would not be the
optimum solution from the many solutions available due to redundancy. So additional
constraints 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 pseudo
inverse 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 = Jp† (q)∆ηp + (In − Jp† (q)JpT (q)) Js† (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 for
the 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 −1 J T (q)(J(q)W −1 J T (q) + λ ∗ I)−1 (10)

The weight matrix W can be designed so that it constraints movement of certain
joints. Its designed so that it introduces the joint limit constraints by selecting the
diagonal elements of the weight matrix W to take the form defined by equations 11 and
12.

∂H(q) (qi,max − qi,max )2 (2qi − qi,max − qi,min )

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

1 + | ∂H

∂qi
| if ∆|∂H/∂ qi | ≥ 0
Wi,i = (12)
1 if ∆|∂H/∂ qi | < 0
This ensures that movements are amplified in the joints in between closer to the middle
of 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 an

Upper extremity prosthetic. 7DOF‘s capture the missing degrees of freedoms of an above
elbow amputee. The end effector operations(hand operations) were not captured in this
initial model, only positioning of the end effector was studied.
The selected task was a pick and place operation. This was selected within the
workspace of the manipulator. A critical review of the IK controllers would required
them to be tested in number of different tasks but for this study a single pick and place
was 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 formulating
the 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 marked
following the physical order of actuation that is evident in arms. Anthropometric data
with respect to arm lengths and joint limits were used in developing the mathematical
model.

VR
Tee = T0V R ∗ Tee
0
(13)
3 APPROACH 6

Frame θi di ai αi
1 θ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

 
−1 0 0 0
0 0 1 0
T0V R =
0
 (14)
1 0 0
0 0 0 1

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. This
was also used to formulate the jacobian matrix using the definition of the jacobian as
found in variational calculus. This formulates a 3x7 velocity jacobian to the 7 DOF
system. Because the Inverse kinematic algorithm attempts to track position only in the
end effector, the angular velocity jacobian is not incorporated in the study. However this
can 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 kinematic
algorithm. Way points were selected in the manipulators task space ensuring a physical
singularity will not be reached in the process. Desired maximum accelerations of 40 mm s2
and Maximum end effector velocities 20 mms
were set, so that each way point is followed in
the
 preset trajectory pattern. Theinitial 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 714
WP2 -255 -350 500
WP3 -255 -200 500
WP4 -145 -200 500
WP5 -145 -350 500

Table 3: Waypoints selected for the task

−100 −100

start point start point

−150 −150

−200 −200
Y(mm)

Y(mm)
−250 −250

−300 −300

−350 −350

−400 −400
−400 −350 −300 −250 −200 −150 −100 −400 −350 −300 −250 −200 −150 −100
X(mm) X(mm)

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 is
arrived taking small steps.

Case B Intermediate end effector trajectories between the waypoints 1 and 2 were not
generated. 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 skeleton
model was developed using VRealm Builder which was operated using a Matlab Graphical
User Interface across a simulink Platform. The visualization was necessary to verify the
models visually.
The coordinate system of the Virtual reality platform was first translated to the base
coordinates of the prosthetic model using euler angles (14). Then suitable adjustments
to the input joint angles to the system were made to match the positive joint rotations
and 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. The
initial conditions were set to a non singular configuration for desirable operation. Within
the CLIK algorithm the jacobian inverse was changed based on the theory presented in
the methods discussed in Chapter 2. For the weighted pseudo inverse approach the weight
matrix was developed based on the joint conditions which are apparent in human arms
presented 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 instability
is evident in Case A where it reaches a singularity and fails to converge to a solution.

100 q1
q2
Joint variables(deg)

q3
50 q4
q5
q6
0
q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
time(s)

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

A

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 successful
in 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 singularities
of the method.
Further it was observed that the algorithm does not take in to account the joint
limits of the system. This leads to physically incompatible solutions to be selected by the
approach. So the Pseudo inverse method lacks joint limit management and is prone to
algorithmic singularities.

4.3 Damped Pseudo Inverse

Damped pseudo inverse mathematically introduces damping to the convergence of the
algorithm. 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

100 q1
q2
Joint variables(deg) q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
time(s)

Figure 4: Jacobian Transpose-Case B

100 q1
q2
Joint variables(deg)

q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
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 limits
management was made.
4 RESULTS 11

100 q1
q2
Joint variables(deg) q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
time(s)

Figure 6: Pseudo Inverse-Case B

100 q1
q2
Joint variables(deg)

q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
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 previous
model used to test IK algorithms. This was a underwater robotic manipulator system. the
4 RESULTS 12

100 q1
q2
Joint variables(deg) q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
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 in
implementation of the process.
Then the amplitude of the sinusoidal movement in y direction was increased. When
the amplitude exceeds the levels which adhering to the secondary task is no longer possible
the algorithm seeks solutions which considers the primary task only with less concern on
the 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 secondary
objective. So a task priority method of inverse kinematics is not suitable to incorporate
joint limits but it proves to be of much use in introducing additional secondary constraints
such 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 has
achieved good results for both the cases in terms of singularity management and in-
corporating joint limits. As the joint limits itself introduce configurations of singularity
the method simultaneously achieves both requirements in a certain extent.
Although the method ensures the output solutions are within joint limits it cannot
recover 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

100 q1
q2
Joint variables(deg) q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
time(s)

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

100 q1
q2
Joint variables(deg)

q3
50 q4
q5
q6
0 q7

−50

1
Joint limit

0
−1
5
Error(mm)

0
0 5 10 15 20 25 30 35 40 45 50
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- Singularities en- exceeds in Case A
stable in Case B countered in Case and B
A
Pseudo Inverse 4mm max in Case A Singularities en- exceeds in Case A
and Case B countered in Case and B
B
Damped Pseudo Inverse 4mm max in Case A Singularities en- exceeds in Case A
and Case B countered in Case and B
B but damped
w.r.t. pseudo
inverse method
Weighted Pseudo Inverse 1mm max in Case A Singularities not satisfied in both
and Case B encountered Case A and B

Table 4: Summary of IK algorithm performances

The compared IK algorithms correspond to different solutions to the IK problem. All

methods were successful in solving the IK problem for the given task for certain time
periods. The first three methods does not apply joint limits to its solution strategy
but performs reasonably well in solving joint variables for the task. Task priority and
Weighted pseudo methods enable incorporation of additional constraints to the Inverse
kinematic problem. But a task priority method fails in meeting strict constraints such
as joint limits. This would have more application in incorporating secondary constraints
such 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 evident
in previous methods were hardly encountered in the Weighted method. It was also able
fulfill 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‘s
were encountered at certain time steps. So with a suitable singular value management
strategy to overcome singularity exposure, is recommended for Inverse kinematic control
of 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 suitable
singular 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 to
the system. A secondary task is designed so that its desired values corresponds to non
singular configurations. So such a hybrid inverse kinematic strategy would better serve
as a suitable system for hardware implementation. This could be further improved for
energy minimization and self collision avoidance.
A simplification to the 7DOF model can be applied by kinematic decoupling at the
wrist joint. 5 DOF model without the wrist to solve the inverse kinematic problem would
reduce 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 augmentation
and 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 of
vehicle-manipulator systems,” IEEE CONTROL SYSTEMS MAGAZINE, 2008.

[3] B. Dariush, Y. Zhu, A. Arumbakkam, and K. Fujimura, “Constrained closed loop

inverse kinematics,” in Robotics and Automation (ICRA), 2010 IEEE International
Conference on, pp. 2499 –2506, May 2010.

[4] A. Ben-Israel and T. Greville, Generalized inverses: Theory and applications. Springer
Verlag, 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 Manipulator
4 %Author: Oscar De Silva
5 %22 nov 2010
6 clear;
7 %include required symbols and the dh parameters
8 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);%dummy
13 Tlink(:,:,1)=dhtsym(0,-17.95,205.22,-90);%theta,d,a,alpha
14 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 cordinates
23 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 here
36 %calculating transformation matrices
37 fprintf('calculating transformation matrices...\n');
38 for i=1:size(Tlink,3)
39 if i==1
40 Tbase(:,:,i)=Tlink(:,:,i);
41 else
42 Tbase(:,:,i)=simplify(Tbase(:,:,i-1)*Tlink(:,:,i));
43 end
44 end
45
46 %Extracting rotation matrices
47 fprintf('Extracting rotation matrices...\n');
48 for i=1:size(Tlink,3)
49 Rbase(:,:,i)=Tbase(1:3,1:3,i);
50 end
51
52 %position vectors of center of mass
53
54 %z biased to overcome zworld(0) problem
55 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 end
59 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) problem
65 for i=2:size(Tlink,3)-j+1
66 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 end
69 end
70 %verified
71

72 fprintf('Calculating link Jacobians...\n');

73 J(:,:,1)=[t1;0;0;0;0;0]; %dummy
74 for j=1:size(Tlink,3)
75 for i=1:j
76 if isempty(symvar(Tlink(1:3,1:3,i)))
77 J(:,i,j)=[zworld(:,i);0;0;0];
78 else
79 J(:,i,j)=[cross(zworld(:,i),pend(1:3,i,j));zworld(:,i)];
80 end
81 end
82 end
83 %verified
84 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)-1
96 Jv(:,i)=diff(Teetovr(1:3,4),q(i,:));
97 end
98 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 point
4 qcurrent=in(4:10);
5 w=in(11:17);
6 K=1*eye(3);%do not damp in feeded waypoint method
7 l=0.5;
A APPENDICES iv

8
9 %calculate JL weighting matrix
10 W=eye(7);
11 for i=1:7
12 W(i,i)=w(i);
13 end
14
15 %qcurrent=qcurrent/180*pi;
16 %P is the target vector and Q is the tracking vector
17 % xwaypoint=[2 0 0.6];
18 % P(:,1)=odinfk2(qcurrent);
19 % for i=1:3%linear trajectory grneration
20 % Q(i,:)=linspace(P(i),xwaypoint(i),30);
21 % end
22 P=humanfk3(qcurrent);
23 Jv=humanjacobianv(qcurrent);
24
25 %Jvinv=inv(Jv);%for square case
26 %Jvinv=Jv.';%simple transpose
27 %Jvinv=Jv.'*inv(Jv*Jv.');%pseudo inverse
28 %Jvinv=Jv.'*inv(Jv*Jv.'+lˆ2*eye(3));%damped pseudo inverse
29 Jvinv=inv(W)*Jv.'*inv(Jv*inv(W)*Jv.'+lˆ2*eye(3));
30 %weighted damped pseudo inverse
31
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:7
8 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)))≥0
13 W(i)=1+abs(H(i));
14 else
15 W(i)=1;
16 end
17 end
A APPENDICES v

18 W;
19
20 out=[qcurrent; W'];

A.5 Matlab Code-Trajectory generation code

1 clear
2 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)-1
12 clear a qu aD a2D th S
13 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==1
27 t(p+i)=0;
28 else
29 t(p+i)=t(p+i-1)+dt;
30 end
31 i=i+1;
32 if aD(i-1)≥umax
33 if (a2D(i-1)>0)
34 th=a(i-1);
35 end
36 a2D(i)=0;
37 else
38 a2D(i)=amax;
39 end
40 if (a(i-1)≥(S-th))
41 a2D(i)=-amax;
42 end
43 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 end
48
49 end
50 t(p+i)=t(p+i-1)+dt;
51 % traj=a.'*qu
52 for k=1:size(a,2)
53 traj(p+k,:)=wp(j,:)+a(k).'*qu;
54 end
55 p=p+size(a,2)
56
57 end
58 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 %ee
13 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 %wrist
49 % 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 %elbow
61
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
150*cos(t8)*(cos(t7)*(cos(t6)*(sin(t3)*sin(t5) - cos(t3)*cos(t4)*cos(t5)) + cos(t3)
310*cos(t5)*sin(t3) - 150*sin(t8)*(sin(t6)*(sin(t3)*sin(t5) - cos(t3)*cos(t4)*cos(t
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(t4)*sin(t3)*sin(t5))) - 310*cos(t3)*sin(t5) + 150*sin(t6)*sin(t8)*(cos(t3)*cos(
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)*
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(t3)*sin(t4)*sin(t6)) + sin(t7)*(cos(t3)*cos(t5) - cos(t4)*sin(t3)*sin(t5)))];