RRT-UR5_RRT算法_RRT_双臂机器人_matlab_UR5

% L =link('d',1.2,'a', 0.3,'alpha',pi/2,'offset',pi/2,'mdh',1,'qlim',[-pi,pi],'m',0.5,'r',[0 0 0.05],'I',[0.001 0 0; 0 0.001 0; 0 0 0.05]); %%-------------------------------------------------------------------------%% %双臂模型构建与开始工作姿态调节 %%-------------------------------------------------------------------------%% tic; %第一个机器人模型，6个旋转关节 L1 = Link('d', 8.9459, 'a', 0, 'alpha', pi/2); L2 = Link('d', 0, 'a', -42.5, 'alpha', 0); L3 = Link('d', 0, 'a', -39.225, 'alpha', 0); L4 = Link('d', 10.915, 'a', 0, 'alpha', pi/2); L5 = Link('d', 9.465, 'a', 0, 'alpha', -pi/2); L6 = Link('d', 8.23, 'a', 0, 'alpha', 0); %第二个机器人模型，6个旋转关节 R1 = Link('d', 8.9459, 'a', 0, 'alpha', pi/2,'qlim',[-pi,pi]); R2 = Link('d', 0, 'a', -42.5, 'alpha', 0,'offset',pi/2,'qlim',[-pi,pi]); R3 = Link('d', 0, 'a', -39.225, 'alpha', 0,'qlim',[-pi,pi]); R4 = Link('d', 10.915, 'a', 0, 'alpha', pi/2,'qlim',[-pi,pi]); R5 = Link('d', 9.465, 'a', 0, 'alpha', -pi/2,'qlim',[-pi,pi]); R6 = Link('d', 8.23, 'a', 0, 'alpha', 0,'qlim',[-pi,pi]); %创建双臂机器人 LR=SerialLink([L1,L2,L3,L4,L5,L6]); RR=SerialLink([R1,R2,R3,R4,R5,R6]); %基坐标转换 LR.base = transl(0,19.414*1.5,0)*trotx(-135/180*pi); RR.base = transl(0,-19.414*1.5,0)*trotx(135/180*pi); %双臂属性设置 LR.name=' leftArm'; %SerialLink 属性值 LR.comment='StrongLi27'; %SerialLink 属性值 RR.name='rightArm'; %SerialLink 属性值 RR.comment='StrongLi27'; %SerialLink 属性值 %设置初始位置双臂的关节角度参数 % LjointAngleStart=[0 -pi/2 0 -pi/2 pi 0]; % RjointAngleStart=[pi -pi 0 -pi/2 pi pi]; LjointAngleStart=[-24.1573 -121.5291 -79.1094 -47.1050 -130.1793 -147.6128]*pi/180; RjointAngleStart=[-156.6995 -146.9504 81.1299 -135.7610 130.4992 148.6556]*pi/180; %输出双臂图像 LR.plot(LjointAngleStart); hold on RR.plot(RjointAngleStart); hold on %设置开始运动时双臂的关节角度参数 LjointAngleEnd=[-24.1573 -121.5291 -79.1094 -47.1050 -130.1793 -147.6128]*pi/180; RjointAngleEnd=[-156.6995 -146.9504 81.1299 -135.7610 130.4992 148.6556]*pi/180; % %轨迹求解 % t=[0:1:10]'; % [q,qd,qdd]=jtraj(LjointAngleStart,LjointAngleEnd,t); % [q1,qd1,qdd1]=jtraj(LjointAngleEnd,LjointAngleStart,t); % % [q2,qd2,qdd2]=jtraj(RjointAngleStart,RjointAngleEnd,t); % [q21,qd21,qdd21]=jtraj(RjointAngleEnd,RjointAngleStart,t); % % %输出双臂图像 % LR.plot(q) % hold on % RR.plot(q2) % % % %获取当前图像下的双臂关节角度值 % LR_currentJpos = LR.getpos(); % RR_currentJpos = RR.getpos(); % % %求解当前图像下的双臂齐次矩阵 % LR_currentTpos = LR.fkine(LR_currentJpos); % RR_currentTpos = RR.fkine(RR_currentJpos); % % %求解当前图像下的双臂RPY欧拉角 % LR_rpy = tr2rpy(LR_currentTpos,'deg'); % RR_rpy = tr2rpy(RR_currentTpos,'deg'); % LR.teach(); % RR.teach(); disp('机械臂搭建完毕'); toc; %%-------------------------------------------------------------------------%% %障碍物构建 %%-------------------------------------------------------------------------%% tic; h1 = drawSphere(10, 70, -50, -60); hold on; h2 = drawSphere(10, 50, 0, -10); hold on; h3 = drawSphere(10, 90, 30, -40); hold on; h4 = drawSphere(10, 60, 65, -30); % (10, 70, 60, -40); hold on; h5 = drawSphere(10, 65, 0, -80); hold on; h6 = drawSphere(10, 60, -55, -35); hold on; disp('障碍物搭建完毕'); toc; %%-------------------------------------------------------------------------%% %路径规划 %%-------------------------------------------------------------------------%% %57.3 80.45 -6.299 %57.92 -77.78 -4.805 mapZ = [10, 70, -50, -60; 10, 50, 0, -10; 10, 90, 30, -40; 10, 60, 65, -30; 10, 65, 0, -80; 10, 60, -55, -35]; mapX_Y = 100; countMax = 1000; dis_delta = 2; dis_gap = 225; %%-------------------------------------------------------------------------%% tic; startnode = [57.3 80.45 -6.299]; endnode = [30 10 -80]; % plot3(startnode(:,1),startnode(:,2),startnode(:,3),'g*'); % plot3(endnode(:,1),endnode(:,2),endnode(:,3),'y*'); [path,pathnum,iternode,edgesindex] = rrtPath(startnode,endnode,dis_delta,countMax,mapX_Y,mapZ,dis_gap); % [edgesRowCount, ~] = size(edgesindex); % for ii = 1 : edgesRowCount % plot3(iternode(ii, 1), iternode(ii, 2), iternode(ii, 3),'cyan*', 'linewidth', 1); % plot3([iternode(edgesindex(ii, 1), 1), iternode(edgesindex(ii, 2), 1)], ... % [iternode(edgesindex(ii, 1), 2), iternode(edgesindex(ii, 2), 2)], ... % [iternode(edgesindex(ii, 1), 3), iternode(edgesindex(ii, 2), 3)], ... % 'b', 'LineWidth', 1); % end % % plot3(iternode(:, 1), iternode(:, 2), iternode(:, 3),'cyan*', 'linewidth', 1); plot3(path(:,1), path(:,2), path(:,3), 'r', 'LineWidth', 1); % disp('LeftArm第一条路径完成'); % toc; %%-------------------------------------------------------------------------%% % tic; startnodeL2 = [30 10 -80]; endnodeL2 = [75 10 -50]; [pathL2,~,~,~] = rrtPath(startnodeL2,endnodeL2,dis_delta,countMax,mapX_Y,mapZ,dis_gap); plot3(pathL2(:,1), pathL2(:,2), pathL2(:,3), 'r', 'LineWidth', 1); % disp('LeftArm第二条路径完成'); % toc; %%-------------------------------------------------------------------------%% % tic; startnodeR1 = [57.92 -77.78 -4.805]; endnodeR1 = [30 -10 -80]; [pathR1,~,~,~] = rrtPath(startnodeR1,endnodeR1,dis_delta,countMax,mapX_Y,mapZ,dis_gap); plot3(pathR1(:,1), pathR1(:,2), pathR1(:,3), 'b', 'LineWidth', 1); % disp('RightArm第一条路径完成'); % toc; %%-------------------------------------------------------------------------%% % tic; startnodeR2 = [30 -10 -80]; endnodeR2 = [75 -10 -50]; [pathR2,~,~,~] = rrtPath(startnodeR2,endnodeR2,dis_delta,countMax,mapX_Y,mapZ,dis_gap); plot3(pathR2(:,1), pathR2(:,2), pathR2(:,3), 'b', 'LineWidth', 1); % disp('RightArm第二条路径完成'); toc; t2 = clock; %%-------------------------------------------------------------------------%% %获取当前图像下的双臂关节角度值 LR_currentJpos = LR.getpos(); RR_currentJpos = RR.getpos(); %求解当前图像下的双臂齐次矩阵 LR_currentTpos = LR.fkine(LR_currentJpos); RR_currentTpos = RR.fkine(RR_currentJpos); %求解当前图像下的双臂RPY欧拉角 LR_rpy = tr2rpy(LR_currentTpos,'deg'); RR_rpy = tr2rpy(RR_currentTpos,'deg'); [pathL1_num,~] = size(path); [pathL2_num,~] = size(pathL2); [pathR1_num,~] = size(pathR1); [pathR2_num,~] = size(pathR2); pathL = [path;pathL2]; step_num = 10; LR_Jpos = zeros(pathL1_num+pathL2_num+1-step_num,6); RR_Jpos = zeros(pathR1_num+pathR2_num+1-step_num,6); LR_Jpos(1,:) = LR_currentJpos; RR_Jpos(1,:) = RR_currentJpos; t=[0:5:10]'; for i = 1:(pathL1_num-step_num) LR_currentTpos.t = path(i+step_num,:)'; LR_Jpos(i+1,:) = LR.ikine(LR_currentTpos,'q0',LR_Jpos(i,:)); [Lq,Lqd,Lqdd]=jtraj(LR_Jpos(i,:),LR_Jpos(i+1,:),t); LR.plot(Lq); end for i = 1:(pathR1_num-step_num) RR_currentTpos.t = pathR1(i+step_num,:)'; RR_Jpos(i+1,:) = RR.ikine(RR_currentTpos,'q0',RR_Jpos(i,:)); [Rq,Rqd,Rqdd]=jtraj(RR_Jpos(i,:),RR_Jpos(i+1,:),t); RR.plot(Rq); if(i == pathR1_num-step_num) disp('OK'); end end %获取当前图像下的双臂关节角度值 LR_currentJpos = LR.getpos(); RR_currentJpos = RR.getpos(); LR_Jpos(1,:) = LR_currentJpos; RR_Jpos(1,:) = RR_currentJpos; for i = 1:(pathL2_num-step_num) LR_currentTpos.t = pathL2(i+step_num,:)'; LR_Jpos(i+1,:) = LR.ikine(LR_currentTpos,'q0',LR_Jpos(i,:)); [Lq,Lqd,Lqdd]=jtraj(LR_Jpos(i,:),LR_Jpos(i+1,:),t); LR.plot(Lq); end for i = 1:(pathR2_num-step_num) RR_currentTpos.t = pathR2(i+step_num,:)'; RR_Jpos(i+1,:) = RR.ikine(RR_currentTpos,'q0',RR_Jpos(i,:)); [Rq,Rqd,Rqdd]=jtraj(RR_Jpos(i,:),RR_Jpos(i+1,:),t); RR.plot(Rq); end % %以第四个关节点为例画位移，速度，加速度参数变化图 % figure % %提取矩阵第四列，即为第四个关节点的位移，速度，加速度参数 % q4=q(:,4); % qd4=qd(:,4); % qdd4=qdd(:,4); % subplot(2,2,1) % plot(t,q4) % title('位移变化'); % su