Proposal for new JointAndLinkModifier

[traversaro]

While experimenting with existing modifiers (see https://github.com/icub-tech-iit/urdf-modifiers/issues/10) I had problems to grasp the existing logic, and I also had problems due to the fact that a user needed to manually add two modifiers (a "Joint" and a "Link") if he wanted to modify the length of this link. For this reason, I developed a new JointAndLinkModifier (suggestion for a better name are welcome), that automatically modify the aspects of a joint related to a lenght of a link, and that in theory should require less manually derived parameters. However, note that at the moment this link modifiers only supports dimension/multiplier modifications as those are the one I needed. The code for this new class can be found in https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/LinkAndJointModifierV1.py, to experiment with it you can just execute the script, as it is self contained and it does not require any change to urdf-modifiers .

For more easily read this issue, make sure that you have an extension to automatically render LaTeX notation such as https://chrome.google.com/webstore/detail/mathjax-plugin-for-github/ioemnmodlmafdkllaclgeombjnmnbima .

To clarify, this new modifier class is explicitly designed to live side by side with existing modifiers and not to replace them if we do not want.

Background Notation

First of all, let's agree with the basic notation regarding a URDF link and the joint that it is connecting it to a child link. In particular, let's take for example a simple link with box visual, that it is connected with its child link with a joint (inertial and collision elements are removed for simplicity):

  <link name="link">
    <visual>
      <origin xyz="0 0 v_o" rpy="0 -0 0"/>
      <geometry>
        <box size="0.1 0.1 v_l"/>
      </geometry>
    </visual>
  </link>
  <joint name="joint" type="continuous">
    <origin xyz="0 0 j_o" rpy="0 -0 0" />
    <axis xyz="0 1 0" />
    <parent link="link" />
    <child link="child_link" />
  </joint>
  <link name="child_link" />

(diagram drawio source code: URDFDiagramExample.zip )

In this case, the main parameters that we can modify are:

• $j_0$ : The distance between the axis connecting the link to its parent and the axis connecting the link to its child, that we call "joint offset" or $j_0$
• $v_l$ : The "lenght" of the visual element (in this case box, but it could be also a cylinder) of the link, that we call "visual lenght"
• $v_o$ : The distance between the link frame and and the center of the visual element, that we call "visual offset"

These are the three main quantities that we want to scale when we change the length of a link. Two additional quantities that are dependent on this are:

• The distance between the link frame and the start of the link visual element, that we call "starting offset" of $s_o$
• The distance between the child link frame and the end of the link visual element, that we call "ending offset" of $e_o$

These quantities can be computed as :

$$ s_o = v_o - \frac{v_l}{2} sign(j_0) $$

$$ e_o = v_o + \frac{v_l}{2} sign(j_0)- j_o $$

for simplifying the notation, we define $\overline{v}_l := v_l sign(j_0)$.

In the rest of the issue, we imagine that we want to deform a link defined as given here by a scale $K$, for which $K > 1$.

Case 0: Naive deformation of all quantities

The first thing I tried is to just scale all the indipendent quantities, i.e. to compute the scaled $v_o'$, $v_l'$ and $j_o'$ as:

$$ j_o' = K j_0 $$

$$ v_l' = K v_l $$

$$ v_o' = K v_o $$

If I try this solution, the problem described in https://github.com/icub-tech-iit/urdf-modifiers/issues/10 is not present anymore if $K = 1$ is tested (set https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/LinkAndJointModifierV1.py#L25 and 1.0 in https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/generateRandomVariants.py#L170):

However, as soon as I try to set $K = 3.0$ (set PURE_SCALING in https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/LinkAndJointModifierV1.py#L25 and 3.0 in https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/generateRandomVariants.py#L170) the two quantities $s_o$ and $e_o$ assume weird values, and in the visualization now there are "gaps" (note: the twoLinks.urdf model has been explicitly designed to showcase this problems, as the original model in which I had this problem cannot be shared):

This lead me to think: given that we have three free variables, why we do not impose that the quantity do not $s_0'$ are $s_0$ are equal? This lead to the "Case 1" :

Case 1: Scaling with Starting Offset Fixed

In this case, we let $j_o$ and $v_l$ scale as in Case 0:

$$ j_o' = K j_0 $$

$$ v_l' = K v_l $$

However, we leave $v_o'$ as a "free" variable, so that we can impose that $s_o' = s_o$. In particular, by imposing this condition and given the definition of $s_o$, one gets:

$$ v_o' = v_o + (K - 1) \frac{v_l}{2} sign(j_o) $$

If one tries that, the weird behavior in the starting offset is solved, but there is now a weird behavior in the final offset, see in particular for our example (set SCALING_WITH_INITIAL_OFFSET_MANTAINED in https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/LinkAndJointModifierV1.py#L25 and 3.0 in https://github.com/traversaro/urdf-modifiers/blob/invariance2/examples/generateRandomVariants.py#L170):

So, this leads to the obvious next step: use another variable to ensure that also the ending offset does not change, i.e. $e_o' = e_o$. This leads to Case 2

Case 2: Scaling with Starting and Final Offset Fixed

In this case, only $j_o$ scales with $K$ (as this is the quantity that actually influences the kinematic structure of the robot:

$$ j_o' = K j_0 $$

while we select $v_l'$ and $v_o'$ such that $e_o' = e_o$ and $s_o' = s_o$:

$$ v_l' = v_l + (K-1) |j_o| $$

$$ v_o' = v_o + (K-1) \frac{j_o}{2} $$

The derivation is left to the reader (just for me to finish writing the issue fast, feel free to ask me more details or actually double check my calculation that could be wrong).

With this modifications, now the modifiers (in this context, where we are not scaling the sphere visual of the link) behaves as we expected:

[traversaro]

fyi @CarlottaSartore @fabiodinatale

[traversaro]

(Edited the message to point to the new branch https://github.com/traversaro/urdf-modifiers/blob/invariance2 updated with the api changes in https://github.com/icub-tech-iit/urdf-modifiers/pull/9 .