matlab笔记

来源：互联网发布：mac safari关闭标签编辑：程序博客网时间：2024/05/17 03:38

CordPoint=eye(6);
CordPoint(1,:)=[224.7,229.3,329.2,0,0,0];
CordPoint(2,:)=[383.7,400.01,560.43,-90,0,0];
CordPoint(3,:)=[374.5,401.3,1375.7,90,0,0];
CordPoint(4,:)=[374.75,229.32,1508,0,0,0];
%未验证
CordPoint(5,:)=[383.7,0,560.43,0,0,0];
CordPoint(6,:)=[383.7,0,560.43,0,0,0];

VCordPoint1=Trsf2Matrix(CordPoint(1,:));
VCordPoint2=Trsf2Matrix(CordPoint(2,:));
VCordPoint3=Trsf2Matrix(CordPoint(3,:));
VCordPoint4=Trsf2Matrix(CordPoint(4,:));
VCordPoint5=Trsf2Matrix(CordPoint(5,:));
VCordPoint6=Trsf2Matrix(CordPoint(6,:));

if Flag>0
Matrix=eye(4);
end

if Flag>1
VCordPoint1=(Matrix)*VCordPoint1;
Matrix=VCordPoint1*Trsf2Matrix([0,0,0,0,0,Joint(1)])*inv(VCordPoint1)*Matrix;
end

if Flag>2
VCordPoint2=(Matrix)*VCordPoint2;
Matrix=VCordPoint2*Trsf2Matrix([0,0,0,0,0,Joint(2)])*inv(VCordPoint2)*Matrix;
end

if Flag>3
VCordPoint3=(Matrix)*VCordPoint3;
Matrix=VCordPoint3*Trsf2Matrix([0,0,0,0,0,Joint(3)])*inv(VCordPoint3)*Matrix;
end

if Flag>4
VCordPoint4=(Matrix)*VCordPoint4;
Matrix=VCordPoint4*Trsf2Matrix([0,0,0,0,0,Joint(4)])*inv(VCordPoint4)*Matrix;
end

if Flag>5
VCordPoint5=(Matrix)*VCordPoint5;
Matrix=VCordPoint5*Trsf2Matrix([0,0,0,0,0,Joint(5)])*inv(VCordPoint5)*Matrix;
end

if Flag>6
VCordPoint6=(Matrix)*VCordPoint6;
Matrix=VCordPoint6*Trsf2Matrix([0,0,0,0,0,Joint(6)])*inv(VCordPoint6)*Matrix;
end