Robotics toolbox - strange results when using inverse dynamics and robot created with DH

Good afternoon,

I am trying to use the inverse dynamics function of the robotics toolbox, but I obtain strange results.

Let's consider the Matlab example to build a puma 560 robot using the DH parameters.

dhparams = [0   	pi/2	0   	0;
            0.4318	0       0       0
            0.0203	-pi/2	0.15005	0;
            0   	pi/2	0.4318	0;
            0       -pi/2	0   	0;
            0       0       0       0];
robot = rigidBodyTree;
body1 = rigidBody('body1');
jnt1 = rigidBodyJoint('jnt1','revolute');

setFixedTransform(jnt1,dhparams(1,:),'dh');
body1.Joint = jnt1;

addBody(robot,body1,'base')
body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
body3 = rigidBody('body3');
jnt3 = rigidBodyJoint('jnt3','revolute');
body4 = rigidBody('body4');
jnt4 = rigidBodyJoint('jnt4','revolute');
body5 = rigidBody('body5');
jnt5 = rigidBodyJoint('jnt5','revolute');
body6 = rigidBody('body6');
jnt6 = rigidBodyJoint('jnt6','revolute');

setFixedTransform(jnt2,dhparams(2,:),'dh');
setFixedTransform(jnt3,dhparams(3,:),'dh');
setFixedTransform(jnt4,dhparams(4,:),'dh');
setFixedTransform(jnt5,dhparams(5,:),'dh');
setFixedTransform(jnt6,dhparams(6,:),'dh');

body2.Joint = jnt2;
body3.Joint = jnt3;
body4.Joint = jnt4;
body5.Joint = jnt5;
body6.Joint = jnt6;

addBody(robot,body2,'body1')
addBody(robot,body3,'body2')
addBody(robot,body4,'body3')
addBody(robot,body5,'body4')
addBody(robot,body6,'body5')

Now, I want to compute the torques at joint level when an external wrench is applied at the body 6 (static).

robot.DataFormat = 'column';

fext = externalForce(robot,'body6',[0;0;0;0;-100;0],[0;0;0;0;0;0]);
tau = inverseDynamics(robot,[0;0;0;0;0;0],[],[],fext);

The external wrench shoud generate a torque at joint 1 different from zero. Instead, I obtain

tau =

   1.0e-14 *

    0.2768
    0.0124
         0
         0
         0
         0

If I try to change the orientation of the external force, in order to have an important torque contribution at joint 2, I obtain

fext = externalForce(robot,'body6',[0;0;0;0;0;-100],[0;0;0;0;0;0]);
tau=inverseDynamics(robot,[0;0;0;0;0;0],[],[],fext);

tau =

   45.2100
    2.0300
         0
         0
         0
         0

It is like the first component of tau is the joint 2 and the torque at joint 1 is not computed. When I use the DH parameters to build the rigid body tree, should I change something before using the dynamics functions?

Thank you

Answer 1

[Yiping Liu]

Hi, Francesco,

I think this is related to an issue that is specific to robot specified through DH parameters. This issue has already been fixed in the upcoming new release. For now, to work around this issue, you can try to use 'mdh', or the transformation matrix when setFixedTransform. OR, you can reach out to MathWorks tech support directly and we can provide you with a hot patch.

-------

Below illustrates the correct behavior of externalForce and inverseDynamics for robots specified through DH.

In the doc page, it is mentioned that fext = externalForce(robot,bodyname,wrench,configuration) (i.e. when robot configuration is specified as an input) composes the external force matrix assuming that wrench is in the bodyname frame for the specified configuration.

As a result, when your external wrench is specified as [0;0;0;0;-100;0] , the external force on the robot will look like something in the picture below (along the -y direction of body6). Since you haven't specified a gravity, the only torque that is needed to counteract this external force is a large poisitve value on joint1.

Here is a detailed look of the body6 frame

>> fext = externalForce(robot,'body6',[0;0;0;0;-100;0],[0;0;0;0;0;0]);

>> tau = inverseDynamics(robot,[0;0;0;0;0;0],[],[],fext)

tau =

45.2100

0.0000

0.0000

0

0

0

>>

If your external wrench is specified as [0;0;0;0;0;-100] , the external force on the robot will look like something in the picture below. Still no gravity, but this time, you don't need torque on joint1 (as the direction of the external force is in joint1's constraint manifold), you only need a positive joint2 torque to counteract this external force (plus a small joint3 torque, as there is a small offset in the puma robot between joint2 and joint2).

>> robot.DataFormat = 'column';

>> fext = externalForce(robot,'body6',[0;0;0;0;0;-100],[0;0;0;0;0;0]);

>> tau=inverseDynamics(robot,[0;0;0;0;0;0],[],[],fext)

tau =

0

45.2100

2.0300

0.0000

0.0000

0.0000