平面二自由度机器人运动学分析

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