clear;
clc;
% 六轴twist
T1=[0; 0; 0; 0; 0; -1];
T2=[0;-491; 0; -1; 0; 0];
T3=[0;-841; 0; -1; 0; 0];
T4=[-841;0; 0; 0; 1; 0];
T5=[0;-841; 350; -1; 0; 0];
T6=[350; 0; 0; 0; 0; 1];
twists=[T1 T2 T3 T4 T5 T6];
% 六轴的初始位姿
Gst0=diag([1 1 1 1]);%eye（4）
Gst0(1:3,4)=[0;350;757]; %第四列的1~3个元素，若Gst0(4,1:3）则为第四行的
% theta的初始值
theta_init=[0;0;0;0;0;0];
theta_func=theta_init;
% gd是目标点的位姿矩阵
% Gd=diag([1 1 1 1]);
% Gd(1:3,4)=[-200;-250;326.5708];
Gd=[
-0.0000 1.0000 0 -200.0000;
-1.0000 -0.0000 0 -250.0000;
0 0 1.0000 326.5708;
0 0 0 1.0000;
];
% 初始化误差
error=10;
i=0;
line = zeros(1,16);
%--------------------------------------------------------------------------
while error > 0.01
forward=Gst_theta(theta_func,twists,Gst0)*inv(Gd);
psi_theta=inverse_Vee(logm(forward));
jac=jacobian(theta_func,twists);
jac_inv=pinv(jac);
theta_new=theta_func-jac_inv*psi_theta;
error=abs(norm(psi_theta));
line(1,i+1)=error;
theta_func=theta_new;
i=i+1;
end
plot([0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15],line,'r-o');
hold on;
display(theta_func);
display(Gst0);
display(Gd);
%--------------------------------------------------------------------------
% 将twist转换成变换矩阵形式
function out=twist2g(T,theta)
v=T(1:3,1);
w=T(4:6,1);
out=diag([1 1 1 1]);
if sum(w)==0
out(1:3,4)=theta*v;
else
out(1:3,4)=(eye(3)-e2rot(w,theta))*cross(w,v)+(w*w'*v*theta);
end
out(1:3,1:3)=e2rot(w,theta);
end
% 将指数形式变换成矩阵形式
function out=e2rot(w,theta)
what=w2hat(w);
out=eye(3)+what*sin(theta)+what*what*(1-cos(theta));
end
% 求w的hat运算
function out=w2hat(w)
out(1,2)=-w(3);
out(1,3)= w(2);
out(2,1)= w(3);
out(2,3)=-w(1);
out(3,1)=-w(2);
out(3,2)= w(1);
end
% Vee的逆运算
function out=inverse_Vee(Vee)
out(1,1)=Vee(1,4);
out(2,1)=Vee(2,4);
out(3,1)=Vee(3,4);
out(4,1)=Vee(3,2);
out(5,1)=Vee(1,3);
out(6,1)=Vee(2,1);
end
% 正运动学运算
function out=Gst_theta(theta,twists,Gst0)
out=eye(4);
for i=1:6
twist=twists(1:6,i);
out=out*twist2g(twist,theta(i,1));
end
out=out*Gst0;
end
% 计算雅克比矩阵
function out=jacobian(theta,twists)
out=eye(6);
for i=1:6
if i==1
out(1:6,i)=twists(1:6,i);
else
temp=eye(6);
for l=1:i-1
twist=twists(1:6,l);
temp=temp*Ad(twist2g(twist,theta(l,1)));
end
out(1:6,i)=temp*twists(1:6,i);
end
end
end
% 伴随变换Ad
function out=Ad(matrix)
p=matrix(1:3,4);
R=matrix(1:3,1:3);
out(1:3,1:3) = R;
out(1:3,4:6) = w2hat(p)*R;
out(4:6,1:3) = 0;
out(4:6,4:6) = R;
end
test.zip_SCARA逆解_scara正逆解_六轴 逆_六轴机械臂_机械臂运动
版权申诉
120 浏览量
2022-07-14
19:15:50
上传
评论 1
收藏 15KB ZIP 举报
test.zip （11个子文件） 
test 
moving.m 496B Newton.m 3KB
牛顿迭代__ding.txt 3KB rel.m 510B Helios_test.m 683B Build_SCARA.m 468B mix.m 145B 四轴.txt 780B move.m 3KB move_scara.m 822B
六轴.jpg 13KB
评论0