fixedOffsetModifiers for non-z-aligned links/joints

[AlexAntn]

We currently have the option to do changes to links/joints while keeping the offsets, by using the fixedOffsetModifier class.

Unfortunately this only works for z-aligned links/joints, where the computation of the offset is quite linear.

It could be useful for the users to have a fixedOffsetModifiers for links/joints that are not z-aligned. The problem arises from defining which offsets should be changed or not, in which direction the link should change, etc.

This issue is meant to open the discussion on how to tackle this issue.

DoD: we have a clear idea on how to proceed with fixed offset calculation for non-z-aligned joints/links

[traversaro]

I think we can separate two cases:

• Cases in which the link has a single child link. In this case, a possible (heuristic) way in which to determine in which direction URDF structure is "directed" is to compute the offset between the parent joint origin and the child joint origin, and check on which direction the vector is oriented, and choose that direction as the modification direction.
• Cases in which the link has no child. In this case, the only meaningful way to determine in which the link should be modified is to take a parameter from the user. I am not a big fan of user-specified parameters, but in this case I think that this is inevitable (as there is no other way of identifying the direction).

To recap I think we can have:

• An heuristic
• The possibility to ovverride this heuristic that is compulsory in the case in the heuristic is failing

[AlexAntn]

After the meeting we have agreed on the following (fixedOffsetModifier):

• Default way of transforming the links and joints (z-axis);
• We have a heuristic for links that have children joints - we compute the main direction of transformation (based on offsets);
• The user can instead specify an axis for the transformations, which overrides the heuristic.

To implement this we have to drop the z-aligned check we perform in the following lines.

[GrmanRodriguez]

During implementation of the feature we found it to be better to keep all offsets in the 3 directions by default. To do this we expand the calculations we were using to use vectors and matrices, for example:

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**

The remaining question is, how to find ô, PO and PC?

Luckily, 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:

This has been implemented in this branch, 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)

From my understanding this is different to what was agreed in the previous meeting. However, @AlexAntn and I think this is a more predictable behavior than trying to estimate an axis for the user.

What do you think @traversaro @CarlottaSartore ?

[traversaro]

From my understanding this is different to what was agreed in the previous meeting. However, @AlexAntn and I think this is a more predictable behavior than trying to estimate an axis for the user.

Supercool! Yes, I agree that it is more predictable, just a few questions:

• In which orientation frames are all those vectors expressed?
• In which frame the the offset_mask is expressed?

[GrmanRodriguez]

I would guess the answer to both is the global XYZ of gazebo

[traversaro]

I would guess the answer to both is the global XYZ of gazebo

I did not checked all the computations, but it seems that in the computations in https://github.com/icub-tech-iit/urdf-modifiers/commit/19162fcbd621a6d9de92279831e15fc1a7fd3fbf the get_joint_origin and get_link_origin methods are used, and those return the origin of those elements w.r.t. to the link frame, not to the global frame (see https://github.com/icub-tech-iit/urdf-modifiers/blob/19162fcbd621a6d9de92279831e15fc1a7fd3fbf/urdfModifiers/core/fixedOffsetModifier.py#L176 and https://github.com/icub-tech-iit/urdf-modifiers/blob/19162fcbd621a6d9de92279831e15fc1a7fd3fbf/urdfModifiers/core/fixedOffsetModifier.py#L169). To avoid this kind of errors, when dealing with 3D positions vectors and 4x4 homogeneous transform it is convenient to indicate explicitly the frames in which this quantities are expressed, for example writing ${}^A o_B$ for 3D vectors or ${}^A H_B$ for homogeneous transforms, as usually robotics textbooks do.

[GrmanRodriguez]

Ah wait I think I misunderstood the question

O, C, S and E are expressed with reference to P

However I think the unitary i, j and k vectors to express the vectors are the same as the world origin, so those ones define also the offset mask

[traversaro]

However I think the unitary i, j and k vectors to express the vectors are the same as the world origin, so those ones define also the offset mask

I am not sure about that, if you see the code it seems that vectors returned by get_link_origin (visual origin location, expressed in link frame) and get_direction_vector ( the unitary vector) are summed together with just a rotation for link_rotation_matrix (see https://github.com/icub-tech-iit/urdf-modifiers/blob/19162fcbd621a6d9de92279831e15fc1a7fd3fbf/urdfModifiers/core/fixedOffsetModifier.py#L201), that is the rotation matrix ${}^L R_V$, where we call $L$ the link frame and $V$ the visual frame. So it seems that the offset_mask is expressed w.r.t. to the visual frame, that I am not sure if it is intuitive for the user.

On the other hand, if np.dot(link_rotation_matrix, unit_vector) is a 3D vector, I am not sure what link_length / 2 * np.dot(link_rotation_matrix, unit_vector) (a scalar divided by 3D vector?) means, so I may be reading this not correctly.

[GrmanRodriguez]

So it seems that the offset_mask is expressed w.r.t. to the visual frame, that I am not sure if it is intuitive for the user.

I'll play with it a bit and let you know the answer for sure

I am not sure what link_length / 2 * np.dot(link_rotation_matrix, unit_vector) (a scalar divided by 3D vector?) means, so I may be reading this not correctly.

you can read it as (link_length / 2) * np.dot(link_rotation_matrix, unit_vector), so a scalar multiplied by a vector

[traversaro]

I am not sure what link_length / 2 * np.dot(link_rotation_matrix, unit_vector) (a scalar divided by 3D vector?) means, so I may be reading this not correctly.

you can read it as (link_length / 2) * np.dot(link_rotation_matrix, unit_vector), so a scalar multiplied by a vector

Ahh, that is much more clear!

[GrmanRodriguez]

I'll play with it a bit and let you know the answer for sure

After some tests I can assure that all the calculations and frames of reference are with respect to the link's parent joint.

Also the directions of the offset mask are relative to the parent joint's direction.

An example of this behavior is that the modifications and offset mask work the same even if the urdf is not aligned to gazebo's world frame:

I changed the function descriptions to be more explicit about the reference of the origins:

https://github.com/icub-tech-iit/urdf-modifiers/blob/c03e624f36c1b60a0a77625549200652fd0bd28b/urdfModifiers/core/fixedOffsetModifier.py#L168-L169

https://github.com/icub-tech-iit/urdf-modifiers/blob/c03e624f36c1b60a0a77625549200652fd0bd28b/urdfModifiers/core/fixedOffsetModifier.py#L175-L176

[traversaro]

With "with respect to the link's parent joint" you mean "with respect to the link's parent link frame."? If not, how do you define the joint frame?

[GrmanRodriguez]

Referring back to this comment, I mean that get_joint_origin returns CP and get_link_origin returns OP.

They are both relative to P, the link's parent joint.

[traversaro]

They are both relative to P, the link's parent joint.

P is the origin of the link frame. If the vector are also expressed in the orientation of the link frame, I think it is more clear to users to just write that they are expressed in the link frame, see for example the image:

from the official URDF docs (http://wiki.ros.org/urdf/XML/link).

[GrmanRodriguez]

Ah ok I understand. I changed the wording:

https://github.com/icub-tech-iit/urdf-modifiers/blob/635c51d0ad41716023f1d87cf673d53694983d60/urdfModifiers/core/fixedOffsetModifier.py#L168-L169

https://github.com/icub-tech-iit/urdf-modifiers/blob/635c51d0ad41716023f1d87cf673d53694983d60/urdfModifiers/core/fixedOffsetModifier.py#L175-L176

[traversaro]

Great, thanks!

[GrmanRodriguez]

PR here. Already merged.