From 121aed093f6968f95a9fdc8167f6a64dc24c915f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 21 Jan 2015 16:03:27 +0100
Subject: [PATCH] Add passiveDofs_ member to GraphComponent
---
include/hpp/manipulation/graph/edge.hh | 4 +++-
include/hpp/manipulation/graph/fwd.hh | 2 ++
include/hpp/manipulation/graph/graph-component.hh | 8 +++++++-
include/hpp/manipulation/graph/node-selector.hh | 4 +++-
include/hpp/manipulation/graph/node.hh | 5 ++++-
src/graph/edge.cc | 9 +++++++--
src/graph/graph-component.cc | 12 +++++++++---
7 files changed, 35 insertions(+), 9 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index dc9a9dba..163492f0 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -222,7 +222,8 @@ namespace hpp {
LeafHistogramPtr_t histogram () const;
- void insertConfigConstraint (const NumericalConstraintPtr_t& nm);
+ void insertConfigConstraint (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq) __attribute__ ((deprecated));
@@ -250,6 +251,7 @@ namespace hpp {
/// Extra NumericalConstraints_t
NumericalConstraints_t extraNumericalConstraints_;
+ IntervalsContainer_t extraPassiveDofs_;
/// Extra LockedJoints_t
LockedJoints_t extraLockedJoints_;
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index b6997fb1..ff2f0529 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -55,6 +55,8 @@ namespace hpp {
typedef hpp::core::Equality Equality;
typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
+ typedef hpp::core::SizeIntervals_t SizeIntervals_t;
+ typedef std::vector <SizeIntervals_t> IntervalsContainer_t;
typedef std::list < NumericalConstraintPtr_t > NumericalConstraints_t;
typedef std::list < LockedJointPtr_t > LockedJoints_t;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index be8ceb6b..f06ed8dc 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -50,7 +50,11 @@ namespace hpp {
int id () const;
/// Add core::NumericalConstraint to the component.
- virtual void addNumericalConstraint (const NumericalConstraintPtr_t& numConstraint);
+ /// \param passiveDofs see ConfigProjector::addNumericalConstraint
+ // for more information.
+ virtual void addNumericalConstraint (
+ const NumericalConstraintPtr_t& numConstraint,
+ const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
/// Add core::DifferentiableFunction to the component.
virtual void addNumericalConstraint
@@ -90,6 +94,8 @@ namespace hpp {
/// Stores the numerical constraints.
NumericalConstraints_t numericalConstraints_;
+ /// Stores the passive dofs for each numerical constraints.
+ std::vector <SizeIntervals_t> passiveDofs_;
/// List of LockedJoint constraints
LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph.
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index fe0bac4f..bcb02d8c 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -43,7 +43,9 @@ namespace hpp {
virtual EdgePtr_t chooseEdge(const NodePtr_t& node) const;
/// Should never be called.
- void addNumericalConstraint (const core::NumericalConstraintPtr_t& /* function */)
+ void addNumericalConstraint (
+ const core::NumericalConstraintPtr_t& /* function */,
+ const SizeIntervals_t& /* passiveDofs */ = SizeIntervals_t ())
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
}
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 8973f302..00036883 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -85,9 +85,11 @@ namespace hpp {
ConstraintSetPtr_t configConstraint() const;
/// Add core::NumericalConstraint to the component.
- virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm)
+ virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs = SizeIntervals_t ())
{
numericalConstraintsForPath_.push_back (nm);
+ passiveDofsForPath_.push_back (passiveDofs);
}
/// Add core::DifferentiableFunction to the component.
@@ -139,6 +141,7 @@ namespace hpp {
/// Stores the numerical constraints for path.
NumericalConstraints_t numericalConstraintsForPath_;
+ IntervalsContainer_t passiveDofsForPath_;
/// A selector that will implement the selection of the next state.
NodeSelectorWkPtr_t selector_;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 2b17af68..b5c1976e 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -429,10 +429,13 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
g->insertNumericalConstraints (proj);
+ IntervalsContainer_t::const_iterator itpdof = extraPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
it != extraNumericalConstraints_.end (); ++it) {
- proj->add (*it);
+ proj->add (*it, *itpdof);
+ ++itpdof;
}
+ assert (itpdof == extraPassiveDofs_.end ());
!extraNumericalConstraints_.empty ();
insertNumericalConstraints (proj);
to ()->insertNumericalConstraints (proj);
@@ -450,9 +453,11 @@ namespace hpp {
return extraConstraints_->get ();
}
- void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm)
+ void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs)
{
extraNumericalConstraints_.push_back (nm);
+ extraPassiveDofs_.push_back (passiveDofs);
}
void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 9406aa54..22733726 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -63,9 +63,11 @@ namespace hpp {
return os;
}
- void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm)
+ void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs)
{
numericalConstraints_.push_back(nm);
+ passiveDofs_.push_back (passiveDofs);
}
void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
@@ -81,9 +83,13 @@ namespace hpp {
bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
{
+ IntervalsContainer_t::const_iterator itpdof = passiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = numericalConstraints_.begin();
- it != numericalConstraints_.end(); ++it)
- proj->add (*it);
+ it != numericalConstraints_.end(); ++it) {
+ proj->add (*it, *itpdof);
+ ++itpdof;
+ }
+ assert (itpdof == passiveDofs_.end ());
return !numericalConstraints_.empty ();
}
--
GitLab