Copyright © 2022AC今夜有雨. All rights reserved.
clear,clc,close all
format compact
DH=DH_Param(4);JointNum=length(DH);
ROCR6v2 关节2、3偏置
qlim=deg2rad([-179,179;-146,146;-146,146;-179,179;-179,179;-179,179]);
for i=1:JointNum
L(i)=RevoluteMDH(‘d’,DH(i,3),‘a’,DH(i,2),‘alpha’,deg2rad(DH(i,1)), …
‘offset’,deg2rad(DH(i,4)),‘qlim’,qlim(i,:));
end
robot=SerialLink(L,‘name’,‘robot’);
robot.display();
robot =
robot:: 6 axis, RRRRRR, modDH, fastRNE
±–±----------±----------±----------±----------±----------+
| j | theta | d | a | alpha | offset |
±–±----------±----------±----------±----------±----------+
| 1| q1| 0.1215| 0| 0| 0|
| 2| q2| 0.1225| 0| 1.5708| -1.5708|
| 3| q3| -0.102| -0.3| 0| 0|
| 4| q4| 0.09| -0.276| 0| -1.5708|
| 5| q5| 0.09| 0| 1.5708| 0|
| 6| q6| 0.082| 0| -1.5708| 0|
±–±----------±----------±----------±----------±----------+
%% 绘制三种类型的B样条曲线，需要前面所给的所有.m文件
%控制顶点
P=[150,50, -120, -223, -258, -303;
-480,-420, -460, -485, -306,-265;
340, 340,340,340,340,340]*0.001;
P(1:2,:)=P(1:2,:)*1;
[m,n]=size§;
k = 3;FixedStep=0.0001;StopTime=1;
flag = 2;% 绘制准均匀B样条曲线
% figure(‘Color’,[1,1,1],‘Position’,[485,180,684,330],‘Visible’,‘on’);
position=Switch_flag(P,n-1,k,flag,StopTime,FixedStep);
T=position’;
SimpleNum=(0:FixedStep:StopTime-FixedStep)*50;
Cunt=length(SimpleNum);q=zeros(6,Cunt)';
InitAngle=[-60.865,-12.697,-69.496,-7.8067,90,119.13]/180*pi*0;
RPY=deg2rad([180,0,-90]);
[X,Y,Z]=deal(zeros(1,Cunt));
for i=1:Cunt
% if i==1
% FK = Pos2Tr_XYZ([T(i,:),RPY]);
% %自己编写求逆解的函数，已判断是否在转动范围以内
% [~,Theta,~]=InverseMDH_ZY(DH,FK,InitAngle);
% else
FK = Pos2Tr_XYZ([T(i,:),RPY]);
%自己编写求逆解的函数，已判断是否在转动范围以内
[~,Theta,~]=InverseMDH_ZY(DH,FK,InitAngle);
InitAngle=Theta;
% end
q(i,:)=Theta*180/pi;
end
qT=q’;
% figure1=figure(‘Color’,[1,1,1],‘Position’,[485,180,536,325],‘Name’,‘B样条末端运动轨迹’);
W=[-700,+400,-500,+400,-0,+600]*0.001;
hold on
plot3(position(1,:)‘,position(2,:)’,position(3,:)',‘b’);hold on;grid on;axis(W);
q=q’*pi/180;save(‘SimBSplineData.mat’,‘q’);
xlabel(‘x(m)’);ylabel(‘y(m)’);zlabel(‘z(m)’);
robot.model3d = ‘ROCR6/MDH_stl’;
qc=q(:,1:round(end/100):end);
light(‘Position’, [1 0.5 1]);
fileName= [‘CartesianSpace_’,datestr(datetime(‘now’),‘yyyy-mm-dd-HH-MM’),‘.gif’];
robot.plot3d(qc’,‘workspace’,W,‘movie’,fileName, …
‘view’,[40 31],‘delay’,0.001,‘trail’,{‘r’,‘LineWidth’,4},‘nowrist’) %显示三维动画
Animate: saving video --> CartesianSpace_2022-05-01-15-29.gif with profile ‘GIF’
Loading STL models from ARTE Robotics Toolbox for Education by Arturo Gil (http://arvc.umh.es/arte)…
axes=gca;
axes.CameraViewAngleMode=‘manual’;
axes.CameraViewAngle=7.5;
gf=gcf;
gf.Position=[400,300,720,420];
saveas(gf,‘B样条末端运动轨迹’,‘svg’)
figure(‘Color’,[1,1,1],‘Position’,[485,180,536,325]);
for i=1:6
plot(SimpleNum,qT(i,1:Cunt),‘LineWidth’,2);hold on;
end
string=‘关节角度(deg)’;
xlabel(‘t(s)’),ylabel(string,‘Interpreter’,‘tex’,‘FontSize’,10);grid on;
legend('joint1 ',‘joint2’,‘joint3’,‘joint4’,‘joint5’,‘joint6’);
figure2 = figure(‘Color’,[1,1,1],‘Position’,[485,180,684,330],‘Name’,‘关节角度’);
% 创建 subplot
for i=1:6
string=[‘theta’,num2str(i),’ (deg)'];
subplot1 = subplot(3,2,i,‘Parent’,figure2);
plot(SimpleNum,qT(i,1:Cunt),‘Parent’,subplot1,‘LineWidth’,2,‘Color’,[1 0 0]);grid on;
% 创建 ylabel
ylabel(string,‘Interpreter’,‘tex’,‘FontSize’,10);
% 创建 xlabel
xlabel(‘t(s)’,‘Interpreter’,‘tex’,‘FontSize’,10);
end
saveas(figure2,figure2.Name,‘svg’)
g,‘Interpreter’,‘tex’,‘FontSize’,10);
% 创建 xlabel
xlabel(‘t(s)’,‘Interpreter’,‘tex’,‘FontSize’,10);
end
saveas(figure2,figure2.Name,‘svg’)