-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtest_J.m
137 lines (117 loc) · 4.6 KB
/
test_J.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
clear;
robot = robot_iiwa();
test_kuka_examples;
%% Test relationship between J_space and J_body
% From W7-L1, pg 13:
% Jb = [Ad Tbs] Js
% Js = [Ad Tsb] Jb
fprintf('%7s %12s %12s\n', 'Case #', 'S vs Trans B', 'B vs Trans S');
test_cases = 1:16; %[1]; %1:size(joints,1);
for i_test_cases = 1:numel(test_cases)
i = test_cases(i_test_cases);
joint_angles = joints(i,:);
Js = J_space(robot, joint_angles);
Jb = J_body(robot, joint_angles);
Tsb = FK_space(robot, joint_angles);
Tbs = inv(Tsb);
diffs = Js - adjoint_transform(Tsb) * Jb;
diffb = Jb - adjoint_transform(Tbs) * Js;
errs = norm(diffs);
errb = norm(diffb);
fprintf('Case %2d %12g %12g\n', i, errs, errb);
assert(errs < 1e-7, 'Space is not transformed body');
assert(errb < 1e-7, 'Body is not transformed space');
end
disp('All good');
%% Test Jacobian function J_space with a numerical example
dummyrobot.dof = 4;
dummyrobot.screw = [[0; 0; 1; 0; 0.2; 0.2], ...
[1; 0; 0; 2; 0; 3], ...
[0; 1; 0; 0; 2; 1], ...
[1; 0; 0; 0.2; 0.3; 0.4]];
joint_angles = [0.2; 1.1; 0.1; 1.2]';
Js_shouldbe = [0 0.9801 -0.0901 0.9575
0 0.1987 0.4446 0.2849
1.0000 0 0.8912 -0.0453
0 1.9522 -2.2164 -0.5116
0.2000 0.4365 -2.4371 2.7754
0.2000 2.9603 3.2357 2.2251];
Js = J_space(dummyrobot, joint_angles);
assert(all(abs(Js - Js_shouldbe) < 1e-4, 'all'));
disp('All good');
%% Example poses, carefully reasoned out.
% At zero position
% Jacobian == screw because all the thetas are 0, so the product of
% exponentials is the identity, so the adjoint is identity, so the Jacobian
% columns are just the screw columns.
joint_angles = [0 0 0 0 0 0 0];
Js = J_space(robot, joint_angles);
Js_shouldbe = robot.screw;
assert(all(abs(Js - Js_shouldbe) < 1e-9, 'all'));
joint_angles
Js
% Zero but with last joint turned
% Still Jacobian == screw because the last joint doesn't affect Jacobian.
joint_angles = [0 0 0 0 0 0 pi/2];
Js = J_space(robot, joint_angles);
Js_shouldbe = robot.screw;
assert(all(abs(Js - Js_shouldbe) < 1e-9, 'all'));
joint_angles
Js
% When the second last joint moves, the Jacobian's last column will change.
joint_angles = [0 0 0 0 0 pi/2 0];
rot = zyz2rot(robot.axes(6,:) * pi/2); % rotated so x and z are swapped
w = rot * robot.axes(7,:)'; %[1 0 0]' % now it points +x instead of +z
q = trans2translation(FK_space(robot, joint_angles, 'JointNum', 7));
Js7_shouldbe = screwgeo2twist(w, q);
Js = round(J_space(robot, joint_angles),12);
Js7 = Js(:,7);
assert(all(abs(Js7 - Js7_shouldbe) < 1e-9, 'all'));
joint_angles
Js
%% Test Jacobian J_space function against Matlab Robotics Toolbox
% Function robot.geometricJacobian should be comparable.
% It reports Jacobian for the given end effector name.
% TODO: THIS HAS NOT BEEN MADE TO WORK YET.
% See work in "iiwa_tree"
% Load the robot from the Matlab built-in set.
kuka = loadrobot('kukaIiwa14');
% Prepare an array of structs that can be used for the robot config
config = randomConfiguration(kuka);
eg_indices = [5];
for eg_i = 1:numel(eg_indices)
eg_index = eg_indices(eg_i);
joint_angles = deg2rad(joints(:, eg_index));
ans = num2cell(joint_angles);
[config.JointPosition] = ans{:};
Jmatlab = kuka.geometricJacobian(config, 'iiwa_link_ee');
Js = J_space(robot, joint_angles);
end
%% Test Jacobian function J_space via finite differences
% Since the (geometric) Jacobian is partial derivative of the angle and
% translation of the end effector for a change in a joint's angle,
% we can change the joint angles slightly, and see if the end effector
% frame changes by the jacobian.
% TODO: THIS IS STILL NOT WORKING.
% The "analytic jacobian" is a true derivative
% The "geometric jacobian" (what we have) is change in geo params, which I
% guess isn't the same thing. Meaning we can't do finite differences.
delta = 0.01;
deltas = eye(robot.dof) * delta;
test_cases = [5];
for i_test_cases = 1:numel(test_cases)
i = test_cases(i_test_cases);
joint_angles = deg2rad(joints(i,:)');
pose_init = FK_space(robot, joint_angles);
J = J_space(robot, joint_angles);
for j = 1:robot.dof
pose = FK_space(robot, joint_angles + deltas(:, j));
% If it was analytical jacobian, I think this would be right:
%dpose = (trans2screw(pose) - trans2screw(pose_init)) / delta;
% Change from one frame to another
dpose = inv(pose_init) * pose;
dposetwist = trans2vector(dpose) / delta;
err = J(:, j) - dposetwist;
fprintf('Config %02d Axis %1d Linear err %f\n', i, j, norm(err(1:3)));
end
end