forked from rbenefo/PotentialFieldPlanner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
jacobiancalc.m
72 lines (58 loc) · 3.07 KB
/
jacobiancalc.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
%% Lynx Jacobian
syms t1 t2 t3 t4 t5 t6 d1 a2 a3 d5 d4 d6
% D-H Matrix
syms pi
dhmatrix = [d1 0 -pi/2 t1;...
0 a2 0 t2-pi/2;...
0 a3 0 t3+pi/2;...
0 0 -pi/2 t4-pi/2;...
d5 0 pi/2 t5+pi/2;...
d6 0 0 t6];
%Homogeneous Transformation Matrices for each joint
A1 = [cos(dhmatrix(1,4)) -sin(dhmatrix(1,4))*cos(dhmatrix(1,3)) sin(dhmatrix(1,4))*sin(dhmatrix(1,3)) dhmatrix(1,2)*cos(dhmatrix(1,4));...
sin(dhmatrix(1,4)) cos(dhmatrix(1,4))*cos(dhmatrix(1,3)) -cos(dhmatrix(1,4))*sin(dhmatrix(1,3)) dhmatrix(1,2)*sin(dhmatrix(1,4));...
0 sin(dhmatrix(1,3)) cos(dhmatrix(1,3)) dhmatrix(1,1);...
0 0 0 1];
A2 = [cos(dhmatrix(2,4)) -sin(dhmatrix(2,4))*cos(dhmatrix(2,3)) sin(dhmatrix(2,4))*sin(dhmatrix(2,3)) dhmatrix(2,2)*cos(dhmatrix(2,4));...
sin(dhmatrix(2,4)) cos(dhmatrix(2,4))*cos(dhmatrix(2,3)) -cos(dhmatrix(2,4))*sin(dhmatrix(2,3)) dhmatrix(2,2)*sin(dhmatrix(2,4));...
0 sin(dhmatrix(2,3)) cos(dhmatrix(2,3)) dhmatrix(2,1);...
0 0 0 1];
A3 = [cos(dhmatrix(3,4)) -sin(dhmatrix(3,4))*cos(dhmatrix(3,3)) sin(dhmatrix(3,4))*sin(dhmatrix(3,3)) dhmatrix(3,2)*cos(dhmatrix(3,4));...
sin(dhmatrix(3,4)) cos(dhmatrix(3,4))*cos(dhmatrix(3,3)) -cos(dhmatrix(3,4))*sin(dhmatrix(3,3)) dhmatrix(3,2)*sin(dhmatrix(3,4));...
0 sin(dhmatrix(3,3)) cos(dhmatrix(3,3)) dhmatrix(3,1);...
0 0 0 1];
A4 = [cos(dhmatrix(4,4)) -sin(dhmatrix(4,4))*cos(dhmatrix(4,3)) sin(dhmatrix(4,4))*sin(dhmatrix(4,3)) dhmatrix(4,2)*cos(dhmatrix(4,4));...
sin(dhmatrix(4,4)) cos(dhmatrix(4,4))*cos(dhmatrix(4,3)) -cos(dhmatrix(4,4))*sin(dhmatrix(4,3)) dhmatrix(4,2)*sin(dhmatrix(4,4));...
0 sin(dhmatrix(4,3)) cos(dhmatrix(4,3)) dhmatrix(4,1);...
0 0 0 1];
A5 = [cos(dhmatrix(5,4)) -sin(dhmatrix(5,4))*cos(dhmatrix(5,3)) sin(dhmatrix(5,4))*sin(dhmatrix(5,3)) dhmatrix(5,2)*cos(dhmatrix(5,4));...
sin(dhmatrix(5,4)) cos(dhmatrix(5,4))*cos(dhmatrix(5,3)) -cos(dhmatrix(5,4))*sin(dhmatrix(5,3)) dhmatrix(5,2)*sin(dhmatrix(5,4));...
0 sin(dhmatrix(5,3)) cos(dhmatrix(5,3)) dhmatrix(5,1);...
0 0 0 1];
A6 = [cos(dhmatrix(6,4)) -sin(dhmatrix(6,4))*cos(dhmatrix(6,3)) sin(dhmatrix(6,4))*sin(dhmatrix(6,3)) dhmatrix(6,2)*cos(dhmatrix(6,4));...
sin(dhmatrix(6,4)) cos(dhmatrix(6,4))*cos(dhmatrix(6,3)) -cos(dhmatrix(6,4))*sin(dhmatrix(6,3)) dhmatrix(6,2)*sin(dhmatrix(6,4));...
0 sin(dhmatrix(6,3)) cos(dhmatrix(6,3)) dhmatrix(6,1);...
0 0 0 1];
T06 = A1*A2*A3*A4*A5*A6;
T05 = A1*A2*A3*A4*A5;
T04 = A1*A2*A3*A4;
T03 = A1*A2*A3;
T02 = A1*A2;
T01 = A1;
%Coordinates of end effector
pe = [T04(1,4), T04(2,4)+34, T04(3,4)+34];
%Linear velocity Jacobian (Differentiation method)
Jv = [diff(pe(1,1),t1), diff(pe(1,1),t2), diff(pe(1,1),t3), diff(pe(1,1),t4);...
diff(pe(1,2),t1), diff(pe(1,2),t2), diff(pe(1,2),t3), diff(pe(1,2),t4);...
diff(pe(1,3),t1), diff(pe(1,3),t2), diff(pe(1,3),t3), diff(pe(1,3),t4)];
%Orientation of each joint
R10 = T01(1:3,1:3);
R20 = T02(1:3,1:3);
R30 = T03(1:3,1:3);
R40 = T04(1:3,1:3);
R50 = T05(1:3,1:3);
R60 = T06(1:3,1:3);
z0 = [0;0;1];
%Angular velocity Jacobian and overall velocity Jacobian
Jw = [z0 R10*z0 R20*z0 R30*z0];
J = simplify([Jv;Jw]);