-
Notifications
You must be signed in to change notification settings - Fork 0
/
inverse_jacobian_one_iter.m
75 lines (71 loc) · 2.13 KB
/
inverse_jacobian_one_iter.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
close all; clear;
%Input parameter
origin = [0,0];
DoF = 3;
a = [10 10 5];
target = [5 10];
%initialize parameter
theta = sym('theta',[1 DoF]);
d = zeros(1, DoF);
alpha = zeros(1, DoF);
delta_theta = zeros(1, DoF);
DHmatrices = cell(DoF,1);
T = eye(4);
positions = cell(DoF,1);
joints_x = sym('joints_x',[1 DoF]);
joints_y = sym('joints_y',[1 DoF]);
theta_sub = zeros(1, DoF);
b = 0;
dis_errors = [];
iters = [];
iter = 0;
%DHmatrix
for i=1:DoF
DHmatrices{i} = DHmatrix(theta(i),d(i),a(i),alpha(i));
T = T*DHmatrix(theta(i),d(i),a(i),alpha(i));
positions{i} = transpose(T(1:2,4));
joints_x(i) = positions{i}(1);
joints_y(i) = positions{i}(2);
end
%Jacobian
J = jacobian(T(1:2,4), theta);
%update
converge_begin = cputime;
while b==0
delta_theta = reshape(delta_theta, [1, DoF]);
delta_theta = round(double(delta_theta),5);
theta_sub = theta_sub + 0.01*radtodeg(delta_theta);
disp(theta_sub)
%step1: forward kinematicsa
T_sub = subs(T, theta, theta_sub);
disp('end effector pos:');disp(T_sub);
joints_x_sub = subs(joints_x, theta, theta_sub);
joints_y_sub = subs(joints_y, theta, theta_sub);
plot([0 joints_x_sub],[0 joints_y_sub],'-o','LineWidth',4);
axis([-31,31,-31,31]);
grid on;
%calculate jacobian
J_sub = subs(J, theta, theta_sub);
disp('J:'); disp(J_sub);
% update delta_theta
delta_p = target - T_sub(1:2,4)';
delta_theta = pinv(J_sub)*delta_p';
dis_error = norm(delta_p);
dis_errors = [dis_errors dis_error];
iters = [iters iter];
disp('dis_error');
disp(round(double(dis_error),3));
text(joints_x_sub(DoF),joints_y_sub(DoF),[' (', num2str(round(double(joints_x_sub(DoF)),3),3), ...
', ', num2str(round(double(joints_y_sub(DoF)),3),3), ')']);
text(-25,-17,'Orinerror:','Color','red','FontSize',12)
text(-25,-20,num2str(round(double(dis_error),3)),'Color','red','FontSize',12)
if dis_error <= 0.2
b=1;
converge_end = cputime;
time = converge_end - converge_begin;
% figure
% plot(iters, dis_errors, '-o','LineWidth',4);
end
iter = iter+1;
pause(0.01);
end