ik-geo/matlab/automatic_IK/detect_intersecting_parallel_axes.m at main · rpiRobotics/ik-geo · GitHub

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
function [is_intersecting, is_intersecting_nonconsecutive, is_parallel, is_spherical] = detect_intersecting_parallel_axes(kin)
% Test robot kinematics to determine intersecting or parallel axes

THRESH = 1e-3;

N = length(kin.joint_type);

% Make sure kin.P and kin.H are the right sizes
assert(width(kin.P) == N + 1);
assert(width(kin.H) == N);

is_intersecting   = false([1, N-1]); % joints i, i+1 intersect
is_intersecting_nonconsecutive = false([1, N-2]); % joints i, i+2 intersect
is_parallel       = false([1, N-1]); % joints i, i+1 parallel
is_spherical      = false([1, N-2]); % joints i, i+1, i+2 spherical
% (Three parallel joints can be implied from is_parallel)

% Test for 2R and 3R joints
prev_intersection = NaN(3,1);
for i = 1:N-1
    j = i+1;

    p_ij = kin.P(:,j);
    h_i = kin.H(:,i);
    h_j = kin.H(:,j);

    ab = pinv([h_i h_j]) * p_ij;

    dist_ij = norm(p_ij - [h_i h_j] * ab);
    if dist_ij < THRESH
        is_intersecting(i) = true;
        intersection = sum(kin.P(:,1:j),2) + h_j*ab(2);
        if norm(intersection - prev_intersection) < THRESH
            is_spherical(i-1) = true;
        end
        prev_intersection = intersection;
    else
        prev_intersection = NaN(3,1);
    end
end


% Test for nonconsecutive intersecting axes i, i+2
% There may be a more elegant way to do this
% Set joint i+1 to three angles and see if it's always intersecting
for i = 1:N-2
    is_intersecting_nonconsecutive(i) = true;
    j = i+1;
    k = i+2;

    for theta = [0, 0.1, 0.2]
        p_ik = kin.P(:,j) + rot(kin.H(:,j), theta)*kin.P(:,k);
        h_i = kin.H(:,i);
        h_k = rot(kin.H(:,j), theta)*kin.H(:,k);

        ab = pinv([h_i h_k]) * p_ik;
        dist_ik = norm(p_ik - [h_i h_k] * ab);

        if not(dist_ik < THRESH)
            is_intersecting_nonconsecutive(i) = false;
            break
        end
    end
end


% Test for 2R|| (and implicitly 3R||) joints
for i = 1:N-1
    j = i+1;
    h_i = kin.H(:,i);
    h_j = kin.H(:,j);

    if abs(dot(h_i, h_j)) > 1-THRESH
        is_parallel(i) = true;
    end
end

end