Add Possibility to have multiple child joint

[CarlottaSartore]

This PR Adresses #21

C.C. @GrmanRodriguez @AlexAntn @traversaro

[traversaro]

Can you briefly describe how you are considering multiple joints in this PR? We can probably using the formalism used in https://github.com/icub-tech-iit/urdf-modifiers/issues/11 that is similar to the one used in the doc of the fixedOffsetModifier class.

[CarlottaSartore]

Can you briefly describe how you are considering multiple joints in this PR? We can probably using the formalism used in #11 that is similar to the one used in the doc of the fixedOffsetModifier class.

I did not introduced any new formalism, basically now the computation for the child joint is applied to all the child joints of the link in the same fashion as per #11

Let me know if it is not clear, I will then add a more formal description :)

[traversaro]

Let me know if it is not clear, I will then add a more formal description :)

I was a bit confused by "in the same fashion as", I am not so able to go deep in the code, I will try to document it from the code.

If we have a second_child_link we need first of all to introduce in the notation, so taking the nice ascii art from https://github.com/icub-tech-iit/urdf-modifiers/blob/8a5b9bb6d0f76ac6706691695a2d88789f43a15c/urdfModifiers/core/fixedOffsetModifier.py#L15 we have:

  --------- +-----------+ -
  ^     ^   |           | ^
  |e_so |   |     ^     | |
  |     |   |     |     | |
  |-----|---|     o-->  | |------------------------
        |   |second_child |                       ^
        |   |           | |                       |
        |   |           | |                       |
    e_o |   |     ^     | |                       |
        |   |     |     | |                       |
        --- |     o-->  | |-----------            |
            |   child   | |          ^            |
            |           | | v_l      |            |
            |           | |          |            |
            |     o---- | |----      |            |
            |   visual  | |   ^      | j_o        | j_so
            |           | |   |      |            |
            |           | |   |      |            |
            |           | |   |      |            |
            |           | |   |      |            |
            |           | |   |      |            |
            |           | v   |      |            |
        --- +-----------+ -   | v_o  |            |
        ^         ^           |      |            |
    s_o |         |           |      |            |
        |         o-->        |      |            |
                parent

That correspond to the case:

  <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" />
  <joint name="second_joint" type="continuous">
    <origin xyz="0 0 j_so" rpy="0 -0 0" />
    <axis xyz="0 1 0" />
    <parent link="link" />
    <child link="second_child_link" />
  </joint>
  <link name="second_child_link" />

So, if we want to find a relation between the original quantities v_o, v_l and j_o and the resize quantities v_o', v_l' and j_o', we need to impose three constraints. In the original issue https://github.com/icub-tech-iit/urdf-modifiers/issues/11 we imposed (if K was the multiplier):

e_o' = e_o 
s_o' = s_o 
j_o' = Kj_o  

while in https://github.com/icub-tech-iit/urdf-modifiers/pull/20 it was then imposed a slighly different version, i.e. :

e_o' = e_o 
s_o' = s_o 
v_l' = Kv_l  

(this was one of the reason I was confused, if the offset is now update following what was done in https://github.com/icub-tech-iit/urdf-modifiers/issues/11 this means that it changed w.r.t. to what was done in https://github.com/icub-tech-iit/urdf-modifiers/pull/20?).

However, in both case the result is such that the offset do not change, i.e. e_o' = e_o and s_o' = s_o

The resulting equations are (from the code comments):

v_l' = Kv_l  
v_o' = s_o + v_l' * sign(j_o) / 2
j_o' = v_o' - e_o + v_l' * sign(j_o) / 2

[traversaro]

At this point, I tried to understand from the what is the equation used to update j_so', but I got a bit lost in the code . It is possible that for congrunence with the rest of the code it is the equation imposing imposing e_so' = e_so, but to be honest I do not know if this is the case and that was the reason I asked.

[CarlottaSartore]

Thanks @traversaro , for the comment and the reasoning! It will for sure help in understanding possible bugs in the code :)

Let me try to explain better what I implemented, starting from the fancy ascii you were also using:

 --------- +-----------+ -
  ^     ^   |           | ^
  |e_o2 |   |     ^     | |
  |     |   |     |     | |
  |-----|---|     o-->  | |------------------------
  v     |   |second_child |                       ^
        |   |           | |                       |
        |   |           | |                       |
   e_o1 |   |     ^     | |                       |
        |   |     |     | |                       |
        v-- |     o-->  | |-----------            |
            |   child   | |          ^            |
            |           | | v_l      |            |
            |           | |          |            |
            |     o---- | |----      |            |
            |   visual  | |   ^      | j_o1        | j_o2
            |           | |   |      |            |
            |           | |   |      |            |
            |           | |   |      |            |
            |           | |   |      |            |
            |           | |   |      |            |
            |           | v   |      |            |
        --- +-----------+ -   | v_o  |            |
        ^         ^           |      |            |
    s_o |         |           |      |            |
        |         o-->        |      |            |
        v       parent        v      v            v

Basically what we want to compute is $e_{i0}$ for each joint $i \in child$ in the case depicted $child = [1,2]$ and $s_0$ which are the quantities that we want to keep constant throughout the scaling. Such quantities should then be constant, independently of the modification applied, and can be computed as:

$$ \begin{equation} e_{o,i} = v_o +v_l \frac{sign(j_{oi})}{2} - j_{o,i} \end{equation} $$

Code https://github.com/CarlottaSartore/urdf-modifiers/blob/37291f9e64a2071c4883b925a0002bb880d6ec10/urdfModifiers/core/fixedOffsetModifier.py#L171-L172

$$ \begin{equation} s_o = v_o- v_l \frac{sign(j_{o,1})}{2} \end{equation} $$

Code https://github.com/CarlottaSartore/urdf-modifiers/blob/37291f9e64a2071c4883b925a0002bb880d6ec10/urdfModifiers/core/fixedOffsetModifier.py#L166

Note for computing the parent offset, only one joint is taken into account, namely the first one.

Given a length multiplier $k$ we can scale the component as follow:

$$ \begin{equation} v_l^{'} = kv_l \end{equation} $$ Code:

https://github.com/CarlottaSartore/urdf-modifiers/blob/37291f9e64a2071c4883b925a0002bb880d6ec10/urdfModifiers/core/fixedOffsetModifier.py#L194

$$ \begin{equation} v_{o}^{'} = s_o+v_l{'}\frac{sign(j_{o,1})}{2} \end{equation} $$

Code: https://github.com/CarlottaSartore/urdf-modifiers/blob/37291f9e64a2071c4883b925a0002bb880d6ec10/urdfModifiers/core/fixedOffsetModifier.py#L211

$$ j_{o,i}^{'} = s_o + v_{l}^{'}sign({j_{o,i}}) - e_{o,i} $$

Code: https://github.com/CarlottaSartore/urdf-modifiers/blob/37291f9e64a2071c4883b925a0002bb880d6ec10/urdfModifiers/core/fixedOffsetModifier.py#L227-L229

Is it sound to you ?

[traversaro]

Is it sound to you ?

Yes, thanks! The only fishy is:

Note for computing the parent offset, only one joint is taken into account, namely the first one.

I am a bit confused on why this happens. Are we sure about the equation: \begin{equation} s_o = v_o- vl \frac{sign(j{o,1})}{2} \end{equation} ?

To be honest, I think I introduced (if I introduced it, I do not remember) that sign(j_{o,1}) with the assumption that $sign(vo) = sign(j{o,1}) = sign(j_{o,2})$, and I do not know if this assumption is not respected. Probably we can just enforce this assumption in the code (i.e. raise an error if this is not the case) and so we solve the ambiguity of "preferring" a joint w.r.t. to another?

[CarlottaSartore]

In the end, I think the more robust solution would be to change the above formulation as follow:

$$ \begin{equation} e_{o,i} = v_o +v_l \frac{sign(j_{oi})}{2} - j_{o,i} \end{equation} $$

$$ \begin{equation} s_o = (v_o- \frac{v_l}{2})sign(v_o) \end{equation} $$

$$ \begin{equation} v_l^{'} = kv_l \end{equation} $$

$$ \begin{equation} v_{o}^{'} = (s_o+\frac{v_l{'}}{2})sign(v_0) \end{equation} $$

$$ j_{o,i}^{'} = s_o + v_{l}^{'}sign({j_{o,i}}) - e_{o,i} $$

This should work in different cases, I was studying the case in which $v_o = 0 $ as per the attached representation

Seems to me that everything works fine for the computations, indeed:

$$ \begin{equation} e_{o,i} = v_l \frac{sign(j_{oi})}{2} - j_{o,i} \end{equation} $$

$$ \begin{equation} s_o = (- \frac{v_l}{2})sign(0)= - \frac{v_l}{2} \end{equation} $$

$$ \begin{equation} v_l^{'} = kv_l \end{equation} $$

$$ \begin{equation} v_{o}^{'} = - \frac{v_l}{2}+v_l{'}\frac{sign(0)}{2}= - \frac{v_l}{2} + K\frac{v_l}{2} = \frac{v_l}{2} (k-1) \end{equation} $$

Note This would mean that $v_o^{'}$ will not always be zero but it will be, such that $s_o$ will be equal to $-\frac{v_l}{2}$ always, as per the other cases.

$$ j_{o,i}^{'} = s_o + v_{l}^{'}sign({j_{o,i}}) - e_{o,i} $$

Case in which the $v_o$ is negative

Seems also that everything is working

@traversaro could you please double-check it? thanks :) I will then code it and check also the tests!

I think that with this implementation, we should get rid of the need for the following warning:

To be honest, I think I introduced (if I introduced it, I do not remember) that sign(j_{o,1}) with the assumption that sign(vo)=sign(jo,1)=sign(jo,2), and I do not know if this assumption is not respected. Probably we can just enforce this assumption in the code (i.e. raise an error if this is not the case) and so we solve the ambiguity of "preferring" a joint w.r.t. to another?

[CarlottaSartore]

Gentle reminder @traversaro :)

[traversaro]

Gentle reminder @traversaro :)

Thanks, I had missed this.

[traversaro]

@traversaro could you please double-check it? thanks :) I will then code it and check also the tests!

I am a bit confused by the definition of s_o (eq. 7), it seems discontinuous around v_o = 0, and that does not seems correct. I will try to write down the different cases, but if you are available we can discuss on the whiteboard.

[traversaro]

@traversaro could you please double-check it? thanks :) I will then code it and check also the tests!

I am a bit confused by the definition of s_o (eq. 7), it seems discontinuous around v_o = 0, and that does not seems correct. I will try to write down the different cases, but if you are available we can discuss on the whiteboard.

To clarify, this is the discontinuity:

(plotted by plotting (x-y/2)*sgn(x) with https://c3d.libretexts.org/CalcPlot3D/index.html, with x = v_o and y = v_l).

I think anyhow I got it now, basically if v_o changes sign, the "side" of the box that is used to compute s_o changes, hence the discontinuity. In that case, it seems to be that everything is working fine, but probably the best way to check is to modify the test in https://github.com/icub-tech-iit/urdf-modifiers/blob/9e8fff10e4a6812defc1e9e3b1a1bb30ce2b480b/tests/tests.py#L868 to ensure that the offset indeed does not change for all the possible cases of relative signs between $vo$ and $e{o,i}$ .

[CarlottaSartore]

I have updated the definition of all the variables as per the comment https://github.com/icub-tech-iit/urdf-modifiers/pull/24#issuecomment-1081834443 and rebase on top of master.

All the tests are passing! @traversaro

[traversaro]

I have updated the definition of all the variables as per the comment #24 (comment) and rebase on top of master.

All the tests are passing! @traversaro

Thanks, good for me, even if I guess there are still comments from the others.