inverseDynamics function in Robotics System Toolbox gives wrong results for joint torque calculations for external force applied on the end effector of the robot
21 views (last 30 days)
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:
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.
Thanks in advance.
Here is the code:
% 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');
%Add body2 to rigid body tree
body2 = rigidBody('body2');
body2.Joint = rigidBodyJoint('joint2','revolute');
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
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
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)
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.