matlab robotic toolbox工具坐标系下的雅可比矩阵

%%计算工具坐标系下的雅可比矩阵

clear,clc,close all;

format compact

syms d1 d2 d3 d4 d5 d6 a2 a3 alp1 alp4 alp5

syms q1 q2 q3 q4 q5 q6

%% 建立机器人DH参数，初始状态为竖直状态

% 连杆偏移d,连杆长度a,连杆扭转角alpha

L(1)=RevoluteMDH('d',d1,'a',0,'alpha',0);

L(2)=RevoluteMDH('d',0,'a',0,'alpha',alp1,'offset',0); %-pi/2

L(3)=RevoluteMDH('d',0,'a',a2,'alpha',0);

L(4)=RevoluteMDH('d',d4,'a',a3,'alpha',0,'offset',0);

L(5)=RevoluteMDH('d',d5,'a',0,'alpha',alp4);

L(6)=RevoluteMDH('d',d6,'a',0,'alpha',alp5);

robot=SerialLink(L,'name','robot');

q=[q1 q2 q3 q4 q5 q6];

%工具坐标系下的几何雅可比矩阵，雅可比将关节空间速度映射到末端效应器速度(相对于基坐标系)。

je0=robot.jacob0(q);

je0=subs(je0,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);

je0=simplify(je0);

je=robot.jacobe(q);   %空间雅可比，即空间固定坐标形下的雅克比矩阵，参考现代机器人学5.1.1

je=subs(je,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);

je=simplify(je);

T01=L(1).A(q(1));T12=L(2).A(q(2));T23=L(3).A(q(3));

T34=L(4).A(q(4));T45=L(5).A(q(5));T56=L(6).A(q(6));

T01=double(T01);T12=double(T12);T23=double(T23);

T34=double(T34);T45=double(T45);T56=double(T56);

%% parameters

syms db dt

%% transition matrix for robot

Tbase = [1 0 0 0;

       0 1 0 0;

       0 0 1 db;

       0 0 0 1];  

 

Ttool = [1 0 0 0;

       0 1 0 0;

       0 0 1 dt;

       0 0 0 1];

T = {Tbase*T01 T12 T23 T34 T45 T56};

%%

U = Ttool ;

for i = 6:-1:1

   Jn(:,i) = [ -U(1,1).*U(2,4)+U(2,1).*U(1,4);

               -U(1,2).*U(2,4)+U(2,2).*U(1,4);

               -U(1,3).*U(2,4)+U(2,3).*U(1,4);

               U(3,1);

               U(3,2);

               U(3,3)];

    U = T{i}*U;

end

%Jn 关节速度映射到世界坐标系下末端执行器空间速度，几何雅可比

Jn=subs(Jn,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);

Jn=simplify(Jn)

Jn = 

FK = Tbase * T01 * T12 * T23 * T34 * T45 * T56 * Ttool;

FK=subs(FK,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);

FK=simplify(FK)

FK = 

FK66 = FK(1:3,1:3);

RR = [ FK66 zeros(3,3);

      zeros(3,3) FK66];

Jn_Base = RR*Jn;   %末端执行器自身坐标系的雅可比

付费内容图片：

需要技术服务联系qq2386317960