Fixing and adding support for non straight joint axis in the URDF parser

[hubble14567]

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:

<joint name="Arm_Roll3" type="continuous">
  <origin xyz="0.280898 0.0 -0.338668" rpy="0 0 0"/>
  <parent link="Arm_PitchLink2_2"/>
  <child link="Arm_RollLink3_2"/>
  <axis xyz="0.638404 -0.0 -0.769701"/>
</joint>

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 rarer else, case of a crooked axis, is wrong:

# Check if axis of rotation/tanslation is not 1
if np.count_nonzero(joint.axis) < 2:
    ets = rtb.ET.SE3(SE3(trans) * SE3.RPY(rot))
else:
    # Normalise the joint axis to be along or about z axis
    # Convert rest to static ETS
    v = joint.axis
    u, n = unitvec_norm(v)
    R = angvec2r(n, u)

    R_total = SE3.RPY(joint.rpy) * R
    rpy = tr2rpy(R_total)

    ets = rtb.ET.SE3(SE3(trans) * SE3.RPY(rpy))

    joint.axis = [0, 0, 1]

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 the joint.axis, by the length of joint.axis. This does not result in Rz being aligned with joint.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:

            ets = translation * rotation * axis * [Rx] * axis.inv()

translation and rotation 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 example rtb.ET.Rx()). Notice how axis.inv() is used. This results in [Rx] having an orientation (defined by axis), 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 the else 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 joint N, and the parent N-1, with this, here is the new formula for the ets:

            ets(N) =  axis(N-1).inv() * translation(N) * rotation(N) * axis(N) * [Rx](N)

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:

======================================== warnings summary ========================================
tests/test_CustomXacro.py:12
  Warning: The distutils package is deprecated and slated for removal in Python 3.12. Use setuptools or check PEP 632 for potential alternatives

tests/test_BaseRobot.py::TestBaseRobot::test_teach
tests/test_DHRobot.py::TestDHRobot::test_plot_vellipse
  Warning: divide by zero encountered in divide

tests/test_DHRobot.py::TestDHRobot::test_SerialLink
  Warning: SerialLink is deprecated, use DHRobot instead

tests/test_ELink.py::TestLink::test_collided
  Warning: base kwarg is deprecated, use pose instead

tests/test_ERobot.py::TestERobot::test_collided
tests/test_Robot.py::TestRobot::test_collided2
  Warning: method collided is deprecated, use iscollided instead

tests/test_blocks.py: 58 warnings
tests/test_mobile.py: 47 warnings
tests/test_mobile_planner.py: 118 warnings
  Warning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)

========================= 563 passed, 8 skipped, 230 warnings in 18.76s ==========================

[hubble14567]

By the way, I do not have any test to verify that it is now working with urdf like:

<joint name="Arm_Roll3" type="continuous">
  <origin xyz="0.280898 0.0 -0.338668" rpy="0 0 0"/>
  <parent link="Arm_PitchLink2_2"/>
  <child link="Arm_RollLink3_2"/>
  <axis xyz="0.638404 -0.0 -0.769701"/>
</joint>

Those URDF are not covered by pytest.

It does work on three different versions of my robot (4 legs 3DoF, 4 legs 7DoF, 5 legs 7 DoF) made with Fusion. The IK and FK work perfectly now.

An easy test would be to use <axis xyz="0 1.5 0"/> this will result in the long rotation formula being used to rotate a Rx() joint by 90deg along z (equivalent to a Ry() joint).