资料：https://blog.csdn.net/huangjunsheng123/article/details/110630665

用vscode直接看工具箱api代码比较方便，代码说明很多

一、模型设置

1、基础效果

%采用机器人工具箱进行正逆运动学验证
a=[0,-0.3,-0.3,0,0,0];%DH参数
d=[0.05,0,0,0.06,0.05,0.04];
alp=[pi/2,0,0,pi/2,-pi/2,0];
%机器人工具箱模型，每一个L建立一个连杆，输入DH参数即可，offset是连杆的初始偏移量，此处为0
L(1) = Link('d', d(1), 'a', a(1), 'alpha', alp(1));
L(2) = Link('d', d(2), 'a', a(2), 'alpha', alp(2), 'offset', 0 );
L(3) = Link('d', d(3), 'a', a(3), 'alpha', alp(3), 'offset', 0 );
L(4) = Link('d', d(4), 'a', a(4), 'alpha', alp(4), 'offset',0);
L(5) = Link('d', d(5), 'a', a(5), 'alpha', alp(5));
L(6) = Link('d', d(6), 'a', a(6), 'alpha', alp(6));
bot = SerialLink(L,'name','rbt');%将六个连杆组合起来 
figure(1)
bot.display();%显示机器人模型
view(3)
bot.teach;

2、link的常用属性

运动学参数
%  theta        kinematic: joint angle   关节角度
%  d            kinematic: link offset   
%  a            kinematic: link length
%  alpha        kinematic: link twist   %  jointtype    kinematic: 'R' if revolute, 'P' if prismatic    关节类型
%  mdh          kinematic: 0 if standard D&H, else 1                参数类型（标准/改进）
%  offset       kinematic: joint variable offset                            关节变量偏移量（设置新零点）
%  qlim         kinematic: joint variable limits [min max]       关节变量限制范围
动力学参数
%  m            dynamic: link mass   连杆质量
%  r            dynamic: link COG wrt link coordinate frame 3x1   质心
%  I            dynamic: link inertia matrix, symmetric 3x3, about link COG.  惯性张量
%  B            dynamic: link viscous friction (motor referred)     连杆粘性摩擦
%  Tc           dynamic: link Coulomb friction                   连杆库仑摩擦
%-
%  G            actuator: gear ratio                减速比
%  Jm           actuator: motor inertia (motor referred)    电机惯量

二、运动学

1、正逆运动学

q=[1,-1,1,-1,1,1];%给定初始角度
T_rbt = bot.fkine(q);%采用机器人工具箱函数求正运动学
q_rbt = bot.ikine(T_rbt,'q0',q)%采用RTB的逆运动学计算上边得到的齐次矩阵对应的关节角度，给定初始值可以更快的收敛到给定的角度

2、雅各比矩阵

bot.jacob0()   关于绝对坐标系的雅各比矩阵
bot.jacobn()  关于工作坐标系的雅各比矩阵

三、动力学

1、查看动力学参数

bot.dyn()

2、正逆动力学

bot.nofriction()                                                     //无摩擦力动力学模型
bot.accel(角度，角速度，外力矩阵)              //计算关节加速度bot.fdyn(时间，外力矩阵)                                                                               //正动力学
bot.rne(角度，角速度，角加速度，重力加速度，输出动力要求)    //逆动力学

3、计算动力学方程

bot.gravload()    //G
bot.inertia()       //M
bot.coriolis()     //V
bot.payload()    //有效载荷

四、例子：二自由度平面机械臂

syms Ix1 Iy1 Iz1 Ix2 Iy2 Iz2
syms q1 q2 m1 m2 L1 L2
syms qd1 qd2 g 
%%%%%%%%%%%%%%%%%%%1、设置运动学参数%%%%%%%%%%%%%%%%%%%
a=[0,L1,L2];%DH参数
d=[0,0,0];
alp=[0,0,0];q = [q1,q2,0];
qd = [qd1,qd2,0];L(1) = Link('d', d(1), 'a', a(1), 'alpha', alp(1),'modified');        %'modified'代表使用改进dh法
L(2) = Link('d', d(2), 'a', a(2), 'alpha', alp(2),'modified');
L(3) = Link('d', d(3), 'a', a(3), 'alpha', alp(3),'modified');
bot = SerialLink(L,'name','rbt');                                                       %将三个连杆组合起来，形成机器人 
%%%%%%%%%%%%%%%%%%%2、设置动力学参数%%%%%%%%%%%%%%%%%%%
%质量
bot.links(1).m=m1;
bot.links(2).m=m2;
bot.links(3).m=0;
%质心
bot.links(1).r=[L1/2,0,0];
bot.links(2).r=[L2/2,0,0];
bot.links(3).r=[0,0,0];
%转动惯量
bot.links(1).I = [Ix1, Iy1, Iz1, 0, 0,0];
bot.links(2).I= [Ix2, Iy2, Iz2,0, 0,0];
bot.links(3).I = [0, 0, 0, 0, 0,0];
%重力
bot.gravity=[0;0;-g];bot.dyn();    %显示所有杆的动力学参数
%%%%%%%%%%%%%%%%%%%3、计算运动学参数%%%%%%%%%%%%%%%%%%%
T = bot.fkine(q);            %机器人正运动学，输入关节角度，输出T_e^0
J = bot.jacob0(q);         %计算雅各比矩阵，基于世界坐标的
%%%%%%%%%%%%%%%%%%%4、计算动力学参数%%%%%%%%%%%%%%%%%%%
M = bot.inertia(q);             %关节空间质量矩阵
V = bot.coriolis(q,qd);      %计算离心力和科氏力矢量，注意这里给出的是矩阵V，V*qd才等于计算出的向量V（qd）
G = bot.gravload(q);         %重力矢量
%%%%%%%%%%%%%%%%%%%5、正逆运动学%%%%%%%%%%%%%%%%%%%
%逆运动学，角度（不是微分运动学）
theta1 = bot.ikine(T,'mask',[1,1,1,0,0,0],'q0',[0,0,0]);   %T代表的是工作点的位姿，输出各个关节角度，q0是基座坐标，mask是关节小于6个时选择开启的
theta2 = bot.ikunc(T); 
%%正逆微分运动学
%%%%%%%%%%%%%%%%%%%6、正逆动力学%%%%%%%%%%%%%%%%%%%
bot_nf=bot.nofriction();   %设置没有摩擦力  %其实就是根据关节力矩和作用力矩+初始q和qd算qdd，再根据qdd更新q和qd
%句柄那里不可以用[1,2,3,4,5,6]这样赋值了，可以直接给[]就是不给力
%0.4是时间间隔，@函数句柄（函数需要特别设置，因为要解算方程），第一个qz是q0,第二个qz是qd0,5和1就是函数句柄的参数
%输出就是一系列t、q、qd[t q qd] = p560.nofriction().fdyn(0.4, @test1, qz,qz,5,1);torqfun = [0,30,6];                %设定一组关节力
qdd = bot_nf.accel(q(i,:),qd(i,:),torqfun);  %获得中间量的办法，fdyn应该调用了accelfprintf("-----------------------")
%逆动力学
tau = bot.rne(q, qd, qdd, 'gravity',[0,9.8,0],'fext',[0,0,0,0,0,0]);%fext是外力

test1.m

function tau = test1(t,q,qd,qstar,P,D)
tau=P*(qstar-q) + D*qd;
tau =[0,0,0,0,0,0];
end