-
Notifications
You must be signed in to change notification settings - Fork 3
/
Geometric_Jacobian_Sym.m
59 lines (50 loc) · 1.35 KB
/
Geometric_Jacobian_Sym.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
function [J,T0,T,R0,R,Z,P0] = Geometric_Jacobian_Sym(n,is_rev,a,alpha,d,theta)
%Notes: The first index of T,Z,R cell array should be summed with 1 i.e. index 1 refers to 0(base) frame
%Compute Transformation Matrices for joint i about i-1 ... Be Carefull about indices!
for i=1:n
T{i-1+1,i} = [cos(theta(i)) -sin(theta(i))*cos(alpha(i)) sin(theta(i))*sin(alpha(i)) a(i)*cos(theta(i))
sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i)) a(i)*sin(theta(i))
0 sin(alpha(i)) cos(alpha(i)) d(i)
0 0 0 1];
end
%Compute Transformation Matrices for joint i about 0
T0{1} = T{1-1+1,1};
for i=2:n
T0{i} = T0{i-1}*T{i-1+1,i};
end
%Compute Rotation Matrices about 0
for i=1:n
R0{i} = T0{i}(1:3,1:3);
end
%Compute Rotation Matrix for joint i about i-1
for i=1:n
R{i-1+1,i} = T{i-1+1,i}(1:3,1:3);
end
%Compute Z(i) ... Be Careful about indices!
Z{0+1} = [0 0 1]';
for i=1:n
Z{i+1} = R0{i}*[0 0 1]';
end
%Compute P(0,i)
for i=1:n
P0{i} = T0{i}(1:3,4);
end
%Compute Jacobian Matrix
if is_rev(1)
J(1:3,1) = cross(Z{1-1+1},P0{n});
J(4:6,1) = Z{1-1+1};
else
J(1:3,1) = Z{1-1+1};
J(4:6,1) = [0 0 0]';
end
for i=2:n
if is_rev(i)
J(1:3,i) = cross(Z{i-1+1},(P0{n}-P0{i-1}));
J(4:6,i) = Z{i-1+1};
else
J(1:3,i) = Z{i-1+1};
J(4:6,i) = [0 0 0]';
end
end
% J = simplify(J);
end