In Edge::buildConfigConstraints, add node constraints when edge is in from.

  - only destination node constraints were added.

src/graph/edge.cc

@@ -207,11 +207,17 @@ namespace hpp {
         g->insertNumericalConstraints (proj);
         insertNumericalConstraints (proj);
         to ()->insertNumericalConstraints (proj);
+	if (node () == from ()) {
+	  from ()->insertNumericalConstraints (proj);
+	}
         constraint->addConstraint (proj);
 
         g->insertLockedJoints (proj);
         insertLockedJoints (proj);
         to ()->insertLockedJoints (proj);
+	if (node () == from ()) {
+	  from ()->insertLockedJoints (proj);
+	}
 
         constraint->edge (wkPtr_.lock ());
         return constraint;
@@ -692,6 +698,9 @@ namespace hpp {
 
         insertNumericalConstraints (proj);
         to ()->insertNumericalConstraints (proj);
+	if (node () == from ()) {
+	  from ()->insertNumericalConstraints (proj);
+	}
         constraint->addConstraint (proj);
 
         g->insertLockedJoints (proj);
@@ -701,6 +710,9 @@ namespace hpp {
         }
         insertLockedJoints (proj);
         to ()->insertLockedJoints (proj);
+	if (node () == from ()) {
+	  from ()->insertLockedJoints (proj);
+	}
 
         constraint->edge (wkPtr_.lock ());
         return constraint;