updating contact Refvel

[Saeed-Mansouri]

Hello @stephane-caron

As far as I understood the contact.velRef is not updated in FootstepPlan::updateInitialTransform function. So I think that this is a bug of the LIPM. I like to suggest adding the following commands to the mentioned function.

for(auto & contact : contacts_)
  {
    contact.refVel = (X_delta.rotation()).inverse() * contact.refVel;
  }

[stephane-caron]

Good catch! :fishing_pole_and_fish:

This velocity is only set in PlanInterpolator::runWalking_() and it should indeed be updated by FootstepPlan::updateInitialTransform(), otherwise ModelPredictiveControl::updateVelCost() uses the velocity in the wrong frame.

I agree with your fix. Here is how it checks for me:

• Let's write v' the new contact.refVel after update and v the old one
• We want v'_C' in the new contact frame C' to be the same as v_C in the old contact frame C
• Old and new contact frame orientations are, by construction: R_0_C' = R_0_C * R_delta
• Then v'_C' == v_C becomes R_0_C' * v'_0 == R_0_C * R_delta * v'_0 == R_0_C * v_0
• That is v'_0 == R_delta^{-1} * v_0