-
Notifications
You must be signed in to change notification settings - Fork 12
/
Copy pathInverse.m
212 lines (133 loc) · 5.52 KB
/
Inverse.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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
%% Space Robotics and Autonomy - EEEM029
% Coursework 4/12/18
% Rachel Wiles
% Student ID 6553707
% q1) Finding inverse kinematic solution for all possible arm
% configurations so as to reach the target position and orientation.
% 6dof, with the last joint being a spherical wrist, therefore the last 3
% dof intersect. Arm is joints 1-3, wrist is joints 4-6.
% Position of end effector is determined by configuration of arm.
% Orientation of end effector is determined by the rotation of the wrist.
%% Setup - from question paper
% DH matrix
T = [-1/sqrt(2) 0 1/sqrt(2) 1; 0 -1 0 1; 1/sqrt(2) 0 1/sqrt(2) 0; 0 0 0 1];
% From inspection of DH matrix...
% Lecture 2 slide 33
n_x = -1/sqrt(2);
n_y = 0;
n_z = 1/sqrt(2);
s_x = 0;
s_y = -1;
s_z = 0;
a_x = 1/sqrt(2);
a_y = 0;
a_z = 1/sqrt(2);
p_x = 1;
p_y = 1;
p_z = 0;
% From inspection of table values...
% Twist angles - degrees
alpha_1 = -90;
alpha_2 = 0;
alpha_3 = 90;
alpha_4 = -90;
alpha_5 = 90;
alpha_6 = 0;
% Link length - meters
a_1 = 0;
a_2 = 0.5;
a_3 = 0;
a_4 = 0;
a_5 = 0;
a_6 = 0;
% Offset distance - meters
d_1 = 0;
d_2 = 0.25;
d_3 = 0;
d_4 = 1;
d_5 = 0;
d_6 = 0.5;
%% Calculate position coordinates of arm
% Lecture 3 Slide 15
position_arm = [p_x; p_y; p_z] - (d_6.*[a_x; a_y; a_z]);
parm_x = position_arm (1,1);
parm_y = position_arm (2,1);
parm_z = position_arm (3,1);
%% Calculate joint angle theta 1 & display
% Lecture 3 Slide 18
theta_1_1 = (atan2d(parm_y, parm_x)) - (atan2d(d_2,(sqrt((parm_x^2)+(parm_y^2)-(d_2^2)))));
%% Calculate joint angle theta 2 & display
% Lecture 3 Slide 18
c_1 = cosd(theta_1_1);
s_1 = sind(theta_1_1);
A = (c_1*parm_x) + (s_1*parm_y);
B = (A^2) + (parm_z^2) + (a_2^2) - (d_4^2);
theta_2_1 = (atan2d(A,parm_z)) - (atan2d(B, (sqrt((A^2) + (parm_z^2) - (B^2)))));
%% Calculate joint angle theta 3 & display
% Lecture 3 slide 19
c_2 = cosd(theta_2_1);
s_2 = sind(theta_2_1);
theta_3_1 = (atan2d((A - (a_2*c_2)),(parm_z + (a_2*s_2)))) - theta_2_1;
%% Calculate joint angle theta 4 & display
% Lecture 3 slide 20
c_23 = cosd(theta_2_1 + theta_3_1);
s_23 = sind(theta_2_1 + theta_3_1);
theta_4_1 = atan2d(((-s_1*a_x) + (c_1*a_y)),(c_23*((c_1*a_x) + (s_1*a_y)) - (s_23*a_z)));
%% Calculate joint angle theta 5 & display
% Lecture 3 slide 20
theta_5_1 = atan2d((sqrt(((c_1*c_23*a_x) + (s_1*c_23*a_y) - (s_23*a_z))^2 + ((-s_1*a_x) + (c_1*a_y))^2)),(s_23*((c_1*a_x) + (s_1*a_y)) + (c_23*a_z)));
%% Calculate joing angle theta 6 & display
% Lecture 3 slide 20
theta_6_1 = atan2d((s_23*((c_1*s_x) + (s_1*s_y)) + (c_23*s_z)), -(s_23*((c_1*n_x) + (s_1*n_y)) + (c_23*n_z)));
%% Repeat with sign change
theta_1_2 = (atan2d(parm_y, parm_x)) - (atan2d(d_2,(-sqrt((parm_x^2)+(parm_y^2)-(d_2^2))))); %Negative SQRT here
c_1 = cosd(theta_1_2);
s_1 = sind(theta_1_2);
A = (c_1*parm_x) + (s_1*parm_y);
B = (A^2) + (parm_z^2) + (a_2^2) - (d_4^2);
theta_2_2 = (atan2d(A,parm_z)) - (atan2d(B, (sqrt((A^2) + (parm_z^2) - (B^2)))));
c_2 = cosd(theta_2_2);
s_2 = sind(theta_2_2);
theta_3_2 = (atan2d((A - (a_2*c_2)),(parm_z + (a_2*s_2)))) - theta_2_2;
c_23 = cosd(theta_2_2 + theta_3_2);
s_23 = sind(theta_2_2 + theta_3_2);
theta_4_2 = atan2d(((-s_1*a_x) + (c_1*a_y)),(c_23*((c_1*a_x) + (s_1*a_y)) - (s_23*a_z)));
theta_5_2 = atan2d((sqrt(((c_1*c_23*a_x) + (s_1*c_23*a_y) - (s_23*a_z))^2 + ((-s_1*a_x) + (c_1*a_y))^2)),(s_23*((c_1*a_x) + (s_1*a_y)) + (c_23*a_z)));
theta_6_2 = atan2d((s_23*((c_1*s_x) + (s_1*s_y)) + (c_23*s_z)), -(s_23*((c_1*n_x) + (s_1*n_y)) + (c_23*n_z)));
%% Repeat with sign change
theta_1_3 = (atan2d(parm_y, parm_x)) - (atan2d(d_2,(-sqrt((parm_x^2)+(parm_y^2)-(d_2^2))))); %Negative SQRT here
c_1 = cosd(theta_1_3);
s_1 = sind(theta_1_3);
A = (c_1*parm_x) + (s_1*parm_y);
B = (A^2) + (parm_z^2) + (a_2^2) - (d_4^2);
theta_2_3 = (atan2d(A,parm_z)) - (atan2d(B, (-sqrt((A^2) + (parm_z^2) - (B^2))))); %Negative SQRT here
c_2 = cosd(theta_2_3);
s_2 = sind(theta_2_3);
theta_3_3 = (atan2d((A - (a_2*c_2)),(parm_z + (a_2*s_2)))) - theta_2_3;
c_23 = cosd(theta_2_3 + theta_3_3);
s_23 = sind(theta_2_3 + theta_3_3);
theta_4_3 = atan2d(((-s_1*a_x) + (c_1*a_y)),(c_23*((c_1*a_x) + (s_1*a_y)) - (s_23*a_z)));
theta_5_3 = atan2d((sqrt(((c_1*c_23*a_x) + (s_1*c_23*a_y) - (s_23*a_z))^2 + ((-s_1*a_x) + (c_1*a_y))^2)),(s_23*((c_1*a_x) + (s_1*a_y)) + (c_23*a_z)));
theta_6_3 = atan2d((s_23*((c_1*s_x) + (s_1*s_y)) + (c_23*s_z)), -(s_23*((c_1*n_x) + (s_1*n_y)) + (c_23*n_z)));
%% Repeat with sign change
theta_1_4 = (atan2d(parm_y, parm_x)) - (atan2d(d_2,(sqrt((parm_x^2)+(parm_y^2)-(d_2^2)))));
c_1 = cosd(theta_1_4);
s_1 = sind(theta_1_4);
A = (c_1*parm_x) + (s_1*parm_y);
B = (A^2) + (parm_z^2) + (a_2^2) - (d_4^2);
theta_2_4 = (atan2d(A,parm_z)) - (atan2d(B, (-sqrt((A^2) + (parm_z^2) - (B^2))))); %Negative SQRT here
c_2 = cosd(theta_2_4);
s_2 = sind(theta_2_4);
theta_3_4 = (atan2d((A - (a_2*c_2)),(parm_z + (a_2*s_2)))) - theta_2_4;
c_23 = cosd(theta_2_4 + theta_3_4);
s_23 = sind(theta_2_4 + theta_3_4);
theta_4_4 = atan2d(((-s_1*a_x) + (c_1*a_y)),(c_23*((c_1*a_x) + (s_1*a_y)) - (s_23*a_z)));
theta_5_4 = atan2d((sqrt(((c_1*c_23*a_x) + (s_1*c_23*a_y) - (s_23*a_z))^2 + ((-s_1*a_x) + (c_1*a_y))^2)),(s_23*((c_1*a_x) + (s_1*a_y)) + (c_23*a_z)));
theta_6_4 = atan2d((s_23*((c_1*s_x) + (s_1*s_y)) + (c_23*s_z)), -(s_23*((c_1*n_x) + (s_1*n_y)) + (c_23*n_z)));
%% Combine thetas
theta_1 = [theta_1_1 theta_1_2 theta_1_3 theta_1_4]
theta_2 = [theta_2_1 theta_2_2 theta_2_3 theta_2_4]
theta_3 = [theta_3_1 theta_3_2 theta_3_3 theta_3_4]
theta_4 = [theta_4_1 theta_4_2 theta_4_3 theta_4_4]
theta_5 = [theta_5_1 theta_5_2 theta_5_3 theta_5_4]
theta_6 = [theta_6_1 theta_6_2 theta_6_3 theta_6_4]