首先利用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')
结果如图所示: