六自由度机器人正运动学分析并验证和蒙特卡洛工作空间分析并输出图像

clc;clear; a=[0,0.440, 0.260, 0.970, 0.50, 0.00 ];%MDH参数 d=[-0.186, 0, 0, 0, 0 , 0 ]; alp=[-pi, pi/2, pi/2, 0, pi/2, pi/2 , pi/2 ]; %机器人工具箱模型，每一个L建立一个连杆，输入DH参数即可，offset是连杆的初始偏移量，此处为0 L(1) = Link('d', d(1), 'a', a(1), 'alpha', alp(1), 'offset', -pi/2,'modified'); 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), 'offset', -3*pi/4 ,'modified'); L(4) = Link('d', d(4), 'a', a(4), 'alpha', alp(4), 'offset',-1*pi/4,'modified'); L(5) = Link('d', d(5), 'a', a(5), 'alpha', alp(5),'modified'); L(6) = Link('d', d(6), 'a', a(6), 'alpha', alp(6),'modified'); bot = SerialLink(L,'name','rbt');%将六个连杆组合起来 First_Theta = [0 0 0 0 0 0]; Final_Theta = [150*pi/180 0*pi/180 150*pi/180 90*pi/180 360*pi/180 360*pi/180]; step = 10; [q,qd,qdd] = jtraj(First_Theta,Final_Theta,step); bot.display();%显示机器人模型 view(3) view([0.001,0,2]) bot.plot(q,'workspace',[-1,3 -2 2 -2 2],'tilesize',2,'scale',0.5,'jointdiam',2) % bot.teach; q0=[1,-1,1,-1,1,1];%给定初始角度 T_rbt = bot.fkine(q0);%采用机器人工具箱函数求正运动学 T_fkine = fkinematics(q0);%编写的正运动学 fkine_err = double(T_rbt)-T_fkine%计算RTB计算的正运动学和编写的函数计算得到的正运动学误差 %% 保存为gif % filename = 'robot_animation.gif'; % figure(2) % view(3) % view([0.001,0,2]) % bot.plot(q(1,:),'workspace',[-1,3 -2 2 -2 2],'tilesize',2,'scale',0.5,'jointdiam',2); % for i = 1:2:step % hold on % bot.plot(q(i,:),'workspace',[-1,3 -2 2 -2 2],'tilesize',2,'scale',0.5,'jointdiam',2); % drawnow; % % % 将当前图像写入gif文件 % frame = getframe(gcf); % im = frame2im(frame); % [imind,cm] = rgb2ind(im,256); % if i == 1 % imwrite(imind,cm,filename,'gif','Loopcount',inf); % else % imwrite(imind,cm,filename,'gif','WriteMode','append'); % end % end % % disp("动画已保存为"+filename);