MATLAB机器人常用代码程序（以UR5e机器人为例）

需要一个UR5e的机器人模型。MATLAB的Robotics Toolbox或者Robotics System Toolbox提供了创建和模拟机器人模型的功能。

UR5e = importrobot('universalUR5e.urdf');
show(UR5e)
showdetails(UR5e)
figure(Name="Interactive GUI")
gui = interactiveRigidBodyTree(UR5e,MarkerScaleFactor=0.5);

UR5e = loadrobot("universalUR5e",DataFormat="row");  % column
robotSM = smimport(UR5e,ModelName="UR5e_Subsystem");
model = get_param(robotSM,"Name");
UR5e.Gravity = [0, 0, -9.80665]';

UR5e = loadrobot("universalUR5e", "DataFormat", "row", "Gravity", [0 0 -9.81]);

http://www.petercorke.com/Robotics_Toolbox.html

% DH参数
    a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000];
    d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823];
    alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0];
    q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0];
    joint_direction = [-1, -1, 1, 1, 1, 1];
    mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879];
    center_of_mass = [[0, -0.02561, 0.00193]; [0.2125, 0, 0.11336]; [0.11993, 0.0, 0.0265]; [0, -0.0018, 0.01634]; [0, 0.0018, 0.01634]; [0, 0, -0.001159]];

roblocks

mdl_ur5

function r = mdl_ur5()
    
    deg = pi/180;
    
    % robot length values (metres)
    a = [0, -0.42500, -0.39225, 0, 0, 0]';

    d = [0.089459, 0, 0, 0.10915, 0.09465, 0.0823]';

    alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0]';
    
    theta = zeros(6,1);
    
    DH = [theta d a alpha];

    mass = [3.7000, 8.3930, 2.33, 1.2190, 1.2190, 0.1897];

    center_of_mass = [
        0,-0.02561, 0.00193
        0.2125, 0, 0.11336
        0.15, 0, 0.0265
        0, -0.0018, 0.01634
        0, -0.0018, 0.01634
        0, 0, -0.001159];
    
    
    % and build a serial link manipulator
    
    % offsets from the table on page 4, "Mico" angles are the passed joint
    % angles.  "DH Algo" are the result after adding the joint angle offset.

    robot = SerialLink(DH, ...
        'name', 'UR5', 'manufacturer', 'Universal Robotics');
    
    % add the mass data, no inertia available
    links = robot.links;
    for i=1:6
        links(i).m = mass(i);
        links(i).r = center_of_mass(i,:);
    end
    
    links(1).I = diag([0.010267495893,0.010267495893,0.00666]);
    links(2).I = diag([0.22689067591,0.22689067591 ,0.0151074]);
    links(3).I = diag([0.049443313556,0.049443313556,0.004095]);
    links(4).I = diag([0.111172755531,0.111172755531,0.21942]);
    links(5).I = diag([0.111172755531,0.111172755531,0.21942]);
    links(6).I = diag([0.0171364731454,0.0171364731454,0.033822]);
    
     links(1).Jm = 33e-6;
     links(2).Jm = 33e-6;
     links(3).Jm = 33e-6;
     links(4).Jm = 33e-6;
     links(5).Jm = 33e-6;
     links(6).Jm = 33e-6;
     links(1).qlim = [-180 180]*deg;
     links(2).qlim = [-180 180]*deg;
     links(3).qlim = [-180 180]*deg;
     links(4).qlim = [-180 180]*deg;
     links(5).qlim = [-180 180]*deg;
     links(6).qlim = [-180 180]*deg;

    
    % place the variables into the global workspace
    if nargin == 1
        r = robot;
    elseif nargin == 0
        assignin('caller', 'ur5', robot);
        assignin('caller', 'qz', [0 0 0 0 0 0]); % zero angles
        assignin('caller', 'qr', [180 0 0 0 90 0]*deg); % vertical pose as per Fig 2
    end
end

机器人动力学方程如下：

输入时间，关节角，控制力矩。输出加速度

function dS = robot_dynamics(t,q,T)
    q1 = q(1); q2 = q(2); q3 = q(3);
    q4 = q(4); q5 = q(5); q6 = q(6);
    dq1 = q(7); dq2 = q(8); dq3 = q(9);
    dq4 = q(10); dq5 = q(11); dq6 = q(12);
    
    dS = zeros(12,1);
    dq = [dq1, dq2, dq3, dq4, dq5, dq6]';
    M = M_fun(q1,q2,q3,q4,q5,q6);
    C = C_fun(q1,q2,q3,q4,q5,q6,dq1,dq2,dq3,dq4,dq5,dq6);
    G = G_fun(q1,q2,q3,q4,q5,q6);
%%
    dS(1) = dq1;
    dS(2) = dq2;
    dS(3) = dq3;
    dS(4) = dq4;
    dS(5) = dq5;
    dS(6) = dq6;
    
    J = Je_fun(q1,q2,q3,q4,q5,q6);


    Fx = 0; Fy = 0; Fz = 0;
    tx = 0; ty = 0; tz = 0;
    
    Fe = [Fx; Fy; Fz; tx; ty; tz];
    
    Te = J' * Fe;

    ddq = inv(M) * (-C*dq - G' + T + Te);
    
    dS(7) = ddq(1);
    dS(8) = ddq(2);
    dS(9) = ddq(3);
    dS(10) = ddq(4);
    dS(11) = ddq(5);
    dS(12) = ddq(6);
end

if strcmp(target,'xml')
alpha = [pi/2 0 0 pi/2 -pi/2 0];
d = [0.163, 0, 0, 0.134, 0.1, 0.0771]; % d is given in z-direction
a = [0, -0.425, -0.392, 0, 0, 0]; % a is given in x-direction
m = [3.7, 8.393, 2.275, 1.219, 1.219, 0.1889];

p_c1 = [0, 0, 0];
p_c2 = [0.2125, 0, 0.138];
p_c3 = [0.196, 0, 0.007];
p_c4 = [0, 0, 0];
p_c5 = [0, 0, 0];
p_c6 = [0, 0, 0];

In_1 = diag([0.010267, 0.010267, 0.00660]);
In_2 = diag([0.015107, 0.13389, 0.13389]);
In_3 = diag([0.004095, 0.031178, 0.031178]);
In_4 = diag([0.0025599, 0.0021942, 0.0025599]);
In_5 = diag([0.0025599, 0.0025599, 0.0021942]);
In_6 = diag([0.00009804, 0.00009804, 0.00013210]);

end
%% From robosuite XML files
% In_1 = diag([0.010267, 0.010267, 0.00660]);
% In_2 = diag([0.015107, 0.13389, 0.13389]);
% In_3 = diag([0.004095, 0.031178, 0.031178]);
% In_4 = diag([0.0025599, 0.0021942, 0.0025599]);
% In_5 = diag([0.0025599, 0.0025599, 0.0021942]);
% In_6 = diag([0.00009804, 0.00009804, 0.00013210]);
%% From matlab UR5 moment of inertia file
if strcmp(target,'matlab')
alpha = [pi/2 0 0 pi/2 -pi/2 0];
d = [0.163, 0, 0, 0.134, 0.1, 0.0771]; % d is given in z-direction
a = [0, -0.425, -0.39225, 0, 0, 0]; % a is given in x-direction
m = [3.7, 8.393, 2.33, 1.219, 1.219, 0.1889];

p_c1 = [0, -0.02561, 0.00193];
p_c2 = [0.2125, 0, 0.11336];
p_c3 = [0.196, 0, 0.0265];
p_c4 = [0, -0.0018, 0.01634];
p_c5 = [0, -0.0018, 0.01634];
p_c6 = [0, 0, -0.001159];

In_1 = [0.010267, 0.00660, 0.010267];
In_2 = [0.0151, 0.8849, 0.8849];
In_3 = [0.004095, 0.1916, 0.1916];
In_4 = [0.1112, 0.2194, 0.1112];
In_5 = [0.1112, 0.2194, 0.1112];
In_6 = [0.0171, 0.0171, 0.0338];
end
%%

L(1) = Revolute('d', d(1), ...   
    'a', a(1), ...               
    'alpha', alpha(1), ...        
    'I', [In_1(1), In_1(2), In_1(3), 0, 0, 0], ... 
    'r', p_c1, ...       
    'm', m(1)); 

L(2) = Revolute('d', d(2), ...   
    'a', a(2), ...               
    'alpha', alpha(2), ...        
    'I', [In_2(1), In_2(2), In_2(3), 0, 0, 0], ... 
    'r', p_c2, ...       
    'm', m(2)); 

L(3) = Revolute('d', d(3), ...   
    'a', a(3), ...               
    'alpha', alpha(3), ...        
    'I', [In_3(1), In_3(2), In_3(3), 0, 0, 0], ... 
    'r', p_c1, ...       
    'm', m(1)); 

L(4) = Revolute('d', d(4), ...   
    'a', a(4), ...               
    'alpha', alpha(4), ...        
    'I', [In_4(1), In_4(2), In_4(3), 0, 0, 0], ... 
    'r', p_c4, ...       
    'm', m(4)); 

L(5) = Revolute('d', d(5), ...   
    'a', a(5), ...               
    'alpha', alpha(5), ...        
    'I', [In_5(1), In_5(2), In_5(3), 0, 0, 0], ... 
    'r', p_c5, ...       
    'm', m(5)); 

L(6) = Revolute('d', d(6), ...   
    'a', a(6), ...               
    'alpha', alpha(6), ...        
    'I', [In_6(1), In_6(2), In_6(3), 0, 0, 0], ... 
    'r', p_c6, ...       
    'm', m(6)); 

UR5e = SerialLink(L, 'name', 'UR5e');

UR5e.sym()