From 8108ce6ac97eae9ef5336fc6ff8cc244aef87e76 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 10 Feb 2015 14:41:40 +0100
Subject: [PATCH] Update LevelSetEdge::applyConstraints to NumericalConstraint
class
---
src/graph/edge.cc | 17 ++++-------------
1 file changed, 4 insertions(+), 13 deletions(-)
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index b5c1976e..c5b85a5e 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -341,26 +341,17 @@ namespace hpp {
ConstraintSetPtr_t cs = extraConfigConstraint ();
const ConfigProjectorPtr_t cp = cs->configProjector ();
assert (cp);
- vector_t offset = cp->rightHandSideFromConfig (q_offset);
- size_t row = 0, nbRows = 0;
+ cp->rightHandSideFromConfig (q_offset);
for (NumericalConstraints_t::const_iterator it =
extraNumericalConstraints_.begin ();
it != extraNumericalConstraints_.end (); ++it) {
- const core::DifferentiableFunction& f = (*it)->function ();
- nbRows = f.outputSize ();
- vector_t value = vector_t::Zero (nbRows);
- // TODO: fix this function
- if (!(*it)->comparisonType ()->constantRightHandSide ()) {
- f (value, levelsetTarget);
- offset.segment (row, nbRows) = value;
- row += nbRows;
- }
- }
- cp->rightHandSide (offset);
+ (*it)->rightHandSideFromConfig (levelsetTarget);
+ }
for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
it != extraLockedJoints_.end (); ++it) {
(*it)->rightHandSideFromConfig (levelsetTarget);
}
+ cp->updateRightHandSide ();
// Eventually, do the projection.
if (cs->apply (q))
--
GitLab