ContactForcePublisher crashes when going from an empty contact set to a non empty one

[arntanguy]

ContactForcePublisher crashes on

mc_rtc_ros/mc_rtc_ticker/src/ContactForcePublisher.cpp

Line 183 in fbdbc4a

auto r1Dir = qp_c.force(lambda, i, qp_c.r1Cones);

when going from an empty contact set to a non-empty contact set.

This fails

  solver().setContacts({{}});
  // ... then at a later iteration
  solver().setContacts({
      mc_rbdyn::Contact(robots(), "LeftFoot", "AllGround"),
      mc_rbdyn::Contact(robots(), "RightFoot", "AllGround")
  });

but this works

  solver().setContacts({
      mc_rbdyn::Contact(robots(), "LeftFoot", "AllGround"),
      mc_rbdyn::Contact(robots(), "RightFoot", "AllGround")
  });
  // ... then at a later iteration
  solver().setContacts({
      mc_rbdyn::Contact(robots(), "LeftFoot", "AllGround")
  });
}

[arntanguy]

This happens if one runs MCControl::run() before adding the contact in the same iteration, which makes the state of qpRes inconsistent:

Controller::Controller()
{
   solver().setContacts({{}}); // empty set
}

Controller::run()
{
   bool ret = MCController::run();
   // calls __fillResult, so qpRes.lambdaVec has size = 0
   solver().setContacts({ ... });
   // sets qpRes.contacts vector != 0
   // as far as tasks library is concerned, there is a contact so solver.lambdaVec size != 0
   // lambdaVec hasn't been computed yet there so its values are zero
   return ret;
}

ContactForcePublisher::update()
{
  // qpRes.lambdaVec size is 0 since we added the contact after solving the QP (it is only set by QPSolver::__fillResult())
  // however qpRes.contacts size != 0 (it is set by QPSolver::setContacts directly)
  // hence we have a mismatch qpRest.contacts.size != 0 but qpRes.lambdaVec.size = 0; and ContactForcePublisher (correctly) assumes that lambdaVec is not null here and thus crashes
}

Thus to keep things consistent I suggest we do jrl-umi3218/mc_rtc@8b3dc2f