[Edge] Compute forward kinematics before filtering collision pair.

  In method hpp::core::ObstacleUser::filterCollisionPairs, a collision test
  is performed in block "case RelativeMotion::Constrained:". This test
  requires prior computation of the forward kinematics.

Author: florent-lamiraux

src/graph/edge.cc

@@ -302,6 +302,10 @@ ConstraintSetPtr_t Edge::buildPathConstraint() {
   shared_ptr<core::ObstacleUserInterface> oui =
       HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
   if (oui) {
+    // Compute forward kinematics and geometry placement to avoid assertions  on non
+    // initialized data
+    g->robot()->computeForwardKinematics(pinocchio::COMPUTE_ALL);
+    g->robot()->updateGeometryPlacements();
     relMotion_ = RelativeMotion::matrix(g->robot());
     RelativeMotion::fromConstraint(relMotion_, g->robot(), constraint);
     oui->filterCollisionPairs(relMotion_);