-
Notifications
You must be signed in to change notification settings - Fork 0
/
get_compliant_workspace.m
115 lines (95 loc) · 3.32 KB
/
get_compliant_workspace.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
%% This function outputs the compliant workspace based on the geometry-based method.
% param: kinematic parameters
% limit: joint limitation (radians)
% home_pos: compliant robot home position
% mode: compliant robot initial working mode
% orientation: platform's orientation
function [comp_workspace] = get_compliant_workspace(param, limit, home_pos, mode, orientation)
% home position
xini = home_pos(1);
yini = home_pos(2);
alphaini = home_pos(3);
% working mode
mode = regexprep(mode,{'+','-'},{'1','2'});
working_mode = str2double(split(mode));
% get working mode for each kinematic chain
wm1 = working_mode(1);
wm2 = working_mode(2);
wm3 = working_mode(3);
% initial angles
[th10,th20,th30] = ikm(param,xini,yini,alphaini);
theta = [th10,th20,th30];
[ph10,ph20,ph30,psi10,psi20,psi30] = ikm_phi_psi(param,theta,xini,yini,alphaini);
% obtain 1st kinematic chain reachable area %
% sweep across all possible values of theta and phi (initial angle +- max)
theta = linspace(th10(wm1)-limit,th10(wm1)+limit);
phi = linspace(ph10(wm1)-limit,ph10(wm1)+limit);
% calculate psi from phi
psi = pi-phi+pi/6+alphaini+orientation;
% find value of psi greater than limit
idx = find(angdiff(psi,psi10(wm1))>limit);
% delete values of phi that lead to angle limit violation
phi(idx) = [];
% obtain the reachable points
coord1 = [];
for t=theta
for p=phi
coord1 = [coord1,get_coord(param,t,p,alphaini+orientation,2*pi/3)];
end
end
% obtain 2nd kinematic chain reachable area %
% sweep across all possible values of theta and phi (initial angle +- max)
theta = linspace(th20(wm2)-limit,th20(wm2)+limit);
phi = linspace(ph20(wm2)-limit,ph20(wm2)+limit);
% calculate psi from phi
psi = pi-phi+5*pi/6+alphaini+orientation;
% find value of psi greater than limit
idx = find(angdiff(psi,psi20(wm2))>limit);
% delete values of phi that lead to angle limit violation
phi(idx) = [];
% obtain the reachable points
coord2 = [];
for t=theta
for p=phi
coord2 = [coord2,get_coord(param,t,p,alphaini+orientation,-2*pi/3)];
end
end
% obtain 3rd kinematic chain reachable area %
% sweep across all possible values of theta and phi (initial angle +- max)
theta = linspace(th30(wm3)-limit,th30(wm3)+limit);
phi = linspace(ph30(wm3)-limit,ph30(wm3)+limit);
% calculate psi from phi
psi = pi-phi+3*pi/2+alphaini+orientation;
% find value of psi greater than limit
idx = find(angdiff(psi,psi30(wm3))>limit);
% delete values of phi that lead to angle limit violation
phi(idx) = [];
% obtain the reachable points
coord3 = [];
for t=theta
for p=phi
coord3 = [coord3,get_coord(param,t,p,alphaini+orientation,0)];
end
end
%%%%% PLOT %%%%%
% obtain the boundary of the reachable area
k = boundary(coord1(1,:)',coord1(2,:)');
% and transform the boundary in a polyshape figure
wrkspace1 = polyshape(coord1(1,k),coord1(2,k));
% repeat for the other kinematic chains
k = boundary(coord2(1,:)',coord2(2,:)');
wrkspace2 = polyshape(coord2(1,k),coord2(2,k));
k = boundary(coord3(1,:)',coord3(2,:)');
wrkspace3 = polyshape(coord3(1,k),coord3(2,k));
% obtain workspace from intersection
% uncomment following line to plot the kinematic chains
% figure
% hold on
% plot(wrkspace1)
% plot(wrkspace2)
% plot(wrkspace3)
% hold off
% legend('1st kinematic chain','2nd kinematic chain','3rd kinematic chain')
% axis equal
comp_workspace = intersect([wrkspace1,wrkspace2,wrkspace3]);
end