七自由度机械臂重力补偿MATLAB仿真（Gravity Compensation）

工具：robotics toolbox -- peter corke / Simulink

思路：1. 创建一个 position controller, 让 end effector 去到指定点，测出在该点时对应的 joint angles, torque；

           2. 使用 toolbox 里的 gravload(q) 求出对应该点的 gravity torque；

           3. 比较两个 torque 是否一致。

---

• Create robot using SerialLink and set dynamics parameters

startup_rvc

%%            theta d a alpha
L(1) = Link([  0  0 0  pi/2]);
L(1).m = 0.00;
L(1).r = [0 0 0];
L(1).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(1).G = 1;
L(1).Jm = 0.0;

L(2) = Link([  0  0 0.30  0]);
L(2).m = 0.9507;
L(2).r = [-0.32213 -0.01724 -0.05311 ];
L(2).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(2).G = 1;
L(2).Jm = 0.0;

L(3) = Link([  0  0 0.35  -pi/2]);
L(3).m = 0.4138;
L(3).r = [-0.2076 0.0000 0.0000];
L(3).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(3).G = 1;
L(3).Jm = 0.0;

L(4) = Link([  0  0.1347 0  pi/2]);
L(4).m = 0.1540;
L(4).r = [ 0 -0.08887 0.06342 ];
L(4).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(4).G = 1;
L(4).Jm = 0.0;

L(5) = Link([  0  0 0  -pi/2]);
L(5).m = 0.1051;
L(5).r = [0.0000 -0.05926 -0.05544 ];
L(5).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(5).G = 1;
L(5).Jm = 0.0;

L(6) = Link([  0  0 0 pi/2]);
L(6).m = 0.0738;
L(6).r = [0.0000 -0.02402 0.03396];
L(6).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(6).G = 1;
L(6).Jm = 0.0;

L(7) = Link([  0  0 0  0]);
L(7).m = 0.00;
L(7).r = [0 0 0];
L(7).I = [0.1, 0.1, 0.1, 0, 0, 0];
L(7).G = 1;
L(7).Jm = 0.0;


syms q1 q2 q3 q4 q5 q6 q7;
rob = SerialLink(L,'name','MasterHand');
rob.offset = [0 -pi/2 pi/2 0 0 -pi/2 pi/2];
rob.qlim = [-1/3*pi 1/3*pi;
            -1/3*pi 25/180*pi;
            -1/18*pi 75/180*pi;
            -245/180*pi 65/180*pi;
            -186/180*pi 98/180*pi;
            -41/180*pi 41/180*pi;
            -250/180*pi 250/180*pi];
% rob.plot([0 0 0 0 0 0 0],'jointdiam',1,'base','wrist','arrow','workspace',[-1 1 -1 1 -1 1]);
% rob.gravity = [0 0 9.81];

• Create Position Controller in Simulink

• Compare Result