include/hpp/manipulation/device.hh

@@ -55,7 +55,9 @@ class HPP_MANIPULATION_DLLAPI Device
   /// \param name of the new instance,
   static DevicePtr_t create(const std::string& name);
 
-  DevicePtr_t self() { return shared_from_this(); }
+  DevicePtr_t self() {
+    return enable_shared_from_this<Device>::shared_from_this();
+  }
 
   /// Print object in a stream
   virtual std::ostream& print(std::ostream& os) const;

src/device.cc

@@ -139,8 +139,9 @@ void Device::removeJoints(const std::vector<std::string>& jointNames,
   Parent_t::removeJoints(jointNames, referenceConfig);
 
   for (auto& pair : grippers.map)
-    pair.second =
-        pinocchio::Gripper::create(pair.second->name(), shared_from_this());
+    pair.second = pinocchio::Gripper::create(
+        pair.second->name(),
+        std::enable_shared_from_this<Device>::shared_from_this());
   // TODO update handles and jointAndShapes
 }
 

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_);