inverseDynamics function in Robotics System Toolbox gives wrong results for joint torque calculations for external force applied on the end effector of the robot

Dears,

Could you please check if there is a problem with the joint torque calculations with externalForce and inverseDynamics functions in Robotics System Toolbox?

I am trying to calculate joint torques when the external force is present. As you can see from attachment, I modeled a very simple two link manipulator and force applied

Fx = [0.5; 0.5];

to the end effector (body2). If I calculate with multiplying this external force by the transpose of the geometric Jacobian for

q = [pi/4; 3*pi/8];

the result is as expected:

Fq =
   -0.6533
   -0.6533

However, if I use the externalForce function to create external force matrix and then use inverseDynamics functions, the result seems wrong - the required torque for the first joint is zero, however it should not be.

Fq2 =
    0.0000
   -0.6533

Thanks in advance.

Best regards,

Hakan

Here is the code:

% https://studywolf.wordpress.com/2013/09/02/robot-control-jacobians-velocity-and-force/
% reference documentation
 
twolink = rigidBodyTree('MaxNumBodies', 2, 'DataFormat', 'column');
dhparams = [1.0 0.0 0.0 0.0;
            1.0 0.0 0.0 0.0];
 
%Add body1 to rigid body tree
body1 = rigidBody('body1');
body1.Joint = rigidBodyJoint('joint1','revolute');
body1.Joint.setFixedTransform(dhparams(1,:),'dh');
twolink.addBody(body1,twolink.BaseName);
 
%Add body2 to rigid body tree
body2 = rigidBody('body2');
body2.Joint = rigidBodyJoint('joint2','revolute');
body2.Joint.setFixedTransform(dhparams(2,:),'dh');
twolink.addBody(body2, 'body1');

Q = [pi/4; 3*pi/8]; %robot configuration

JAC = geometricJacobian(twolink, Q,'body2'); %calculate Jacobian

Fx = [zeros(3,1);0.5;0.5;0]; % external forces
 
Fq = JAC'*Fx % manual joint torques calculation
 
Fext = externalForce(twolink,'body2',Fx); % Fext creation with externalForce built-in function
 
Fq2 = inverseDynamics(twolink, Q, [], [], Fext) % joint torques calculation with inverseDynamics built-in function

Answer 1

[Yiping Liu]

Hi, Hakan,

Thanks for raising the questions about the external forces in rigidBodyTree inverse dynamics.

The quick answer is that this is not wrong result, but there is a misinterpretation of the externalForce method.

Before I going into the details, I want to point you to this post in which someone raised a similar question as you did.

Also, if you use a version of MATLAB before 21a, you may run into another issue related to computing dynamics quantities for robots constructed with DH parameters. To get around that issue, you can try to represent your robot with MDH parameters (or homogeneous transformation matrices).

----

So based on the code your posted, I assume you are trying to do something like below (where the red arrow showing how you want your external forces applied).

So the issue here is,

Fx = [zeros(3,1);0.5;0.5;0];
Fext = externalForce(twolink,'body2',Fx);

The above two lines of code does not let you apply the external force in the way you like.

Based on the doc, the externalForce API without specifying the configuration as the last input argument, requires you to provide the external force exerted on body i, but expressed in the base coordinate. Pay attention to what "express" means here, it's not just a change of coordinates for the force vectors, but it implies a spatial transform. In other words, if you want an external force like shown in the figure above applied to the system, you need to provide Fx to externalForce as the wrench vector that, if applied at the base of the robot, will generate the same effect as the force vector drawn in the figure.

The correct Fx value should be

         0
         0
   -0.6533
    0.5000
    0.5000
         0

Note there is an additional z-torque. Then do ID with the Fx

 >> Fx = [0, 0, -0.6533, 0.5000, 0.5000, 0];
 >> Fext = externalForce(twolink,'body2',fx);
 >> Fq2 = inverseDynamics(twolink, Q, [], [], Fext)

 Fq22x =

    0.6533
    0.6533
 

NOTE that both joint torques should be positive, as only positive torques can counter-act the effect of the external force.

You need to run the code in 21a or later to get the same result.

Currently, the documentation page for externalForce function does not provide enough details, and its interface (the one that does not take configuration as the last input argument) is not very intuitive as well. We are working to improve them.

Let us know if this answers your question.

[Hakan Ertugrul]

Dear Yiping,

Thanks for your detailed explanation. I pay attention to "expressed" word. If I can provide the configuration, then there will be no problem, right (like below)?

fext = externalForce(robot,bodyname,wrench,configuration)

And also, I am sorry but could not understand how did you find the correct Fx values directly. Any explanation would really appreciated.

Best regards,

Hakan

[Yiping Liu]

You can provide the fourth input argument, but then you also need to transform the wrench to the local body frame so that the orientation matches that in the global frame.

For your second question, a good reference might be the Springer Handbook of Robotics, the Dynamics Chapter (both versions are good), esp. pay attention to the explanation of the external force in the Recursive Newton Euler Algorithm.