How to compute the J and JDotNu for contact vertices

[VenusPasandi]

We need $J$ and $\dot{J} \nu$ of the contact vertices for computing the forward dynamics of the system. Actually, we need just the part of these terms that relates to the linear velocity and linear acceleration of the contact vertices.

Considering $f$ as the foot frame, we can write the linear velocity of a contact vertex of the frame $f$ like $i$ as:

\begin{equation} v_i = vf - S(r{if}) \omega_f \end{equation}

As $v = J_L \nu$ and $\omega_f = J_A \nu$ where $J_L$ and $J_A$ are the linear and angular parts of the Jacobian matrix of the frame $f$, we have

\begin{equation} J_{Li} = J{Lf} - S(r{if}) J_{A_f} \end{equation}

Differentiating the above equation and multiplying it by $\nu$, we have

\begin{equation} \dot{J}_{Li} \nu = \dot{J}\{Lf} \nu - S(v{if}) J_{Af} \nu - S(r{if}) \dot{J}_{A_f} \nu \end{equation}

However, in https://github.com/ami-iit/matlab-whole-body-simulator/blob/master/lib/%2Bmwbs/%40Contacts/compute_J_and_JDot_nu_in_contact_frames.m, we used the following relations:

\begin{equation} J_{Li} = J{Lf} - S(r{if}) J_{Af}, \ \dot{J}\{Li} \nu = \dot{J}\{Lf} \nu - S(r{if}) \dot{J}_{A_f} \nu \end{equation}

I can not understand why the term $- S(v{if}) J{A_f} \nu$ is ignored. This term is equal to $- (\omegaf \times r{if}) \times \omega_f$. Does any body have any idea on it?

cc: @traversaro, @nunoguedelha, @GiulioRomualdi

[DanielePucci]

@VenusPasandi I believe that the math is not rendering properly

[VenusPasandi]

@VenusPasandi I believe that the math is not rendering properly

Sorry @DanielePucci. I tried to correct the rendering issue. It seems there is a rendering issue with writing $\dot{J}_i$!! I don't know if anyone has a solution for it?

cc: @ami-iit/artificial-mechanical-intelligence

[traversaro]

It is rendering fine for me. Anyhow, when you have a problem in some formula involving a _, you can tipically change it to \_ to avoid the problems.

[VenusPasandi]

It is rendering fine for me. Anyhow, when you have a problem in some formula involving a _, you can tipically change it to \_ to avoid the problems.

Thank you @traversaro, your trick works nice :)

[traversaro]

This term is equal to $- (\omegaf \times r{if}) \times \omega_f$. Does any body have any idea on it?

Are you sure that that term does not evalute to 0 due to some formulas such as the one https://en.wikipedia.org/wiki/Triple_product ? Intuitively I would say no, but it may be worth to check ?

[S-Dafarra]

Is $S(r{if})$ changing? If $r{if}$ is the relative position between the vertex and the center, then this would not change, right?

[VenusPasandi]

Is S(rif) changing? If rif is the relative position between the vertex and the center, then this would not change, right?

Yes, $r_{if}$ is the relative position of the vertex in the foot frame. So, its magnitude is constant and does not change. But, its direction changes.

[S-Dafarra]

Is S(rif) changing? If rif is the relative position between the vertex and the center, then this would not change, right?

Yes, $r_{if}$ is the relative position of the vertex in the foot frame. So, its magnitude is constant and does not change. But, its direction changes.

It depends on the frame where it is measured. If it is measured in the frame attached to the foot, then also the direction is constant

[VenusPasandi]

Is S(rif) changing? If rif is the relative position between the vertex and the center, then this would not change, right?

Yes, $r_{if}$ is the relative position of the vertex in the foot frame. So, its magnitude is constant and does not change. But, its direction changes.

It depends on the frame where it is measured. If it is measured in the frame attached to the foot, then also the direction is constant

Actually, the first time that I read the code, I thought the same as you.

But we are computing the time derivative with respect to the inertial/world frame. So, even if it is defined in the local frame, because of the rotation of the local frame, its time derivative is not zero.

[Giulero]

Hi @VenusPasandi!

The math should be here https://github.com/ami-iit/matlab-whole-body-simulator/issues/1#issue-693202831

Maybe there is something wrong there. This is a good occasion to double check it! :)

[VenusPasandi]

Hi @VenusPasandi!

The math should be here #1 (comment)

Maybe there is something wrong there. This is a good occasion to double check it! :)

Hi @Giulero! Thank you for your reference. Unfortunately, in https://github.com/ami-iit/matlab-whole-body-simulator/issues/1#issue-693202831 the part related to the JDotNu is ignored and only the final relation is written! But, there is a hint which says that we obtain the relation in the same way as the relation of J, so, not with the differentiation of the relation of $J$. So, let us try in this way: The acceleration of the point i is as

\begin{equation} \begin{aligned} a_i = a_f + \alphaf \times r{if} + \omega_f \times (\omegaf \times r{if}) \ \newline = (\dot{J}_{Lf} \nu + J{Lf} \dot{\nu}) + (\dot{J}\{Af} \nu + J{Af} \dot{\nu}) \times r{if} - (J_{Af} \nu \times r{if}) \times J_{Af} \nu \ \newline = (\dot{J}\{Lf} - S(r{if}) \dot{J}_{Af} - S(J{Af} \nu \times r{if}) J_{Af}) \nu + ( J{Lf} - S(r{if}) J_{A_f} ) \dot{\nu} \end{aligned} \end{equation}

So,

\begin{equation} \begin{aligned} Ji = J{Lf} - S(r{if}) J_{Af} \ \newline \dot{J}_i = \dot{J}\{Lf} - S(r{if}) \dot{J}_{Af} - S(J{Af} \nu \times r{if}) J_{A_f} \end{aligned} \end{equation}

So, again we have the term $S(J_{Af} \nu \times r{if}) J_{A_f}$ in the JDotNu expression. Note that, this term is the centrifugal acceleration of point i. So, if we don't have any assumption for the motion of the foot, in general, the centrifugal term is not zero.

[DanielePucci]

TLDR

This term is equal to $- (\omegaf \times r{if}) \times \omega_f$. Does any body have any idea on it?

I do not know if it helps you @VenusPasandi, but notice that

$- (\omegaf \times r{if}) \times \omega_f = \omega_f \times (\omegaf \times r{if}) = S(\omegaf )^2r{if} = (\omega_f \omega^\top_f - |\omegaf|^2 I)r{if}$

[VenusPasandi]

TLDR

This term is equal to $- (\omegaf \times r{if}) \times \omega_f$. Does any body have any idea on it?

I do not know if it helps you @VenusPasandi, but notice that

$- (\omegaf \times r{if}) \times \omega_f = \omega_f \times (\omegaf \times r{if}) = S(\omegaf )^2r{if} = (\omega_f \omega^\top_f - |\omegaf|^2 I)r{if}$

Thank you @DanielePucci. This relation is really useful if we want to add this term to current implemented equation.

[VenusPasandi]

I couldn't find any reason/assumption explaining that the term $ \omega_f \times (\omegaf \times r{if}) $ is equal to zero. So, to be sure, I tested this term in a simulation. I performed two simulations; first by ignoring this term and second by considering this term (where I implemented this term by using the formula that @DanielePucci mentioned in https://github.com/ami-iit/matlab-whole-body-simulator/issues/67#issuecomment-1082210907). The results are as the following where the red dotted curves denote the first case (ignoring this term) and the blue curves denote the second case (considering this term).

So, this term is not equal to zero and ignoring or considering it affects the robot velocity vector and contact wrenches. So, I think this term is ignored by a simple mistake like https://github.com/ami-iit/matlab-whole-body-simulator/issues/67#issuecomment-1081796461 if the reasoning explained in https://github.com/ami-iit/matlab-whole-body-simulator/issues/67#issue-1184596992 is correct.

So, I kindly ask @Giulero, @traversaro and @DanielePucci to check the math in https://github.com/ami-iit/matlab-whole-body-simulator/issues/67#issue-1184596992.

[Giulero]

@VenusPasandi! You're right!

I did the computations again. We know that: $$ Ji \nu = \begin{bmatrix} I & -S({}^I R\text{foot} {}^\text{foot} p_I) \end{bmatrix} J \nu $$

so $Ji = \begin{bmatrix} I & -S({}^I R\text{foot} {}^\text{foot} p_I) \end{bmatrix} J$.

Differentiating $J_i$ leads to: $$ \dot{Ji} = \begin{bmatrix} I & -S({}^I R\text{foot} {}^\text{foot} p_I) \end{bmatrix} \dot{J}+ \begin{bmatrix} 0 & - S(S(JA \nu) {}^I R\text{foot} {}^\text{foot} p_I ) \end{bmatrix} J $$

If we inject back $\nu$ and do some manipulations the following term appears: $$ S(J_A \nu) S(JA \nu) {}^I R\text{foot} {}^\text{foot} p_I , $$ that's what you call $\omega \times \omega \times r$.

Now my main concern is that $\nu$ is an optimization variable, and with this new term we lose the linear relationship with $\nu$. It seems that using https://github.com/ami-iit/matlab-whole-body-simulator/issues/67#issuecomment-1082210907 we can extract a quadratic relationship, but how do we compute the 2-norm?

[Giulero]

Ah, I missed your comment! :D How do you write the optimization problem?

[VenusPasandi]

Thank you @Giulero

Now my main concern is that $\nu$ is an optimization variable, and with this new term we lose the linear relationship with $\nu$. It seems that using https://github.com/ami-iit/matlab-whole-body-simulator/issues/67#issuecomment-1082210907 we can extract a quadratic relationship,

Actually, the time derivative of $\nu$ i.e $\dot{\nu}$ is the optimisation variable. If you remember in https://github.com/ami-iit/matlab-whole-body-simulator/issues/1#issue-693202831, we have $\dot{J} \nu + J \dot{\nu}$ where $\dot{\nu}$ is an optimisation variable. So, we don't need the linear relationship with respect to $\nu$.

[Giulero]

Yes, I went through the math and the code and my memory went back! :) Thank you!

[VenusPasandi]

Thank you all for the nice discussion. I added the missing term into the dynamics block via #71

[DanielePucci]

Very well done @VenusPasandi and @Giulero !

[diegoferigo]

Ignore this comment, necessary to render math with