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.
