六自由度机械臂雅可比矩阵与力矩计算（matlab代码）

clear;clc;close all;

format compact

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

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

L(3)=RevoluteMDH('d',-0.102,'a',-0.300,'alpha',0);

L(4)=RevoluteMDH('d',0.090,'a',-0.276,'alpha',0,'offset',-pi/2);

L(5)=RevoluteMDH('d',0.090,'a',0,'alpha',pi/2);

L(6)=RevoluteMDH('d',0.082,'a',0,'alpha',-pi/2);

load('Mass.mat');load('CenterOfMass.mat');load('IcData.mat');

% 连杆质量

L(1).m = m1; L(2).m = m2 ; L(3).m = m3;

L(4).m = m4; L(5).m = m5; L(6).m = m6;

% 电机转动惯量

L(1). Jm=0;L(2).Jm=0;L(3).Jm=0;

L(4). Jm=0;L(5).Jm=0;L(6).Jm=0;

% 连杆质心位置

L(1).r = CenterOfMass1; L(2).r = CenterOfMass2; L(3).r = CenterOfMass3;

L(4).r = CenterOfMass4;L(5).r = CenterOfMass5; L(6).r = CenterOfMass6;

% 连杆惯性张量

L(1).I =Ic1;

L(2).I =Ic2;

L(3).I =Ic3;

L(4).I =Ic4;

L(5).I =Ic5;

L(6).I =Ic6;

robot = SerialLink(L);

robot.name = '6dofrobot';

robot.comment = 'robot';

robot.display()

robot =

6dofrobot:: 6 axis, RRRRRR, modDH, fastRNE                      

- robot;                                                        

+---+-----------+-----------+-----------+-----------+-----------+

| j |     theta |         d |         a |     alpha |    offset |

+---+-----------+-----------+-----------+-----------+-----------+

|  1|         q1|     0.1215|          0|          0|          0|

|  2|         q2|     0.1225|          0|     1.5708|    -1.5708|

|  3|         q3|     -0.102|       -0.3|          0|          0|

|  4|         q4|       0.09|     -0.276|          0|    -1.5708|

|  5|         q5|       0.09|          0|     1.5708|          0|

|  6|         q6|      0.082|          0|    -1.5708|          0|

+---+-----------+-----------+-----------+-----------+-----------+

% Forward Pose Kinematics

% Q=[0,60,60,-30,90,90]*pi/180;

% Qd=[pi/2,pi/2,pi/6,0,pi/2,pi/4];

% Qdd=[0,pi/34,0,pi/5,0,pi/58];

Q=[30,30,30,30,30,30]*pi/180*1; %zeros(1,6);

Qd=zeros(1,6);

Qdd=zeros(1,6);

%% 求各项系数矩阵和力矩

grav=[0,0,9.81];

figure1 = figure('Color',[1,1,1],'Position',[485,180,684,330]);

W=[-600,+800,-800,+800,-100,+1000]*0.001;%限制坐标轴的范围

robot.model3d = 'ROCR6/MDH_stl';

robot.plot3d(Q,'workspace',W,'view',[66.28 31.55],'trail',{'r','LineWidth',3.5}) %显示三维动画

Animate: saving video --> SolverForceByJacobian2022-04-05-16-13.gif with profile 'GIF'

Loading STL models from ARTE Robotics Toolbox for Education  by Arturo Gil (http://arvc.umh.es/arte).......

图片stl文件下载链接

FK=robot.fkine(Q);    %末端执行器位姿

R=t2r(FK);

Jac=robot.jacobe(Q)   %jacob0                      %末端坐标系下的几何雅可比

Jac = 6×6

   0.2982    0.1399    0.1573    0.0470   -0.0710         0

   0.0374   -0.5874   -0.2975   -0.0745    0.0410         0

   0.4148   -0.2395   -0.1645   -0.0450         0         0

  -0.7500    0.4330    0.4330    0.4330   -0.5000         0

   0.4330   -0.2500   -0.2500   -0.2500   -0.8660         0

   0.5000    0.8660    0.8660    0.8660    0.0000    1.0000

Jac0=robot.jacob0(Q)                           %世界坐标系下的几何雅可比          

Jac0 = 6×6

   0.3967   -0.3800   -0.1550   -0.0355   -0.0205         0

  -0.3241   -0.2194   -0.0895   -0.0205    0.0355         0

   0.0000   -0.4790   -0.3290   -0.0900    0.0710         0

        0    0.5000    0.5000    0.5000   -0.8660    0.4330

        0   -0.8660   -0.8660   -0.8660   -0.5000   -0.7500

   1.0000    0.0000    0.0000    0.0000    0.0000    0.5000

Jac2=inv([R zeros(3,3); zeros(3,3) R])*Jac0  %末端坐标系下的几何雅可比

Jac2 = 6×6

   0.2982    0.1399    0.1573    0.0470   -0.0710         0

   0.0374   -0.5874   -0.2975   -0.0745    0.0410         0

   0.4148   -0.2395   -0.1645   -0.0450         0         0

  -0.7500    0.4330    0.4330    0.4330   -0.5000   -0.0000

   0.4330   -0.2500   -0.2500   -0.2500   -0.8660    0.0000

   0.5000    0.8660    0.8660    0.8660         0    1.0000

FEXT=[0 0 0 0 5.5 5.5]; %[Fx Fy Fz Mx My Mz],specify wrench acting on the end-effector

Tau1 =robot.rne(Q,Qd,Qdd,'gravity',grav)   %没有外力时，计算的关节力矩

Tau1 = 1×6

   0.0000  -21.9283  -10.6568   -1.3727    0.4142   -0.0001

Tau2 =robot.rne(Q,Qd,Qdd,'gravity',grav,'fext',FEXT)  %有外力时，计算的关节力矩

Tau2 = 1×6

   5.1316  -18.5401   -7.2686    2.0155   -4.3490    5.4999

Tau3=(Jac'*FEXT')'  %外力引起的关节力矩

Tau3 = 1×6

   5.1316    3.3881    3.3881    3.3881   -4.7631    5.5000

Ftau=Tau2-Tau1   %外力引起的关节力矩

Ftau = 1×6

   5.1316    3.3881    3.3881    3.3881   -4.7631    5.5000

G=robot.gravload(Q,grav)  %Gravity Term

G = 1×6

   0.0000  -21.9283  -10.6568   -1.3727    0.4142   -0.0001

M=robot.inertia(Q)        %Inertia Matrix

M = 6×6

   0.9301   -0.2293   -0.0533   -0.0046   -0.0117    0.0002

  -0.2293    1.6609    0.6864    0.0798   -0.0202    0.0003

  -0.0533    0.6864    0.3753    0.0524   -0.0139    0.0003

  -0.0046    0.0798    0.0524    0.0156   -0.0038    0.0003

  -0.0117   -0.0202   -0.0139   -0.0038    0.0052   -0.0000

   0.0002    0.0003    0.0003    0.0003   -0.0000    0.0004

C=robot.coriolis(Q, Qd)  %Coriolis Matrix

C = 6×6

    0     0     0     0     0     0

    0     0     0     0     0     0

    0     0     0     0     0     0

    0     0     0     0     0     0

    0     0     0     0     0     0

    0     0     0     0     0     0

%% 打印求解结果

% disp('T=');digits(7);vpa(FK,6);disp(FK)

% disp('M=');M=double(M);disp(M)

% disp('C=');C=double(C);disp(C)

% disp('Tau1=');Tau=double(Tau1);disp(Tau1)

% disp('G=');G=double(G);disp(G)

需要可以联系扣扣2386317960