From a3cce3b848e2dba6ac9d5a7f49f7162923fcf78f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 8 Jun 2017 18:07:02 +0200
Subject: [PATCH] Add LockedJoint before NumericalConstraint into
ConfigProjector
---
src/graph/edge.cc | 26 +++++++++++++-------------
src/handle.cc | 2 +-
2 files changed, 14 insertions(+), 14 deletions(-)
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index d08eb652..1a3c5cd7 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 f5c00980..9fdea11b 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
--
GitLab