1 %运动学正解
2 function [x, y, c] = planar_robot_forward_kinematics(L1, L2, theta1, theta2)
3 x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
4 y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
5 c = theta1 + theta2;
6 end
1 %运动学逆解
2 function [theta1, theta2] = planar_robot_inverse_kinematics(L1, L2, x, y, handcoor)
3 c2 = (x^2 + y^2 - L1^2 -L2^2) / (2 * L1 * L2);%若(x,y)在工作空间里，则c2必在[-1,1]里，但由于计算精度，c2的绝对值可能稍微大于1
4 temp = 1 - c2^2;
5 if(temp < 0)
6 temp = 0;
7 end
8 if(handcoor == 0) %right handcoor
9 theta2 = atan2(sqrt(temp), c2);
10 else %left handcoor
11 theta2 = atan2(-sqrt(temp), c2);
12 end
13 s2 = sin(theta2);
14 theta1 = atan2(y, x) - atan2(L2 * s2, L1 + L2 * c2);
15 end
1 %画圆弧
2 function draw_arc(x0, y0, r, theta1, theta2, options)
3 deltaTheta = 0.1 * pi / 180;
4 theta = theta1 : deltaTheta : theta2;
5 x = x0 + r * cos(theta);
6 y = y0 + r * sin(theta);
7 plot(x, y, 'LineStyle', options.LineStyle, 'Color', options.Color, 'LineWidth', options.LineWidth);
8 axis equal;
9 end
1 %画工作空间
2 function draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
3 thetaL1 = thetaLimit1(1);
4 thetaU1 = thetaLimit1(2);
5 thetaL2 = thetaLimit2(1);
6 thetaU2 = thetaLimit2(2);
7
8 hold on;
9 if(handcoor == 0) %right handcoor
10 options.LineStyle = '-';
11 options.Color='g';
12 options.LineWidth = 3;
13
14 x0 = 0;
15 y0 = 0;
16 r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
17 alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
18 thetaStart = thetaL1 + alpha;
19 thetaEnd = thetaU1 + alpha;
20 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
21
22 x0 = 0;
23 y0 = 0;
24 r = L1 + L2;
25 thetaStart = thetaL1;
26 thetaEnd = thetaU1;
27 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
28
29 x0 = L1 * cos(thetaU1);
30 y0 = L1 * sin(thetaU1);
31 r = L2;
32 thetaStart = thetaU1;
33 thetaEnd = thetaU1 + thetaU2;
34 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
35
36 x0 = L1 * cos(thetaL1);
37 y0 = L1 * sin(thetaL1);
38 r = L2;
39 thetaStart = thetaL1;
40 thetaEnd = thetaL1 + thetaU2;
41 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
42
43 title('Workspace in right handcoor', 'fontsize', 16);
44 else %left handcoor
45 options.LineStyle = '-';
46 options.Color='b';
47 options.LineWidth = 3;
48
49 x0 = 0;
50 y0 = 0;
51 r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
52 alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
53 thetaStart = thetaL1 - alpha;
54 thetaEnd = thetaU1 - alpha;
55 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
56
57 x0 = 0;
58 y0 = 0;
59 r = L1 + L2;
60 thetaStart = thetaL1;
61 thetaEnd = thetaU1;
62 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
63
64 x0 = L1 * cos(thetaU1);
65 y0 = L1 * sin(thetaU1);
66 r = L2;
67 thetaStart = thetaU1 + thetaL2;
68 thetaEnd = thetaU1;
69 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
70
71 x0 = L1 * cos(thetaL1);
72 y0 = L1 * sin(thetaL1);
73 r = L2;
74 thetaStart = thetaL1 + thetaL2;
75 thetaEnd = thetaL1;
76 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
77 title('Workspace in left handcoor', 'fontsize', 16);
78 end
79 set(gcf, 'color', 'w');
80 axis off;
81 end
1 %画工作空间草图
2 function draw_planar_robot_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
3
4 thetaL1 = thetaLimit1(1);
5 thetaU1 = thetaLimit1(2);
6 thetaL2 = thetaLimit2(1);
7 thetaU2 = thetaLimit2(2);
8
9 hold on;
10 if(handcoor == 0) %right handcoor
11 options.LineStyle = '-';
12 options.Color='g';
13 options.LineWidth = 3;
14
15 x0 = 0;
16 y0 = 0;
17 r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
18 alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
19 thetaStart = thetaL1 + alpha;
20 thetaEnd = thetaU1 + alpha;
21 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
22
23 x0 = 0;
24 y0 = 0;
25 r = L1 + L2;
26 thetaStart = thetaL1;
27 thetaEnd = thetaU1;
28 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
29
30 x0 = L1 * cos(thetaU1);
31 y0 = L1 * sin(thetaU1);
32 r = L2;
33 thetaStart = thetaU1;
34 thetaEnd = thetaU1 + thetaU2;
35 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
36
37 x0 = L1 * cos(thetaL1);
38 y0 = L1 * sin(thetaL1);
39 r = L2;
40 thetaStart = thetaL1;
41 thetaEnd = thetaL1 + thetaU2;
42 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
43
44 %-------------
45 options.LineStyle = '--';
46 options.Color='r';
47 options.LineWidth = 0.5;
48
49 x0 = 0;
50 y0 = 0;
51 r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
52 thetaStart = 0;
53 thetaEnd = 2 * pi;
54 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
55
56 r = L1 + L2;
57 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
58
59 x0 = L1 * cos(thetaU1);
60 y0 = L1 * sin(thetaU1);
61 r = L2;
62 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
63
64 x0 = L1 * cos(thetaL1);
65 y0 = L1 * sin(thetaL1);
66 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
67
68 xA1 = L1 * cos(thetaL1);
69 yA1 = L1 * sin(thetaL1);
70 xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
71 yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
72 xA2 = L1 * cos(thetaU1);
73 yA2 = L1 * sin(thetaU1);
74 xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
75 yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
76 xC1 = (L1 + L2) * cos(thetaL1);
77 yC1 = (L1 + L2) * sin(thetaL1);
78 xC2 = (L1 + L2) * cos(thetaU1);
79 yC2 = (L1 + L2) * sin(thetaU1);
80
81 plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
82 plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
83
84 fontsize = 15;
85 delta = 25;
86 text(0, 0, 'O', 'Fontsize', fontsize);
87 text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
88 text(xB1, yB1 - delta, 'B_1', 'fontsize', fontsize);
89 text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
90 text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
91 text(xC1, yC1, 'C_1', 'fontsize', fontsize);
92 text(xC2, yC2, 'C_2', 'fontsize', fontsize);
93 title('Workspace sketch in right handcoor', 'fontsize', 16);
94
95 else %left handcoor
96 options.LineStyle = '-';
97 options.Color='b';
98 options.LineWidth = 3;
99
100 x0 = 0;
101 y0 = 0;
102 r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
103 alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
104 thetaStart = thetaL1 - alpha;
105 thetaEnd = thetaU1 - alpha;
106 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
107
108 x0 = 0;
109 y0 = 0;
110 r = L1 + L2;
111 thetaStart = thetaL1;
112 thetaEnd = thetaU1;
113 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
114
115 x0 = L1 * cos(thetaU1);
116 y0 = L1 * sin(thetaU1);
117 r = L2;
118 thetaStart = thetaU1 + thetaL2;
119 thetaEnd = thetaU1;
120 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
121
122 x0 = L1 * cos(thetaL1);
123 y0 = L1 * sin(thetaL1);
124 r = L2;
125 thetaStart = thetaL1 + thetaL2;
126 thetaEnd = thetaL1;
127 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
128
129 %-------------
130 options.LineStyle = '--';
131 options.Color='r';
132 options.LineWidth = 0.5;
133
134 x0 = 0;
135 y0 = 0;
136 r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
137 thetaStart = 0;
138 thetaEnd = 2 * pi;
139 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
140
141 r = L1 + L2;
142 draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
143
144 x0 = L1 * cos(thetaU1);
145 y0 = L1 * sin(thetaU1);
146 r = L2;
147 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
148
149 x0 = L1 * cos(thetaL1);
150 y0 = L1 * sin(thetaL1);
151 draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
152
153 xA1 = L1 * cos(thetaL1);
154 yA1 = L1 * sin(thetaL1);
155 xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
156 yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
157 xA2 = L1 * cos(thetaU1);
158 yA2 = L1 * sin(thetaU1);
159 xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
160 yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
161 xC1 = (L1 + L2) * cos(thetaL1);
162 yC1 = (L1 + L2) * sin(thetaL1);
163 xC2 = (L1 + L2) * cos(thetaU1);
164 yC2 = (L1 + L2) * sin(thetaU1);
165
166 plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
167 plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
168
169 fontsize = 15;
170 delta = 25;
171 text(0, 0, 'O', 'fontsize', fontsize);
172 text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
173 text(xB1, yB1 + delta, 'B_1', 'fontsize', fontsize);
174 text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
175 text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
176 text(xC1, yC1, 'C_1', 'fontsize', fontsize);
177 text(xC2, yC2, 'C_2', 'fontsize', fontsize);
178 title('Workspace sketch in left handcoor', 'fontsize', 16);
179 end
180 set(gcf, 'color', 'w');
181 axis off;
182 end
1 %求边界点
2 function p = solve_planar_robot_boundary_point( L1, L2, thetaLimit1, thetaLimit2, handcoor, x0, y0, s)
3
4 thetaL1 = thetaLimit1(1);
5 thetaU1 = thetaLimit1(2);
6 thetaL2 = thetaLimit2(1);
7 thetaU2 = thetaLimit2(2);
8
9 xA1 = L1 * cos(thetaL1);
10 yA1 = L1 * sin(thetaL1);
11 xA2 = L1 * cos(thetaU1);
12 yA2 = L1 * sin(thetaU1);
13 xC1 = (L1 + L2) * cos(thetaL1);
14 yC1 = (L1 + L2) * sin(thetaL1);
15 xC2 = (L1 + L2) * cos(thetaU1);
16 yC2 = (L1 + L2) * sin(thetaU1);
17
18 if(handcoor == 0) %right handcoor
19 r = [sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2)), L1 + L2, L2, L2];
20 xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
21 yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
22 xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
23 yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
24 pp = [xB1, yB1, xB2, yB2; xC1, yC1, xC2, yC2; xC2, yC2, xB2, yB2; xC1, yC1, xB1, yB1];
25 else %left handcoor
26 r = [sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2)), L1 + L2, L2, L2];
27 xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
28 yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
29 xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
30 yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
31 pp = [xB1, yB1, xB2, yB2; xC1, yC1, xC2, yC2; xB2, yB2, xC2, yC2; xB1, yB1, xC1, yC1];
32 end
33 xc = [0, 0, L1 * cos(thetaU1), L1 * cos(thetaL1)];
34 yc = [0, 0, L1 * sin(thetaU1), L1 * sin(thetaL1)];
35
36 p = zeros(8,2);
37 k = 0;
38 if(abs(s(1) - 0) < eps)
39 temp = r.^2 - (x0 - xc).^2;
40 for i = 1 : 4
41 if(temp(i) >= 0)
42 xp1 = x0;
43 yp1 = yc(i) + sqrt(temp(i));
44 xp2 = x0;
45 yp2 = yc(i) - sqrt(temp(i));
46 if(dot([xp1 - x0; yp1 - y0], s) > 0 && (pp(i, 3) - xp1) * (pp(i, 2) - yp1) - (pp(i, 1) - xp1) * (pp(i, 4) - yp1) > 0)
47 k = k + 1;
48 p(k, :) = [xp1, yp1];
49 end
50 if(dot([xp2 - x0; yp2 - y0], s) > 0 && (pp(i, 3) - xp2) * (pp(i, 2) - yp2) - (pp(i, 1) - xp2) * (pp(i, 4) - yp2) > 0)
51 k = k + 1;
52 p(k, :) = [xp2, yp2];
53 end
54 end
55 end
56 else
57 a = (s(2) / s(1))^2 + 1;
58 b = 2 * (s(2) / s(1)) * (y0 - yc - x0 * (s(2) / s(1))) - 2 * xc;
59 c = xc.^2 - r.^2 - 2 * y0 * yc + y0^2 + yc.^2 + (s(2) / s(1))^2 * x0^2 + 2 * (s(2) / s(1)) * x0 * (yc - y0);
60 delta = b.^2 - 4 * a * c;
61 for i = 1 : 4
62 if(delta(i) >= 0)
63 x = roots([a b(i) c(i)]);
64 xp1 = x(1);
65 yp1 = s(2) / s(1) * (x(1) - x0) + y0;
66 xp2 = x(2);
67 yp2 = s(2) / s(1) * (x(2) - x0) + y0;
68 if(dot([xp1 - x0; yp1 - y0], s) > 0 && (pp(i, 3) - xp1) * (pp(i, 2) - yp1) - (pp(i, 1) - xp1) * (pp(i, 4) - yp1) > 0)
69 k = k + 1;
70 p(k, :) = [xp1, yp1];
71 end
72 if(dot([xp2 - x0; yp2 - y0], s) > 0 && (pp(i, 3) - xp2) * (pp(i, 2) - yp2) - (pp(i, 1) - xp2) * (pp(i, 4) - yp2) > 0)
73 k = k + 1;
74 p(k, :) = [xp2, yp2];
75 end
76 end
77 end
78 end
79
80 distance = sqrt((p(1 : k, 1) - x0).^2 + (p(1 : k, 2) - y0).^2);
81 [~, index] = min(distance);
82 p = p(index, :);
83 end
1 %画工作空间(示例)
2 clc;
3 clear;
4 close all;
5 L1 = 200;
6 L2 = 200;
7 thetaLimit1 = [-135, 135] * pi / 180;
8 thetaLimit2 = [-145, 145] * pi / 180;
9
10 figure(1);
11 handcoor = 0;
12 draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
13 figure(2);
14 handcoor = 1;
15 draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
16
17 figure(3);
18 handcoor = 0;
19 draw_planar_robot_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
20 figure(4);
21 handcoor = 1;
22 draw_planar_robot_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
1 %求边界点(示例)
2 clc;
3 clear;
4 close all;
5 L1 = 200;
6 L2 = 200;
7 thetaLimit1 = [-135, 135] * pi / 180;
8 thetaLimit2 = [-145, 145] * pi / 180;
9
10 figure(1);
11 handcoor = 1;
12 draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor);
13 while (1)
14 [x0, y0] = ginput(1);
15 [x1, y1] = ginput(1);
16 s = [x1 - x0; y1 - y0];
17 p = solve_planar_robot_boundary_point(L1, L2, thetaLimit1, thetaLimit2, handcoor, x0, y0, s);
18 plot([x0, p(1)], [y0, p(2)], '--o')
19 end
