Da Vinci Robot MasterHand 工作空间（workspace）分析_mhondh

首先利用DH法对主臂建模

代码如下：

function m = MH_DH_Model()
    m.l_arm = 0.3000;
    m.l_forearm = 0.3500;
    m.h = 0.1347;
    
    m.method = 'Standard';
    m.DH = [
        % type   alpha   a           d       theta
        %=====================================
           1      pi/2  0             0          0;
           1      0     m.l_arm       0         -pi/2;
           1     -pi/2  m.l_forearm   0          pi/2;
           1      pi/2  0             m.h        0;
           1     -pi/2  0             0          0;
           1      pi/2  0             0         -pi/2;
           1      0     0             0          pi/2;
           ];
    m.tip = eye(4);
end

完成建模后做Forward Kinematics（可以自己写function，也可以直接用robotics toolbox）

代码如下：

function [T,Jacobian] = FK_Jacob_Geometry(q,dh_table,Tip_T,DH_method)
% return position of center of mass of ith link
    T = eye(4);
    dh_size = size(dh_table);

    Jacob_ori = [0;0;1];
    z_axis = [0;0;1];
    p_pos = [0;0;0];
    for i=1:dh_size(1)
        theta = dh_table(i,5);
        d = dh_table(i,4);
        a = dh_table(i,3);
        alpha = dh_table(i,2);
        type = dh_table(i,1);
        if type == 1
            theta = theta + q(i);
            T = T*DHtransform(theta,d,a,alpha,DH_method);
            z_axis = [z_axis,T(1:3,3)]; 
            p_pos = [p_pos,T(1:3,4)];
        elseif type ==2
            d = d + q(i);
            T= T*DHtransform(theta,d,a,alpha,DH_method);
            z_axis = [z_axis,T(1:3,3)];
            p_pos = [p_pos,T(1:3,4)];
        else
            msg = sprintf('Encounter a known Joint Type %d, it must be 1 or 2',type);
            error(msg);
        end    
    end
    
     Jacobian = [];
    if DH_method == 'Standard'
        z_axis = z_axis(:,1:end-1);
        p_pos = p_pos(:,1:end-1);
    elseif DH_method == 'Modified'
        z_axis = z_axis(:,2:end);
        p_pos = p_pos(:,2:end);
    end
 
    %Tranform from last joint frame to tip frame
    T = T*Tip_T;
    p_pos = [p_pos,T(1:3,4)];
    
    

    for i=1:dh_size(1)
       type = dh_table(i,1);
       if type == 1
          Jacobian = [Jacobian,[cross(z_axis(:,i),p_pos(:,end)-p_pos(:,i));z_axis(:,i)]];
       elseif type == 2
          Jacobian = [Jacobian,[z_axis(:,i);zeros(3,1)]];
       end
    end
end

现在可以分析end effector 的 workspace, 随机在joint angle constraints里任意生成n个点，利用forward kinematics计算出n个三维空间上的点，再求解其包络图就可以了。

代码如下：

l_arm = 0.3000; % length of arm
l_fore_arm = 0.3500; % length of forearm
h = 0.1347; % height of handle

% all possible theta values for seven joints
% the joints constrains refer to Da Vinci Robot
% theta1 = (-pi/3):0.1:(pi/3);
% theta2 = (-pi/3):0.1:(5*pi/36);
% theta3 = (-pi/18):0.1:(5*pi/12);
% theta4 = (-49*pi/36):0.1:(13*pi/36);
% theta5 = (-93*pi/90):0.1:(49*pi/90);
% theta6 = (-41*pi/180):0.1:(41*pi/180);
% theta7 = (-25*pi/18):0.1:(25*pi/18);

% generate n theta values
n = 100000;
theta1 = rand(n,1)*(2*pi/3)-pi/3;
theta2 = rand(n,1)*(17*pi/36)-pi/3;
theta3 = rand(n,1)*(17*pi/36)-pi/18;
theta4 = rand(n,1)*(31*pi/18)-49*pi/36;
theta5 = rand(n,1)*(71*pi/45)-93*pi/90;
theta6 = rand(n,1)*(41*pi/90)-41*pi/180;
theta7 = rand(n,1)*(25*pi/9)-25*pi/18;

% the last 4 joints will not affect the tip position
% Forward kinematics to generate n 3D points
x = cos(theta1).*(l_fore_arm.*cos(theta2 + theta3) - h.*sin(theta2 + theta3) + l_arm.*sin(theta2));
y = sin(theta1).*(l_fore_arm.*cos(theta2 + theta3) - h.*sin(theta2 + theta3) + l_arm.*sin(theta2));
z = h.*cos(theta2 + theta3) + l_fore_arm.*sin(theta2 + theta3) - l_arm.*cos(theta2);

% plot the points cloud
scatter3(x, y, z, '.')
hold on

% plot the convex
dt = delaunayTriangulation(x, y, z);
[ch, v] = convexHull(dt);
trisurf(ch, dt.Points(:,1), dt.Points(:,2), dt.Points(:,3), 'FaceColor', 'cyan')

title('3D workspace of MasterHand')

结果如图所示：