matlab 机器人工具箱自带函数应用示例

% matlab2016b，RTB 9.9
% clear;clc;close all;

L1=Link('d',0.144,    'a',0,       'alpha',0,      'offset',0,       'modified');  % MBD
L2=Link('d',0,        'a',0,       'alpha',pi/2,   'offset',-pi/2,   'modified');  
L3=Link('d',0,        'a',-0.264,  'alpha',0,                        'modified'); 
L4=Link('d',0.106,    'a',-0.236,  'alpha',0,      'offset',-pi/2,   'modified');  
L5=Link('d',0.114,    'a',0,       'alpha',pi/2,                     'modified'); 
L6=Link('d',0.067,    'a',0,       'alpha',-pi/2,                    'modified');  

% figure(1);
MDHrobot=SerialLink([L1 L2 L3 L4 L5 L6],'name','MDHUR5') ;                                       
% MDHrobot.plot([0 0 0 0 0 0]);  % 绘制出建模时的三维模型，显示当前输入的theta角
% MDHrobot.teach;                % 可施教                           
% MDHrobot.display;              % 打印机械臂相关信息
 
% 请注意，不论STDH还是MDH UR5 构型位姿都是末端到基座，基座不上移的情况下。
%% 实验一：模型验证
% L11=Link('d',0.144,    'a',0,        'alpha',pi/2,   'offset',0,     'standard');  
% L22=Link('d',0,        'a',-0.264,   'alpha',0,      'offset',0,     'standard');  
% L33=Link('d',0,        'a',-0.236,   'alpha',0,                      'standard');  % 当2，4关节偏置-90度与MDH位姿一样
% L44=Link('d',0.106,    'a',0,        'alpha',pi/2,   'offset',0,     'standard');  
% L55=Link('d',0.114,    'a',0,        'alpha',-pi/2,                  'standard'); 
% L66=Link('d',0.067,    'a',0,        'alpha',0,                      'standard');    
% 
% figure(2);
% STDHrobot=SerialLink([L11 L22 L33 L44 L55 L66],'name','STDHUR5') ;                                       
% STDHrobot.plot([0 0 0 0 0 0]);  % 绘制三维模型，显示当前输入的theta角
% STDHrobot.teach;                % 可施教                           
% STDHrobot.display; 
% 
% L111=Link('d',0.144,    'a',0,        'alpha',0,       'offset',pi/2,      'modified');  % 关节1偏置90度，末端xyz方向发生变化
% L222=Link('d',0,        'a',0,        'alpha',pi/2,    'offset',-pi/2,     'modified');  
% L333=Link('d',0,        'a',-0.264,   'alpha',0,                           'modified'); 
% L444=Link('d',0.106,    'a',-0.236,   'alpha',0,       'offset',-pi/2,     'modified');  
% L555=Link('d',0.114,    'a',0,        'alpha',pi/2,                        'modified'); 
% L666=Link('d',0.067,    'a',0,        'alpha',-pi/2,                       'modified');  
% 
% figure(3);
% MDHrobot1=SerialLink([L111 L222 L333 L444 L555 L666],'name','MDHUR51') ;                                       
% MDHrobot1.plot([0 0 0 0 0 0]);  % 绘制三维模型，显示当前输入的theta角
% MDHrobot1.teach;                % 可施教                           
% MDHrobot1.display;

%% 实验二：基于蒙特卡洛法的工作空间分析
% L1.qlim=[-3.0543,3.0543]; % 各关节活动范围弧度制 deg2rad()
% L2.qlim=[-1.7628,1.7628];
% L3.qlim=[-2.8973,2.8973];
% L4.qlim=[-3.0718,3.0543];
% L5.qlim=[-2.8973,2.8973];
% L6.qlim=[-3.3175,3.7525];
% 
% n=200;                                      % 随机点数
% q1= -3.0543+(3.0543-(-3.0543))*rand(n,1);   % 每个关节随机生成角度
% q2= -1.7628+(1.7628-(-1.7628))*rand(n,1);
% q3= -2.8973+(2.8973-(-2.8973))*rand(n,1);
% q4= -3.0718+(3.0543-(-3.0718))*rand(n,1);
% q5= -2.8973+(2.8973-(-2.8973))*rand(n,1);
% q6= -3.3175+(3.7525-(-3.3175))*rand(n,1);
% 
% for i=1:1:n
%     
%   mod06=MDHrobot.fkine([q1(i),q2(i),q3(i),q4(i),q5(i),q6(i)]); % 求末端位姿矩阵
%   MDHrobot.plot([q1(i),q2(i),q3(i),q4(i),q5(i),q6(i)]);        % 动画显示
%   plot3(mod06(1,4),mod06(2,4),mod06(3,4),'b.');                % 绘制落点，蓝色点
%   
% %     if
% %     mod06(1,4)>=0.2&&mod06(1,4)<=0.6&&mod06(2,4)>=-0.3&&mod06(2,4)<=0.3&&mod06(3,4)>=0.2&&mod06(3,4)<=0.8 %某些不可到位置限制
% %         color_Tag=0;
% %     else
% %         color_Tag=1;
% %     end
% %     point(i,:)={mod06(1,4),mod06(2,4),mod06(3,4),color_Tag};  % 末端位置保存到point中
% 
%     point(i,:)={mod06(1,4),mod06(2,4),mod06(3,4)};     % 末端位置保存到point中
%     hold on;
% end   
% %  xlswrite('point.xls',point,'sheet1','A1'); % 写入excel，我电脑会报错，运行此语句

%% 实验三：让机械臂动起来，工具箱自带函数jtraj关节空间规划
% q1 = [0 0 0 0 0 0];                   % 起点
% q2 = [pi/3 pi/3 pi/3 pi/4 pi/6 pi/3]; % 终点
% step=100;
% q = jtraj(q1,q2,step);                % 工具箱自带函数规划
% figure(5);
% MDHrobot.plot(q);

%% 实验四：工具箱正逆解函数调用
% theta=[pi/3 pi/6 pi/3 pi/4 pi/6 pi/3]; % 任意给定第一组角,与teach示教对比
% fk1 = MDHrobot.fkine(theta)            % 调用工具箱正解函数
% ik1 = MDHrobot.ikine(fk1)              % 调用工具箱逆解函数,数值解法。奇异位置时使用逼近思想
% ik2 = MDHrobot.ikine6s(fk1)            % 调用工具箱逆解函数，封闭解法。实际需要满足PUMA560的球形手腕构型后，有个角度修改一下才行，不然报错，详细命令行输入help ikine6s
% ik3 = MDHrobot.ikunc(fk1)              % 调用工具箱逆解函数，采用最优化法求解逆运动学
% ik4 = MDHrobot.ikcon(fk1)              % 调用工具箱逆解函数，有关节限位下采用最优化法求解逆运动学

%% 实验五：调用关节空间5次多项式规划轨迹 [q,qd,qdd] = jtraj(q0,qf,m);  https://blog.csdn.net/qq_26751931/article/details/89950925?ops_request_misc
% q1 = [0 0 0 0 0 0];                    % 起点
% q2 = [pi/4 pi/6 pi/3 pi/3 pi/3 pi/6];  % 终点
% step=100;                              % 插补数
% [q,qd,qdd]= jtraj(q1,q2,step);         % 工具箱自带函数规划
% figure(2);
% subplot(3,2,[1,3]);                    % subplot 对画面分区，[1.3]占用1 3的位置，3.2三行两列
% MDHrobot.plot(q);
% 
% subplot(3, 2, 2);  % 显示位置、速度、加速度变化曲线
% plot(q(:,1)); 
% title('位置');
% grid on;
% 
% subplot(3, 2, 4);
% plot(qd(:,1));
% title('速度');
% grid on;
% 
% subplot(3, 2, 6);
% plot(qdd(:,1));
% title('加速度');
% grid on;
% 
% t =MDHrobot.fkine(q); % 运动学正解
% rpy=tr2rpy(t);        % t中提取位置（xyz）
% subplot(3,2,5);
% plot2(rpy);          

%% 实验六：调用工具箱自带的笛卡尔空间匀加速匀减速规划轨迹 tc = ctraj(T0,T1,n);   https://blog.csdn.net/qiangbaa/article/details/85269138?ops_request_misc
% init_ang = [0 0 0 0 0 0];
% targ_ang = [pi/4, -pi/3, pi/5, pi/2, -pi/4, pi/6];
% step=100;
% T0 = MDHrobot.fkine(init_ang); % 运动学正解
% T1 = MDHrobot.fkine(targ_ang); % 运动学正解
% Tc = ctraj(T0,T1,step);        % 得到每一步的T阵
% 
% tt = transl(Tc);
% figure(3);
% plot2(tt,'r');
% title('T0-T1轨迹');
% grid on;

% 机器人空间平滑过渡-6点5次贝塞尔曲线 https://blog.csdn.net/qq_38023025/article/details/103834564?spm=1001.2014.3001.5501
% Matlab实现实时多次贝塞尔曲线(Bézier Polynomials)插值 https://blog.csdn.net/diaodaa/article/details/106342405?spm=1001.2101.3001.6650.2&utm_medium

%% 实验七：zyx型RPY角,通过工具箱自带的tr2rpy函数
% theta=[pi/3 pi/6 pi/3 pi/4 pi/6 pi/3]; % 任意给定第一组角，与teach示教对比
% T16 = MDHrobot.fkine(theta)            % 调用工具箱正解函数
% X=T16(1,4);Y=T16(2,4);Z=T16(3,4);      % 提取末端x,y,z位置
% R=T16;
% 
% rpyxyz = tr2rpy( R,'deg');             % 求rpy角,绕zyx顺序
% Postion = [X Y Z rpyxyz]               % 输出位置与RPY角
% 
% rpyzyz = tr2eul(R,'deg');              % 调用机器人工具箱自带的tr2eul
% Postion1 = [X Y Z rpyzyz]  

%% 求末端姿态Rotations about X, Y, Z axes (for a robot gripper)
% if abs(abs(R(1,3)) - 1) < eps  % when |R13| == 1
%     % singularity
%     rpy(1) = 0;  % roll is zero
%     if R(1,3) > 0
%         rpy(3) = atan2( R(3,2), R(2,2));   % R+Y
%     else
%         rpy(3) = -atan2( R(2,1), R(3,1));   % R-Y
%     end
%     rpy(2) = asin(R(1,3));
% else
%     rpy(1) = -atan2(R(1,2), R(1,1));
%     rpy(3) = -atan2(R(2,3), R(3,3));
%     rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
% end
% RPY=rpy*180/pi;
% Rall=RPY(1);Pitch=RPY(2);Yaw=RPY(3);
% Pos=[X,Y,Z,Rall,Pitch,Yaw]

%% 机器人学 建模、规划与控制 布鲁诺。西西里安诺 p35-36 求解方法
% R1=[T16(1,1)   T16(1,2)   T16(1,3);
%     T16(2,1)   T16(2,2)   T16(2,3);
%     T16(3,1)   T16(3,2)   T16(3,3);];
% 
% if (R1(1,3)>0||R1(1,3)<0) && (R1(2,3)>0||R1(2,3)<0)
%     a1=atan2(R1(2,3),R1(1,3))/pi*180;                           % atan2(a,b)是4象限反正切，它的取值不仅取决于正切值a/b，还取决于点 (b, a) 落入哪个象限
%     b1=atan2(sqrt((R1(1,3))^2+(R1(2,3))^2),R1(3,3))/pi*180;     % /pi*180 弧度转角度
%     r1=atan2(R1(3,2),-R1(3,1))/pi*180;
% elseif b1==0
%     a1=0;
%     r1=atan2(-R1(1,2),R1(1,1))/pi*180;
% elseif b1==180
%     a1=0;
%     r1=atan2(R1(1,2),-R1(1,1))/pi*180;
% end
% 
% Eulerzyz = [a1 b1 r1]

% https://blog.csdn.net/Cognitive_Technology/article/details/119864093?ops_request_misc MATLAB Robotics Toolbox常用函数简介