Add support for non-Z-aligned links in FixedOffsetMultiplier

[GrmanRodriguez]

This PR expands the functionality of the FixedOffsetMultiplier by removing the restriction that the links must be Z-aligned.

This is done by expanding the previous method of calculation into the 3 dimension vectorial space:

We define ô as the unitary vector pointing to the positive direction of change. For cylinders this vector points to the positive Z axis of the link, while on box geometries it depends on the axis of elongation.

We define the start of the link as the point of the link furthest away in the negative ô direction. The end of the link is the point furthest away in the positive ô direction.

The offsets then become the vector between the parent joint and the start (PS) and the vector between the child joint and the end (CE).

To calculate PS we use the following formula:

*PS = PO - Vl/2 ô**

Similarly, to find the link origin PO given an offset we can rewrite the equation to *PO = PS + Vl/2 ô**

To calculate CE we use the following:

PC + CE = PO + Vl/2 * ô ---> *CE = PO + Vl/2 ô - PC**

To find the origin of the child joint given an offset we rewrite it to *PC = PO + Vl/2 ô - CE**

To find ô, PO and PC, the urdfpy library gives us the transformation matrix between from P to O and from P to C:

O = [[R_o, t_o], * P
     [1  , 0  ]]

C = [[R_c, t_c], * P
     [1  , 0  ]]

From where it's easy to find that PO = t_o, PC = t_c and *ô = R_o [0,0,1]** (for elongation in Z).

Results:

Here is an example of the result:

modifier = FixedOffsetModifier.from_name('non_aligned_link', self.modified_robot)

modification = Modification()
modification.add_dimension(3, absolute=True)

modifier.modify(modification)

We also allow the option to only move in some axes and not others, by using mask:

modifier = FixedOffsetModifier.from_name('non_aligned_link', self.modified_robot)

modification = Modification()
modification.add_dimension(3, offset_mask=[0,0,1], absolute=True) # only move in Z direction

modifier.modify(modification)

[GrmanRodriguez]

cc @CarlottaSartore