Incorrect dynamic torques.

Muhammad Aleem

2019 10 17

0 个回答

28 次查看（30 天）

0 个投票

Running the following code outputs incorrect torques. The static torques come out to be correct but the dynamic torques are incorrect. This is because link 6 has no length and mass but it still has some torque. Have I inputted incorrect values of center of mass or moment of inertia? center of mass and moment of inertia were copied from the solidworks file. Solidworks axes are different but we switched the values accordingly. However, the output is still wrong. Anybody help?

robot=robotics.RigidBodyTree;
body1=robotics.RigidBody('body1');
body1.Mass=0;
body1.CenterOfMass=[0 0 0];
jnt1=robotics.Joint('jnt1','revolute');
tform=trvec2tform([0 0 0]);
setFixedTransform(jnt1,tform);
body1.Joint=jnt1;
addBody(robot,body1,'base')
body2=robotics.RigidBody('b2');
body2.Mass=0.085;
body2.CenterOfMass=[0 0 0.03];
body2.Inertia=[1180 655.2 1129.6 0 0 0]/100000000;
jnt2=robotics.Joint('jnt2','revolute');
jnt2.JointAxis=[0 1 0];
tform=trvec2tform([0 0 0.06]);
setFixedTransform(jnt2,tform);
body2.Joint=jnt2;
body3=robotics.RigidBody('b3');
body3.Mass=0.0829;
body3.CenterOfMass=[0.0603 0.0023 -0.0099];
body3.Inertia=[626.6 320.8 396 0 0 0]/100000000;
jnt3=robotics.Joint('jnt3','revolute');
jnt3.JointAxis=[0 1 0];
tform=trvec2tform([0.07 0 0]);
setFixedTransform(jnt3,tform);
body3.Joint=jnt3;
body4=robotics.RigidBody('b4');
body4.Mass=0.066;
body4.CenterOfMass=[0.0633 -0.0063 -0.0025];
body4.Inertia=[678.3 678.3 734 0 0 0]/100000000;
jnt4=robotics.Joint('jnt4','revolute');
jnt4.JointAxis=[1 0 0];
tform=trvec2tform([0.1 0 0]);
setFixedTransform(jnt4,tform);
body4.Joint=jnt4;
body5=robotics.RigidBody('b5');
body5.Mass=0.02766;
body5.CenterOfMass=[0.04/2 0 0];
body5.Inertia=[412.5 178.1 309.8 0 0 0]/100000000;
jnt5=robotics.Joint('jnt5','revolute');
jnt5.JointAxis=[0 1 0];
tform=trvec2tform([0.04 0 0]);
setFixedTransform(jnt5,tform);
body5.Joint=jnt5;
body6=robotics.RigidBody('b6');
body6.Mass=0.0620;
body6.CenterOfMass=[0.11/2 0 0];
body6.Inertia=[1539 263.2 1509 0 0 0]/100000000;
jnt6=robotics.Joint('jnt6','revolute');
jnt6.JointAxis=[1 0 0];
tform=trvec2tform([0.11 0 0]);
setFixedTransform(jnt6,tform);
body6.Joint=jnt6;
addBody(robot,body2,'body1')
addBody(robot,body3,'b2')
addBody(robot,body4,'b3')
addBody(robot,body5,'b4')
addBody(robot,body6,'b5')
robot.DataFormat='row';
robot.Gravity=[0 0 -9.81];
config=homeConfiguration(robot);
% fext = externalForce(robot,'b',wrench);
inverseDynamics(robot,config,[0.1 0.1 0.1 0.1 0.1 0.1],[0.05 0.05 0.05 0.05 0.05 0.05])