diff --git a/src/graph/edge.cc b/src/graph/edge.cc index d08eb652fbf85d9564900ef783c50c3dc45a78fa..1a3c5cd73c7be1e516f4cb3f0bfe2d2d2cf0cb9a 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -205,14 +205,6 @@ namespace hpp { ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n); ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); - g->insertNumericalConstraints (proj); - insertNumericalConstraints (proj); - to ()->insertNumericalConstraints (proj); - if (state () != to ()) { - state ()->insertNumericalConstraints (proj); - } - constraint->addConstraint (proj); - g->insertLockedJoints (proj); insertLockedJoints (proj); to ()->insertLockedJoints (proj); @@ -220,6 +212,14 @@ namespace hpp { state ()->insertLockedJoints (proj); } + g->insertNumericalConstraints (proj); + insertNumericalConstraints (proj); + to ()->insertNumericalConstraints (proj); + if (state () != to ()) { + state ()->insertNumericalConstraints (proj); + } + + constraint->addConstraint (proj); constraint->edge (wkPtr_.lock ()); return constraint; } @@ -241,15 +241,15 @@ namespace hpp { ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n); ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); - g->insertNumericalConstraints (proj); - insertNumericalConstraints (proj); - state ()->insertNumericalConstraintsForPath (proj); - constraint->addConstraint (proj); - g->insertLockedJoints (proj); insertLockedJoints (proj); state ()->insertLockedJoints (proj); + g->insertNumericalConstraints (proj); + insertNumericalConstraints (proj); + state ()->insertNumericalConstraintsForPath (proj); + + constraint->addConstraint (proj); constraint->edge (wkPtr_.lock ()); // Build steering method diff --git a/src/handle.cc b/src/handle.cc index f5c009807687754b23e8feda36539dd0506148e6..9fdea11b3ec95b73cce8b3db454e5739a245e6d5 100644 --- a/src/handle.cc +++ b/src/handle.cc @@ -114,7 +114,7 @@ namespace hpp { return ExplicitRelativeTransformation::create ("Explicit_relative_transform_" + name () + "_" + gripper->name (), gripper->joint ()->robot (), gripper->joint (), joint (), - gripper->objectPositionInJoint (), localPosition()); + gripper->objectPositionInJoint (), localPosition())->createNumericalConstraint(); } return NumericalConstraintPtr_t (NumericalConstraint::create (RelativeTransformation::create