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

4 views (last 30 days)
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

Answers (1)

Yiping Liu
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.
  2 Comments
Hakan Ertugrul
Hakan Ertugrul on 13 May 2021
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)?
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
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.

Sign in to comment.

Products


Release

R2020b

Community Treasure Hunt

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

Start Hunting!