13 views (last 30 days)

Show older comments

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

Yiping Liu
on 14 Apr 2021

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.

Yiping Liu
on 14 May 2021

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.

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!