jrl-umi3218/lipm_walking_controller

updating contact Refvel

Opened this issue · 1 comments

Saeed-Mansouri commented

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 commented

Good catch! 🎣

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
  • 👍1