Draw the scara robot workspace

1. Drawing the workspace of the scara robot

  As shown above, the scara robot arm length is $L_1$, Forearm length $L_2$，${\theta }_1$Is the angle of joint 1, ${\theta }_2$Is the angle of joint 2, and ${\theta }_1\in [{\theta }_{L1},{\theta }_{U1}],{\theta }_2\in [{\theta }_{L\text{'}2},{\theta }_{U2}]$ .

  The workspace of the scara robot consists of four arcs (as shown in the figure above). The arc equation is as follows:
$$\{\begin{array}{l}{x}^2+Y^2=(L_1+L_2{)}^2\\ x^2+Y^2=L_1^2+L_2^2+2L_1{L}_{2}cos\theta \\ (x-L_{1}cos{\theta }_{L1}{)}^2+(and-L_{1}sin{\theta }_{L1}{)}^2=L_2^2\\ (x-L_{1}cos{\theta }_{U1}{)}^2+(and-L_{1}sin{\theta }_{U1}{)}^2=L_2^2\end{array}\text{\hspace{1em}( 1 )}$$
  Among them, in the left-hand system,$\theta ={\theta }_{L\text{'}2}$; In the right hand system, $\theta ={\theta }_{U2}$。

Two, MATLAB code

  Draw a 2D arc:

%{
Function: draw_2d_arc
Description: 绘制平面圆弧
Input: 圆弧圆心(x0, y0),半径r，起始角度theta1(rad)，结束角度theta2(rad), 曲线样式选择options
Output: 无
Author: Marc Pony([email protected])
%}
function draw_2d_arc(x0, y0, r, theta1, theta2, options)
deltaTheta = 0.1 * pi / 180;
theta = theta1 : deltaTheta : theta2;
x = x0 + r * cos(theta);
y = y0 + r * sin(theta);
plot(x, y, 'LineStyle', options.LineStyle, 'Color', options.Color, 'LineWidth', options.LineWidth);
axis equal;
end

  Draw the scara robot workspace:

%{
Function: draw_scara_workspace
Description: 绘制scara机器人工作空间
Input: 大臂L1，小臂L2，关节1限位角度thetaLimit1(rad)，关节2限位角度thetaLimit2(rad)，手系handcoor
Output: 无
Author: Marc Pony([email protected])
%}
function draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
thetaL1 = thetaLimit1(1);
thetaU1 = thetaLimit1(2);
thetaL2 = thetaLimit2(1);
thetaU2 = thetaLimit2(2);

hold on;
if(handcoor == 1) %right handcoor
    options.LineStyle = '-';
    options.Color='g';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 + alpha;
    thetaEnd = thetaU1 + alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1;
    thetaEnd = thetaU1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1;
    thetaEnd = thetaL1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    title('Workspace in right handcoor', 'fontsize', 16);
else  %left handcoor
    options.LineStyle = '-';
    options.Color='b';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 - alpha;
    thetaEnd = thetaU1 - alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1 + thetaL2;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1 + thetaL2;
    thetaEnd = thetaL1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    title('Workspace in left handcoor', 'fontsize', 16);
end
set(gcf, 'color', 'w');
axis off;
end

  Draw the sketch of the scara robot workspace:

%{
Function: draw_scara_workspace_sketch
Description: 绘制scara机器人工作空间草图
Input: 大臂L1，小臂L2，关节1限位角度thetaLimit1(rad)，关节2限位角度thetaLimit2(rad)，手系handcoor
Output: 无
Author: Marc Pony([email protected])
%}
function draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)

thetaL1 = thetaLimit1(1);
thetaU1 = thetaLimit1(2);
thetaL2 = thetaLimit2(1);
thetaU2 = thetaLimit2(2);

hold on;
if(handcoor == 1) %right handcoor
    options.LineStyle = '-';
    options.Color='g';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 + alpha;
    thetaEnd = thetaU1 + alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1;
    thetaEnd = thetaU1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1;
    thetaEnd = thetaL1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    %-------------
    options.LineStyle = '--';
    options.Color='r';
    options.LineWidth = 0.5;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    thetaStart = 0;
    thetaEnd = 2 * pi;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    r = L1 + L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    xA1 = L1 * cos(thetaL1);
    yA1 = L1 * sin(thetaL1);
    xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
    yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
    xA2 = L1 * cos(thetaU1);
    yA2 = L1 * sin(thetaU1);
    xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
    yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
    xC1 = (L1 + L2) * cos(thetaL1);
    yC1 = (L1 + L2) * sin(thetaL1);
    xC2 = (L1 + L2) * cos(thetaU1);
    yC2 = (L1 + L2) * sin(thetaU1);
    
    plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
    plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
    
    fontsize = 15;
    delta = 25;
    text(0, 0, 'O', 'Fontsize', fontsize);
    text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
    text(xB1, yB1 - delta, 'B_1', 'fontsize', fontsize);
    text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
    text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
    text(xC1, yC1, 'C_1', 'fontsize', fontsize);
    text(xC2, yC2, 'C_2', 'fontsize', fontsize);
    title('Workspace sketch in right handcoor', 'fontsize', 16);
    
else  %left handcoor
    options.LineStyle = '-';
    options.Color='b';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 - alpha;
    thetaEnd = thetaU1 - alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1 + thetaL2;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1 + thetaL2;
    thetaEnd = thetaL1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    %-------------
    options.LineStyle = '--';
    options.Color='r';
    options.LineWidth = 0.5;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    thetaStart = 0;
    thetaEnd = 2 * pi;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    r = L1 + L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    xA1 = L1 * cos(thetaL1);
    yA1 = L1 * sin(thetaL1);
    xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
    yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
    xA2 = L1 * cos(thetaU1);
    yA2 = L1 * sin(thetaU1);
    xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
    yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
    xC1 = (L1 + L2) * cos(thetaL1);
    yC1 = (L1 + L2) * sin(thetaL1);
    xC2 = (L1 + L2) * cos(thetaU1);
    yC2 = (L1 + L2) * sin(thetaU1);
    
    plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
    plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
    
    fontsize = 15;
    delta = 25;
    text(0, 0, 'O', 'fontsize', fontsize);
    text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
    text(xB1, yB1 + delta, 'B_1', 'fontsize', fontsize);
    text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
    text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
    text(xC1, yC1, 'C_1', 'fontsize', fontsize);
    text(xC2, yC2, 'C_2', 'fontsize', fontsize);
    title('Workspace sketch in left handcoor', 'fontsize', 16);
end
set(gcf, 'color', 'w');
axis off;
end

clc;
clear;
close all;
L1 = 200;
L2 = 200;
thetaLimit1 = [-135, 135] * pi / 180;
thetaLimit2 = [-145, 145] * pi / 180;

%% 画工作空间
figure(1);
handcoor = 0;
draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
figure(2);
handcoor = 1;
draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)

%% 画工作空间草图
figure(3);
handcoor = 0;
draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
figure(4);
handcoor = 1;
draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)