Fixing and adding support for non straight joint axis in the URDF parser #441
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Problem Overview
I discovered that rtb does not work with URDF using NOT straight axis for their joint. The Fusion CAD software tends to define its URDF export using those axis that are not only made of 0, 1 and -1. This made it impossible to load a wide variety of URDF, even though RViz and other software support those crooked axis.
For example this joint will not result in the proper link being created:
Faulty code
This is the piece of code that is currently responsible for creating the ets for the link representing the joint (here in code). The
if
case does not have any issues and is the only case covered by the tests of the repo. The much rarerelse
, case of a crooked axis, is wrong:Problem 1
Just by looking at it we can see that the math is wrong (sadly the problem does not stop there). The geometric operation performed here is: Rotate a
Rz
joint, in the plane defined by thejoint.axis
, by the length ofjoint.axis
. This does not result inRz
being aligned withjoint.axis
.Problem 2
Second and much bigger problem (this is why this PR has 300 line change) is that the correct math for a link corresponding to a joint should be defined like so:
translation
androtation
are defined from<origin xyz="? ? ?" rpy="? ? ?"/>
,axis
is a rotation matrix from[Rx]
to the axis defined in the urdf:<axis xyz="? ? ?"/>
and[Rx]
is a variable joint/ets from robotics-toolbox (for examplertb.ET.Rx()
). Notice howaxis.inv()
is used. This results in[Rx]
having an orientation (defined byaxis
), but this orientation is not propagated onto the next link.The current code -- aside from the wrong rotation -- also does not multiply back by
axis.inv()
, and the axis orientation propagates. (this only propagates if theelse
case is used)Fix 1
First, I fixed the wrong rotation problem with a short function in this commit..
Fix 2
The big pain here is that, a link must end with a variable ets -- this is
[Rx]
. So, to have[Rx]
at the end, we must change the formula to a recursive one using the axis of the parent. Let's call the current jointN
, and the parentN-1
, with this, here is the new formula for the ets:If you chain those (
ets(N-1) * ets(N) * ets(N+1)
), you will get back to the first formula.This is implemented recursively in this (long) commit..
Getting the ets from the joint and applying the formula is done in this function.
The recursion is done directly inside the URDF object in this method.
I displaced almost all of the previous link creation code, only the children-parent relationship is required before the recursion where everything will be set properly (here).
Finally, after the proper ets is stored in the links, the other parameters of the link can be set. I initially I did the ets last to change as little code as possible, but tests did not pass, so I had to do this commit to finish defining the links inside the recursion.
PyTest
PyTest did pass successfully. Although there are warnings, but I don't know if their are related to my changes: