robotics section 5 jacobian matlab practice

题目

代码

% (3)

% 1)
clear;clc;

%初始化参数
L=[4;3;2];  %连杆长度
Theta=deg2rad([10;20;30]);  %起始关节角
X_d=[0.2;-0.3;-0.2];    %恒定的期望笛卡尔速度
W=[1;2;3];  %恒定的期望笛卡尔力和力矩
dt=0.1; %时步
duration=5; %仿真时间

%机器人工具箱
link(1)=Link('alpha',0,'a',0,'d',0,'modified');
link(2)=Link('alpha',0,'a',L(1),'d',0,'modified');
link(3)=Link('alpha',0,'a',L(2),'d',0,'modified');
bot=SerialLink(link,'name','robot','tool',[L(3),0,0]);

%创建各个参数的list，用于存储
Theta_d_list=zeros([3,duration/dt]);
Theta_list=zeros([3,duration/dt]);
X_list=zeros([3,duration/dt]);
J_det_list=zeros([1,duration/dt]);
T_list=zeros([3,duration/dt]);

%仿真
idex=1; %list的下标
for t=0:dt:duration
    
    %以当前的关节角度求出以下参数：
    %雅可比矩阵
    J=[-L(1)*sin(Theta(1))-L(2)*sin(Theta(1)+Theta(2))-L(3)*sin(Theta(1)+Theta(2)+Theta(3))... 
       -L(2)*sin(Theta(1)+Theta(2))-L(3)*sin(Theta(1)+Theta(2)+Theta(3))...
       -L(3)*sin(Theta(1)+Theta(2)+Theta(3));
        L(1)*cos(Theta(1))+L(2)*cos(Theta(1)+Theta(2))+L(3)*cos(Theta(1)+Theta(2)+Theta(3))...
        L(2)*cos(Theta(1)+Theta(2))+L(3)*cos(Theta(1)+Theta(2)+Theta(3))...
        L(3)*cos(Theta(1)+Theta(2)+Theta(3));
        1 1 1;];
    %关节角速度，式5.72
    Theta_d=J\X_d;
    %3个笛卡尔分量
    %由T0H的第1行第4列得x，%由T0H的第2行第4列得y，%三个关节角相加得phi
    X=[L(3)*cos(Theta(1)+Theta(2)+Theta(3))+L(2)*cos(Theta(1)+Theta(2))+L(1)*cos(Theta(1));
        L(3)*sin(Theta(1)+Theta(2)+Theta(3))+L(2)*sin(Theta(1)+Theta(2))+L(1)*sin(Theta(1));
        Theta(1)+Theta(2)+Theta(3)];
    %雅可比矩阵行列式
    J_det=det(J);
    %3个活动关节力矩，式5.97
    T=J'*W;
    
    %将各参数存到各自的list中
    Theta_d_list(:,idex)=Theta_d;   
    Theta_list(:,idex)=rad2deg(Theta);  %以角度制存储关节角度
    X_list(:,idex)=X;
    J_det_list(idex)=det(J);
    T_list(:,idex)=T;
    
    %更新关节角
    Theta=Theta+Theta_d*dt;
    %list的下标idex加一
    idex=idex+1;
    
end

%绘图
t=0:dt:duration;

figure
plot(t,Theta_d_list)
title('3个主动关节速率与时间的关系')
grid on
xlabel('t/s')
ylabel('$\dot{\Theta }/(rad\cdot s^{-1} )$','Interpreter','latex')
legend('$\dot{\theta }_1$','$\dot{\theta }_2$','$\dot{\theta }_3$'...
    ,'Interpreter','latex')

figure
plot(t,Theta_list)
title('3个主动关节角与时间的关系')
grid on
xlabel('t/s')
ylabel('$\Theta/^{\circ}$','Interpreter','latex')
legend('$\theta_1$','$\theta_2$','$\theta_3$', ...
    'Interpreter','latex')

figure
plot(t,X_list)
title('3个笛卡尔分量与时间的关系')
grid on
xlabel('t/s')
ylabel('笛卡尔分量')
legend('$x/m$','$y/m$','$\phi/rad $', ...
    'Interpreter','latex')

figure
plot(t,J_det_list)
title('雅可比矩阵行列式与时间的关系')
grid on
xlabel('t/s')
ylabel('$\left | J \right | $','Interpreter','latex')

figure
plot(t,T_list)
title('3个活动关节力矩与时间的关系')
grid on
xlabel('t/s')
ylabel('$T/(N\cdot m)$','Interpreter','latex')
legend('$\tau_1$','$\tau_2$','$\tau_3$', ...
    'Interpreter','latex')

% 机器人工具箱的动画仿真
figure
bot.plot(deg2rad(Theta_list)','view','top','movie','rob.mp4')


% 2)
clear;clc;
%机器人工具箱
L(1)=Link('alpha',0,'a',0,'d',0,'modified');
L(2)=Link('alpha',0,'a',4,'d',0,'modified');
L(3)=Link('alpha',0,'a',3,'d',0,'modified');
bot=SerialLink(L,'name','robot','tool',[2,0,0]);
%初始关节角
Theta_0s=deg2rad([10;20;30]);
%5秒仿真中最后的关节角，由上题结果中Theta_list的最后一项复制而来
Theta_5s=deg2rad([11.8810745317428;18.0197158094528;-27.1965698542779]);
%两组关节角的雅可比矩阵
j0_0s=bot.jacob0(Theta_0s');
j0_0s([1,2,6],:)
j0_5s=bot.jacob0(Theta_5s');
j0_5s([1,2,6],:)