控制科学与工程：随手笔记（2）--工业机器人仿真、matlab
介绍了python库的安装及其链接
https://blog.csdn.net/weixin_41826637/article/details/80966836
不懂的matlab函数
| 函数 | 用法 |
|---|---|
| inv | 矩阵求逆---------inv(A)*b=A\b |
| pause | pause(n)----暂停n秒 pause;-----暂停等待按键触发 |
使用matlab基本函数进行机器人建模仿真
Build_6DOF_Robot_DH.m
// A highlighted block
%% 建立机器人DH参数表
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';
Link=struct('name','Body','theta',0,'di',0,'ai',0,'alpha',0,'az',UZ);
Link(1)=struct('name','Base','theta',0,'di',0,'ai',0,'alpha',0,'az',UZ);
Link(2)=struct('name','J1','theta',pi/2,'di',100,'ai',0,'alpha',-pi/2,'az',UZ);
Link(3)=struct('name','J2','theta',-pi/2,'di',0,'ai',0,'alpha',-pi/2,'az',UZ);
Link(4)=struct('name','J3','theta',0,'di',80,'ai',0,'alpha',0,'az',UZ);
Link(5)=struct('name','J4','theta',0,'di',80,'ai',0,'alpha',pi/2,'az',UZ);
Link(6)=struct('name','J5','theta',0,'di',0,'ai',0,'alpha',-pi/2,'az',UZ);
Link(7)=struct('name','J6','theta',pi/2,'di',80,'ai',0,'alpha',pi/2,'az',UZ);
Link(8)=struct('name','J7','theta',0,'di',50,'ai',0,'alpha',0,'az',UZ);
Link(9)=struct('name','J8','theta',0,'di',-100,'ai',0,'alpha',0,'az',UZ);
Build_DH_Matrix.m
function Build_DH_Matrix(JointNum)
%% 输入关节数量，生成齐次变换矩阵A、旋转矩阵R
global Link
for i=1:JointNum+3
%获取关节参数
Cth=cos(Link(i).theta);
Sth=sin(Link(i).theta);
Ca=cos(Link(i).alpha);
Sa=sin(Link(i).alpha);
ai=Link(i).ai;
di=Link(i).di;
%添加关节变换信息
Link(i).x=[Cth Sth 0 0]';
Link(i).y=[-Sth*Ca Cth*Ca Sa 0]';
Link(i).z=[Sth*Sa -Cth*Sa Ca 0]';
Link(i).p=[ai*Cth ai*Sth di 1]';
Link(i).R=[Link(i).x(1:3),Link(i).y(1:3),Link(i).z(1:3)];
Link(i).A=[Link(i).x,Link(i).y, Link(i).z, Link(i).p];
end
Connect3D.m
function Connect3D(p1,p2,option,pt)
h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)
DH_FK_6DOF.m
function pic=DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,th7,th8,fcla)
global Link
ToDeg = 180/pi;
ToRad = pi/180;
Build_6DOF_Robot_DH; %构建DH表
% 获取关节变量
Link(2).theta=th1*ToRad;
Link(3).theta=th2*ToRad;
Link(4).di=80+dz3;
Link(5).theta=th4*ToRad;
Link(6).theta=th5*ToRad;
Link(7).theta=th6*ToRad;
Link(8).theta=th7*ToRad;
Link(9).theta=th8*ToRad;
Build_DH_Matrix(6); %计算齐次变换矩阵
radius = 10; %初始化
len = 30;
joint_col = 0;
plot3(0,0,0,'ro');
% 绘制连杆及其关节
for i=2:9
Link(i).A =Link(i-1).A*Link(i).A;
Link(i).x= Link(i).A(1:3,1);
Link(i).y= Link(i).A(1:3,2);
Link(i).z= Link(i).A(1:3,3);
Link(i).p= Link(i).A(1:3,4);
Link(i).R=[Link(i).x,Link(i).y,Link(i).z];
Connect3D(Link(i-1).p,Link(i).p,'b',3);
plot3(Link(i).p(1),Link(i).p(2),Link(i).p(3),'rx');
hold on;
if i<=7
DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;
end
end
% 设置坐标系信息
grid on;
%view(129,28);
axis([-400,400,-400,400,-400,400]);
xlabel('x');
ylabel('y');
zlabel('z');
%生成动画帧
drawnow;
pic=getframe;
% 判断是否清空坐标系
if(fcla)
cla;
end
DH_IK_6DOF.m
function [th1,th2,dz3,th4,th5,th6] = DH_IK_6DOF(Target)
global Link
ToDeg = 180/pi;
ToRad = pi/180;
Xc=Target.p(1)-80*Target.y(1);
Yc=Target.p(2)-80*Target.y(2);
Zc=Target.p(3)-80*Target.y(3);
th1=atan2(Yc,Xc);
th2=-atan2(Zc-100,sqrt((Xc)^2+(Yc)^2))-pi/2;
dz3=sqrt((Xc)^2+(Yc)^2+(Zc-100)^2)-160;
th4=atan2((sin(th1)*Target.y(1)-cos(th1)*Target.y(2)),cos(th1)*cos(th2)*Target.y(1)+sin(th1)*cos(th2)*Target.y(2)-sin(th2)*Target.y(3))+pi;
th5=atan2((sqrt(1-(-cos(th1)*sin(th2)*Target.y(1)-sin(th1)*sin(th2)*Target.y(2)-cos(th2)*Target.y(3))^2)),(-cos(th1)*sin(th2)*Target.y(1)-sin(th1)*sin(th2)*Target.y(2)-cos(th2)*Target.y(3)));
th6=-atan2((-cos(th1)*sin(th2)*Target.x(1)-sin(th1)*sin(th2)*Target.x(2)-cos(th2)*Target.x(3)),(-cos(th1)*sin(th2)*Target.z(1)-sin(th1)*sin(th2)*Target.z(2)-cos(th2)*Target.z(3)))+pi/2;
th1=th1*ToDeg;
th2=th2*ToDeg;
th4=th4*ToDeg;
th5=th5*ToDeg;
th6=th6*ToDeg;
Draw_6DOF_Workplace.m
close all;
clc;
clear;
global Link
ToDeg = 180/pi;
ToRad = pi/180;
num=1;
th_interval = 30;
d_interval = 10;
for th1=-180:th_interval:180
for th2=-150:th_interval:150
for dz3=0:d_interval:100
for th4=-150:th_interval:150
for th5=-150:th_interval:150
for th6=-150:th_interval:150
theta1=th1*ToRad;
theta2=th2*ToRad;
di3=dz3;
theta4=th4*ToRad;
theta5=th5*ToRad;
theta6=th6*ToRad;
A1=[cos(theta1) 0 -sin(theta1) 0;
sin(theta1) 0 cos(theta1) 0;
0 -1 0 100;
0 0 0 1];
A2=[cos(theta2) 0 -sin(theta2) 0;
sin(theta2) 0 cos(theta2) 0;
0 -1 0 0;
0 0 0 1];
A3=[1 0 0 0;
0 1 0 0;
0 0 1 dz3;
0 0 0 1];
A4=[cos(theta4) 0 sin(theta4) 0;
sin(theta4) 0 -cos(theta4) 0;
0 1 0 80;
0 0 0 1];
A5=[cos(theta5) 0 -sin(theta5) 0;
sin(theta5) 0 cos(theta5) 0;
0 -1 0 0;
0 0 0 1];
A6=[cos(theta6) 0 sin(theta6) 0;
sin(theta6) 0 -cos(theta6) 0;
0 1 0 80;
0 0 0 1];
A = A1 * A2 * A3 * A4 * A5 * A6;
point1(num) = A(1,4);
point2(num) = A(2,4);
point3(num) = A(3,4);
num = num + 1;
end
end
end
end
end
end
plot3(point1,point2,point3,'r*');
axis([-500,500,-500,500,-500,500]);
xlabel('x');
ylabel('y');
zlabel('z');
%view(-90,0);
%view(0,90);
grid on;
DrawCylinder.m
function h = DrawCylinder(pos, az, radius,len, col)
% draw closed cylinder
%
%******** rotation matrix
az0 = [0;0;1];
ax = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps
rot = eye(3);
else
ax = ax/ax_n;
ay = cross(az,ax);
ay = ay/norm(ay);
rot = [ax ay az];
end
%********** make cylinder
% col = [0 0.5 0]; % cylinder color
a = 20; % number of side faces
theta = (0:a)/a * 2*pi;
x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));
for n=1:size(x,1)
xyz = [x(n,:);y(n,:);z(n,:)];
xyz2 = rot * xyz;
x2(n,:) = xyz2(1,:);
y2(n,:) = xyz2(2,:);
z2(n,:) = xyz2(3,:);
end
%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);
for n=1:2
patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
end
Err_6DOF.m
function err = Err_6DOF(Target, Current)
a=eye(3);
Target.p= Target.A(1:3,4);
Target.x= Target.A(:,1);
Target.y= Target.A(:,2);
Target.z= Target.A(:,3);
Target.R=[Target.x(1:3),Target.y(1:3),Target.z(1:3)];
Perr = Target.p - Current.p(1:3);
Rerr = Current.R' * Target.R;
b=[Rerr(3,2)-Rerr(2,3),Rerr(1,3)-Rerr(3,1),Rerr(2,1)-Rerr(1,2)]';
theta = acos((Rerr(1,1)+Rerr(2,2)+Rerr(3,3)-1)/2.0);
% if theta==0
% Werr=[0,0,0]';
% else
% Werr = theta/(2.0*sin(theta)) * [Rerr(3,2)-Rerr(2,3);Rerr(1,3)-Rerr(3,1);Rerr(2,1)-Rerr(1,2)];
% Werr = Current.R * Werr;
% end
if Rerr~=a
Werr=theta/(2*sin(theta))*b;
else
Werr=[0 0 0]';
end
err = [Perr;Werr];
MOV_6DOF_Robot.m
%% 初始化
close all;
clear;
clc;
ToDeg = 180/pi;
ToRad = pi/180;
step=20;
th1=90;th2=-90;dz3=0;th4=0;th5=0;th6=90;
%view(-90,0);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
pause;
% 关节1
for i=0:step:180
DH_FK_6DOF(th1+i,th2,dz3,th4,th5,th6,0,0,1);
end
for i=180:-step:0
if i==0
DH_FK_6DOF(th1+i,th2,dz3,th4,th5,th6,0,0,0);
else
DH_FK_6DOF(th1+i,th2,dz3,th4,th5,th6,0,0,1);
end
end
%% 关节2
for i=0:step:180
DH_FK_6DOF(th1,th2+i,dz3,th4,th5,th6,0,0,1);
end
for i=180:-step:0
if i==0
DH_FK_6DOF(th1,th2+i,dz3,th4,th5,th6,0,0,0);
else
DH_FK_6DOF(th1,th2+i,dz3,th4,th5,th6,0,0,1);
end
end
%% 关节3
for i=0:10:100
DH_FK_6DOF(th1,th2,dz3+i,th4,th5,th6,0,0,1);
end
for i=100:-10:0
if i==0
DH_FK_6DOF(th1,th2,dz3+i,th4,th5,th6,0,0,0);
else
DH_FK_6DOF(th1,th2,dz3+i,th4,th5,th6,0,0,1);
end
end
%% 关节4
for i=0:step:180
DH_FK_6DOF(th1,th2,dz3,th4+i,th5,th6,0,0,1);
end
for i=180:-step:0
if i==0
DH_FK_6DOF(th1,th2,dz3,th4+i,th5,th6,0,0,0);
else
DH_FK_6DOF(th1,th2,dz3,th4+i,th5,th6,0,0,1);
end
end
%% 关节5
for i=0:step:180
DH_FK_6DOF(th1,th2,dz3,th4,th5+i,th6,0,0,1);
end
for i=180:-step:0
if i==0
DH_FK_6DOF(th1,th2,dz3,th4,th5+i,th6,0,0,0);
else
DH_FK_6DOF(th1,th2,dz3,th4,th5+i,th6,0,0,1);
end
end
% 关节6
for i=0:step:180
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6+i,0,0,1);
end
for i=180:-step:0
if i==0
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6+i,0,0,0);
else
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6+i,0,0,1);
end
end
Restrain_Value.m
function x = Restrain_Value(x,min,max)
if xmax
x=max;
else
x=x;
end
Build_6DOF_Jacobian.m
function J=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6)
%% 输入关节变量，输出jacobian
global Link
ToDeg = 180/pi;
ToRad = pi/180;
JointNum=6;
J=zeros(6,6);
Link(2).theta=th1*ToRad;
Link(3).theta=th2*ToRad;
Link(4).di=80+dz3;
Link(5).theta=th4*ToRad;
Link(6).theta=th5*ToRad;
Link(7).theta=th6*ToRad;
Build_DH_Matrix(JointNum);
Link(1).p=Link(1).p(1:3);
for i=2:9
Link(i).A =Link(i-1).A*Link(i).A;
Link(i).x= Link(i).A(1:3,1);
Link(i).y= Link(i).A(1:3,2);
Link(i).z= Link(i).A(1:3,3);
Link(i).p= Link(i).A(1:3,4);
Link(i).R=[Link(i).x,Link(i).y,Link(i).z];
end
for i=2:7
if i==4
J(:,i-1)=[Link(i-1).z(1:3);[0 0 0]'];
else
J(:,i-1)=[cross(Link(i-1).z(1:3),(Link(7).p(1:3)-Link(i-1).p(1:3)));Link(i-1).z(1:3)];
end
end
% for i=1:6
% for j=1:6
% fprintf('%2.2f ,',J(i,j));
% end
% fprintf('\n');
% end
Draw_6DOF.m
close all;
clc;
clear;
global Link
ToDeg = 180/pi;
ToRad = pi/180;
th1=130;
th2=-60;
dz3=10;
th4=-120;
th5=72;
th6=110;
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
%view(-90,0);
% pause;
% view(0,90);
Target.A=Link(7).A;
Target.x=Target.A(:,1);
Target.y=Target.A(:,2);
Target.z=Target.A(:,3)';
Target.p=Target.A(1:3,4)';
num=1;
%% 解析解运动
pause;
for i=1:20
Target.p(3)=Target.p(3)+i;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h=plot3(x,y,z,'r.');hold on;
end
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
%% 微分运动
pause;
j=20;%z轴方向上的移动总步数
step=-1;%步长
for i=1:j
J1=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6);
[U,S,V]=svd(J1);
J=V*(inv(S))*(U');
D=[0 0 step*i 0 0 0]';
dth=J*D;
th1=th1+dth(1)*ToDeg;
th2=th2+dth(2)*ToDeg;
dz3=dz3+dth(3);
th4=th4+dth(4)*ToDeg;
th5=th5+dth(5)*ToDeg;
th6=th6+dth(6)*ToDeg;
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h=plot3(x,y,z,'r.');hold on;
end
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
%% 写郑----直接描
pause;
num=1;
step2=1;
cen_point=[-50 0 -50]';
x=[];
y=[];
z=[];
for i=5:step2*0.7:15
Target.p=[i 150 -8*i+260]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h=plot3(x,y,z,'r.');hold on;
end
for i=27:-step2:17
Target.p=[i 150 8*i+8]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h=plot3(x,y,z,'r.');hold on;
end
for i=0:2*step2:31
Target.p=[i 150 140]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h=plot3(x,y,z,'r.');hold on;
end
for i=-2:2*step2:36
Target.p=[i 150 80]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h=plot3(x,y,z,'r.');hold on;
end
for i=16:-step2*0.5:4
Target.p=[i 150 (i-6)*40/3]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=16:step2:32
Target.p=[i 150 -40/6*(i-16)+80]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=140:-step2*7:-50
Target.p=[50 150 i]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=50:step2*2:80
Target.p=[i 150 140]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=80:-step2:60
Target.p=[i 150 3*(i-40)+20]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=60:step2:90
Target.p=[i 150 -2*(i-40)+120]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=90:-step2*2:65
Target.p=[i 150 0.5*i-25]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
for i=65:-step2:57
Target.p=[i 150 -2*i+140]'+cen_point;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
h= plot3(x,y,z,'r.');hold on;
end
view(0,0);
%% 写郑---图片形式
pause;
view(150,15);
num=1;
load('FP.mat');
[FPh,FPl]=size(FP);
x=[];
y=[];
z=[];
for i=1:FPh;
Target.p(1)=FP(i,2);
Target.p(2)=150;
Target.p(3)=FP(i,1);
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
x(num)=Link(7).p(1);
y(num)=Link(7).p(2);
z(num)=Link(7).p(3);
num=num+1;
plot3(x,y,z,'r.');hold on;
if i==FPh
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
else
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
end
end
view(180,0);
%% 牛顿拉夫逊迭代法
% pause;
% th1=100;
% th2=-60;
% dz3=10;
% th4=-100;
% th5=72;
% th6=10;
% DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
% [th1,th2,dz3,th4,th5,th6]=Newton_Raphson(th1,th2,dz3,th4,th5,th6,Target);
% DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
Word.m
clear,clc
A=imread('郑.jpg');
B=rgb2gray(A);
g_max=double(max(max(B)));
g_min=double(min(min(B)));
T=round((g_max-g_min)/2);
D=(double(B)>=T);
[Dm,Dn]=size(D);
num=1;
for m=1:Dm
if mod(m,2)==0
n=Dn;
for ii=1:Dn
if D(m,n)==0
FP(num,1)=(Dm-m)-200;%z轴坐标
FP(num,2)=(Dn-n)-200;%x轴坐标
num=num+1;
end
n=n-1;
end
else
n=1;
for ii=1:Dn
if D(m,n)==0
FP(num,1)=(Dm-m)-200;
FP(num,2)=(Dn-n)-200;
num=num+1;
end
n=n+1;
end
end
end
save('FP.mat','FP');
Newton_Raphson.m(此部份仍然有问题，无法收敛到最小值)
function [th1,th2,dz3,th4,th5,th6] = Newton_Raphson(th1,th2,dz3,th4,th5,th6,Target)
global Link
ToDeg = 180/pi;
ToRad = pi/180;
eps=1e-6;
num=1;
while 1
figure(1);
J1=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6);
%view(-90,0);
%view(0,90);
err = Err_6DOF(Target,Link(7));
E = err'*err;
if E