Inverse Kinematic Control of A Prosthetic Arm
Inverse Kinematic Control of A Prosthetic Arm
Inverse Kinematic Control of A Prosthetic Arm
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.
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.
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.
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.
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;
Damping can be introduced to the mapping using equation 8. This handles inverting
of ill conditioned matrices.
∆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.
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
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
−1 0 0 0
0 0 1 0
T0V R =
0
(14)
1 0 0
0 0 0 1
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
−100 −100
−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
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)
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)
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)
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)
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
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)
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.
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)
limits.
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
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.
[4] A. Ben-Israel and T. Greville, Generalized inverses: Theory and applications. Springer
Verlag, 2003.
A Appendices
A.1 Simulink model
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
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
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];
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'];
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)].'
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)]
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