关于用Robotics System Toolbox 和Robotics Toolbox 解逆动力学遇到的问题
今天做大作业遇到一个bug有点头秃，先记下来以后再想（挖坑待填orz
题目如下：
L 1 = 4 m , L 2 = 3 m , L 3 = 2 m L_1 = 4m,\ L_2 = 3m,\ L_3 = 2m L1=4m, L2=3m, L3=2m
各连杆质量：
m 1 = 20 k g , m 2 = 15 k g , m 3 = 10 k g m_1 = 20kg,\ m_2 = 15kg,\ m_3 = 10kg m1=20kg, m2=15kg, m3=10kg
质心在其几何中心处：
I z z 1 = 0.5 k g ⋅ m 2 , I z z 2 = 0.2 k g ⋅ m 2 , I z z 3 = 0.1 k g ⋅ m 2 I_{zz1} = 0.5 kg\cdot m^2,\ I_{zz2} = 0.2 kg\cdot m^2,\ I_{zz3} = 0.1 kg\cdot m^2 Izz1=0.5kg⋅m2, Izz2=0.2kg⋅m2, Izz3=0.1kg⋅m2
使用Robotics Toolbox建立上述机器人模型，代码如下：
L = Link.empty();
L(3) = Link('revolute', 'd', 0, 'a', 2, 'alpha', 0, 'm', 10, 'I', [0, 0, 0; 0, 0, 0; 0, 0, 0.1], 'r', [-1; 0; 0]);
L(2) = Link('revolute', 'd', 0, 'a', 3, 'alpha', 0, 'm', 15, 'I', [0, 0, 0; 0, 0, 0; 0, 0, 0.2], 'r', [-1.5; 0; 0]);
L(1) = Link('revolute', 'd', 0, 'a', 4, 'alpha', 0, 'm', 20, 'I', [0, 0, 0; 0, 0, 0; 0, 0, 0.5], 'r', [-2; 0; 0]);
trpBot = SerialLink(L, 'name', 'my 3r-plane bot', 'gravity', [0 -9.81 0]);
使用Robotics System Toolbox 建立上述机器人模型，代码如下：
dhparam = [ 2, 0, 0, 0;
3, 0, 0, 0;
4, 0, 0, 0];
robot = rigidBodyTree;
body1 = rigidBody('body1');
body2 = rigidBody('body2');
body3 = rigidBody('body3');
jnt1 = rigidBodyJoint('jnt1','revolute');
jnt2 = rigidBodyJoint('jnt2','revolute');
jnt3 = rigidBodyJoint('jnt3','revolute');
setFixedTransform(jnt3,dhparam(1,:),'dh');
setFixedTransform(jnt2,dhparam(2,:),'dh');
setFixedTransform(jnt1,dhparam(3,:),'dh');
body1.Joint = jnt1;
body2.Joint = jnt2;
body3.Joint = jnt3;
addBody(robot,body1,'base')
addBody(robot,body2,'body1')
addBody(robot,body3,'body2')
robot.Gravity = [0 -9.81 0];
然后任选一组姿态：
q = rand([1 3]) * pi;
dq = rand([1 3]);
ddq = rand([1 3]);
trt = trpBot.rne(q, dq, ddq, 'gravity', [0 -9.81 0]);
robot.DataFormat = 'row';
trst = robot.inverseDynamics(q, dq, ddq);
disp(trt);
disp(trst);
结果差了不止一个数量级：
估计是什么地方没有设置好orz
现在能确定的是运动学方面是没问题的：
以后解决了再填坑。。。