From a2c29d6457a4cba026e50b3b31f44204b2ff7db2 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 14 Jan 2015 10:57:20 +0100
Subject: [PATCH] Follow refactoring of hpp-core.
* Rename LockedDof in LockedJoint
* Use NumericalConstraint instead of DiffFuncAndIneqPair
* Make old methods deprecated.
* Always add a ConfigProjector to ConstraintSet.
---
include/hpp/manipulation/fwd.hh | 4 +-
include/hpp/manipulation/graph/edge.hh | 12 +-
include/hpp/manipulation/graph/fwd.hh | 3 +-
.../hpp/manipulation/graph/graph-component.hh | 23 ++--
.../hpp/manipulation/graph/node-selector.hh | 2 +-
include/hpp/manipulation/graph/node.hh | 22 ++--
include/hpp/manipulation/problem-solver.hh | 16 +--
src/graph/edge.cc | 119 +++++++++---------
src/graph/graph-component.cc | 28 ++---
src/graph/node.cc | 12 +-
src/path-projector.cc | 2 +-
src/path-projector/dichotomy.cc | 8 +-
src/path-projector/progressive.cc | 8 +-
src/problem-solver.cc | 7 +-
14 files changed, 136 insertions(+), 130 deletions(-)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index f5565021..846babf0 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -89,6 +89,8 @@ namespace hpp {
typedef core::ConstraintPtr_t ConstraintPtr_t;
typedef core::LockedJoint LockedJoint;
typedef core::LockedJointPtr_t LockedJointPtr_t;
+ typedef core::NumericalConstraint NumericalConstraint;
+ typedef core::NumericalConstraintPtr_t NumericalConstraintPtr_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
typedef core::ConstraintSet ConstraintSet;
@@ -101,7 +103,7 @@ namespace hpp {
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
- typedef std::map <std::string, LockedJointPtr_t> LockedDofConstraintMap_t;
+ typedef std::map <std::string, LockedJointPtr_t> LockedJointMap_t;
typedef fcl::TriangleP Triangle;
typedef std::list <Triangle> TriangleList;
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 7eea322a..dc9a9dba 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -222,9 +222,11 @@ namespace hpp {
LeafHistogramPtr_t histogram () const;
- void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq);
+ void insertConfigConstraint (const NumericalConstraintPtr_t& nm);
- void insertConfigConstraint (const LockedJointPtr_t lockedDof);
+ void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq) __attribute__ ((deprecated));
+
+ void insertConfigConstraint (const LockedJointPtr_t lockedJoint);
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -246,11 +248,11 @@ namespace hpp {
Constraint_t* extraConstraints_;
virtual ConstraintSetPtr_t extraConfigConstraint () const;
- /// Extra DifferentiableFunctions_t
- DifferentiableFunctions_t extraNumericalFunctions_;
+ /// Extra NumericalConstraints_t
+ NumericalConstraints_t extraNumericalConstraints_;
/// Extra LockedJoints_t
- LockedJoints_t extraLockedDofs_;
+ LockedJoints_t extraLockedJoints_;
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index 925e1c26..b6997fb1 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -55,8 +55,7 @@ namespace hpp {
typedef hpp::core::Equality Equality;
typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
- typedef std::pair < DifferentiableFunctionPtr_t, ComparisonTypePtr_t > DiffFuncAndIneqPair_t;
- typedef std::list < DiffFuncAndIneqPair_t > DifferentiableFunctions_t;
+ typedef std::list < NumericalConstraintPtr_t > NumericalConstraints_t;
typedef std::list < LockedJointPtr_t > LockedJoints_t;
class Histogram;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 2d7806a1..1f1f9710 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -22,6 +22,7 @@
# include <hpp/util/exception.hh>
# include "hpp/manipulation/config.hh"
+# include "hpp/manipulation/deprecated.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include "hpp/manipulation/graph/dot.hh"
@@ -48,29 +49,31 @@ namespace hpp {
/// Return the component id.
int id () const;
- /// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function) __attribute__ ((deprecated));
+ /// Add core::NumericalConstraint to the component.
+ virtual void addNumericalConstraint (const NumericalConstraintPtr_t& numConstraint);
/// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq);
+ virtual void addNumericalConstraint
+ (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
+ HPP_MANIPULATION_DEPRECATED;
/// Add core::LockedJoint constraint to the component.
virtual void addLockedJointConstraint
(const LockedJointPtr_t& constraint);
/// Insert the numerical constraints in a ConfigProjector
- /// \return true is at least one DifferentiableFunctionPtr_t was inserted.
+ /// \return true is at least one NumericalConstraintPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
/// Insert the LockedJoint constraints in a ConstraintSet
/// \return true is at least one LockedJointPtr_t was inserted.
- bool insertLockedDofs (ConstraintSetPtr_t cs) const;
+ bool insertLockedJoints (ConfigProjectorPtr_t& cs) const;
- /// Get a reference to the DifferentiableFunctions_t
- const DifferentiableFunctions_t& numericalConstraints() const;
+ /// Get a reference to the NumericalConstraints_t
+ const NumericalConstraints_t& numericalConstraints() const;
/// Get a reference to the LockedJoints_t
- const LockedJoints_t& lockedDofConstraints () const;
+ const LockedJoints_t& lockedJoints () const;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
@@ -86,9 +89,9 @@ namespace hpp {
{}
/// Stores the numerical constraints.
- DifferentiableFunctions_t numericalConstraints_;
+ NumericalConstraints_t numericalConstraints_;
/// List of LockedJoint constraints
- LockedJoints_t lockedDofConstraints_;
+ LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index 8d854855..fe0bac4f 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -43,7 +43,7 @@ namespace hpp {
virtual EdgePtr_t chooseEdge(const NodePtr_t& node) const;
/// Should never be called.
- void addNumericalConstraint (const core::DifferentiableFunctionPtr_t& /* function */)
+ void addNumericalConstraint (const core::NumericalConstraintPtr_t& /* function */)
{
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 55d41874..85ddb266 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -24,6 +24,7 @@
#include <hpp/core/config-projector.hh>
#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/deprecated.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/edge.hh"
@@ -83,30 +84,31 @@ namespace hpp {
/// Constraint to project onto this node.
ConstraintSetPtr_t configConstraint() const;
- /// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function)
+ /// Add core::NumericalConstraint to the component.
+ virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm)
{
- numericalConstraintsForPath_.push_back (DiffFuncAndIneqPair_t(function,Equality::create()));
+ numericalConstraintsForPath_.push_back (nm);
}
/// Add core::DifferentiableFunction to the component.
virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
+ HPP_MANIPULATION_DEPRECATED
{
- numericalConstraintsForPath_.push_back (DiffFuncAndIneqPair_t(function,ineq));
+ numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
}
/// Insert the numerical constraints in a ConfigProjector
- /// \return true is at least one DifferentiableFunctionPtr_t was inserted.
+ /// \return true is at least one NumericalConstraintPtr_t was inserted.
bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
{
- for (DifferentiableFunctions_t::const_iterator it = numericalConstraintsForPath_.begin();
+ for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
it != numericalConstraintsForPath_.end(); it++)
- proj->addFunction (it->first, it->second);
+ proj->add (*it);
return !numericalConstraintsForPath_.empty ();
}
- /// Get a reference to the DifferentiableFunctions_t
- const DifferentiableFunctions_t& numericalConstraintsForPath () const
+ /// Get a reference to the NumericalConstraints_t
+ const NumericalConstraints_t& numericalConstraintsForPath () const
{
return numericalConstraintsForPath_;
}
@@ -134,7 +136,7 @@ namespace hpp {
Constraint_t* configConstraints_;
/// Stores the numerical constraints for path.
- DifferentiableFunctions_t numericalConstraintsForPath_;
+ NumericalConstraints_t numericalConstraintsForPath_;
/// A selector that will implement the selection of the next state.
NodeSelectorWkPtr_t selector_;
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 84936c27..e9ef9eb8 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -39,7 +39,7 @@ namespace hpp {
{
}
ProblemSolver () : core::ProblemSolver (), robot_ (),
- robotsAndObjects_ (), graspsMap_(), lockedDofConstraintMap_()
+ robotsAndObjects_ (), graspsMap_(), lockedJointMap_()
{
}
/// Set robot
@@ -120,16 +120,16 @@ namespace hpp {
/// Add a LockedJoint constraint to the map
/// \param name key of the constraint as stored in an internal map.
- /// \param lockedDof the constraint to add.
- void addLockedJointConstraint (const std::string& name,
- const LockedJointPtr_t& lockedDof)
+ /// \param lockedJoint the constraint to add.
+ void addLockedJoint (const std::string& name,
+ LockedJointPtr_t& lockedJoint)
{
- lockedDofConstraintMap_ [name] = lockedDof;
+ lockedJointMap_ [name] = lockedJoint;
}
/// Get a LockedJoint constraint by name
/// \param name key of the constraint as stored in an internal map.
- LockedJointPtr_t lockedDofConstraint (const std::string& name) const;
+ LockedJointPtr_t lockedJoint (const std::string& name) const;
/// Reset constraint set and put back the disable collisions
/// between gripper and handle
@@ -143,7 +143,7 @@ namespace hpp {
/// If constraint is a graps, deactivate collision between gripper and
/// object.
virtual void addFunctionToConfigProjector
- (const std::string& constraintName, const std::string& functionName);
+ (const std::string& constraintName, const std::string& functionName);
/// Build a composite robot from several robots and objects
/// \param robotName Name of the composite robot,
@@ -184,7 +184,7 @@ namespace hpp {
/// Map of single robots to store before building a composite robot.
RobotsandObjects_t robotsAndObjects_;
GraspsMap_t graspsMap_;
- LockedDofConstraintMap_t lockedDofConstraintMap_;
+ LockedJointMap_t lockedJointMap_;
TriangleMap contactTriangles_;
}; // class ProblemSolver
} // namespace manipulation
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 0757f090..0866a94d 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -106,16 +106,14 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- // If at least one DifferentiableFunctionPtr_t was inserted, the add the projector.
- bool hasDiffFunc = g->insertNumericalConstraints (proj);
- hasDiffFunc = insertNumericalConstraints (proj) || hasDiffFunc;
- hasDiffFunc = to ()->insertNumericalConstraints (proj) || hasDiffFunc;
- if (hasDiffFunc)
- constraint->addConstraint (proj);
-
- g->insertLockedDofs (constraint);
- insertLockedDofs (constraint);
- to ()->insertLockedDofs (constraint);
+ g->insertNumericalConstraints (proj);
+ insertNumericalConstraints (proj);
+ to ()->insertNumericalConstraints (proj);
+ constraint->addConstraint (proj);
+
+ g->insertLockedJoints (proj);
+ insertLockedJoints (proj);
+ to ()->insertLockedJoints (proj);
return constraint;
}
@@ -135,16 +133,14 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- // If at least one DifferentiableFunctionPtr_t was inserted, the add the projector.
- bool hasDiffFunc = g->insertNumericalConstraints (proj);
- hasDiffFunc = insertNumericalConstraints (proj) || hasDiffFunc;
- hasDiffFunc = node ()->insertNumericalConstraintsForPath (proj) || hasDiffFunc;
- if (hasDiffFunc)
- constraint->addConstraint (proj);
-
- g->insertLockedDofs (constraint);
- insertLockedDofs (constraint);
- node ()->insertLockedDofs (constraint);
+ g->insertNumericalConstraints (proj);
+ insertNumericalConstraints (proj);
+ node ()->insertNumericalConstraintsForPath (proj);
+ constraint->addConstraint (proj);
+
+ g->insertLockedJoints (proj);
+ insertLockedJoints (proj);
+ node ()->insertLockedJoints (proj);
return constraint;
}
@@ -327,7 +323,6 @@ namespace hpp {
bool LevelSetEdge::applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const
{
-#if 0
// First, get an offset from the histogram that is not in the same connected component.
statistics::DiscreteDistribution < core::NodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
const Configuration_t& levelsetTarget = *(distrib ()->configuration ()),
@@ -338,23 +333,23 @@ namespace hpp {
assert (cp);
vector_t offset = cp->rightHandSideFromConfig (q_offset);
size_t row = 0, nbRows = 0;
- for (DifferentiableFunctions_t::const_iterator it =
- extraNumericalFunctions_.begin ();
- it != extraNumericalFunctions_.end (); ++it) {
- const core::DifferentiableFunction& f = *(it->first);
+ 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 (f.isParametric ()) {
+ if (!(*it)->comparisonType ()->constantRightHandSide ()) {
f (value, levelsetTarget);
+ offset.segment (row, nbRows) = value;
+ row += nbRows;
}
- offset.segment (row, nbRows) = value;
- row += nbRows;
}
cp->rightHandSide (offset);
- for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
- it != extraLockedDofs_.end (); ++it) {
- (*it)->valueFromFromConfig (levelsetTarget);
+ for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
+ it != extraLockedJoints_.end (); ++it) {
+ (*it)->rightHandSideFromConfig (levelsetTarget);
}
// Eventually, do the projection.
@@ -368,7 +363,6 @@ namespace hpp {
<< (double)(ss.nbSuccess ()) / ss.numberOfObservations ()
<< ".");
}
-#endif
return false;
}
@@ -394,18 +388,17 @@ namespace hpp {
/// The order is important here for the offset.
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
- if (!extraNumericalFunctions_.empty ()) {
- ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
- it != extraNumericalFunctions_.end (); ++it) {
- proj->addFunction (it->first);
- }
- constraint->addConstraint (proj);
+ ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+ for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
+ it != extraNumericalConstraints_.end (); ++it) {
+ proj->add (*it);
}
- for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
- it != extraLockedDofs_.end (); ++it)
- constraint->addConstraint (*it);
+ for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
+ it != extraLockedJoints_.end (); ++it)
+ proj->add (*it);
+
+ constraint->addConstraint (proj);
hist_ = graph::LeafHistogramPtr_t (new graph::LeafHistogram (constraint));
}
@@ -425,37 +418,41 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- bool hasDiffFunc = g->insertNumericalConstraints (proj);
- for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
- it != extraNumericalFunctions_.end (); ++it) {
- proj->addFunction (it->first, it->second);
+ g->insertNumericalConstraints (proj);
+ for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
+ it != extraNumericalConstraints_.end (); ++it) {
+ proj->add (*it);
}
- hasDiffFunc = !extraNumericalFunctions_.empty () || hasDiffFunc;
- hasDiffFunc = insertNumericalConstraints (proj) || hasDiffFunc;
- hasDiffFunc = to ()->insertNumericalConstraints (proj) || hasDiffFunc;
- if (hasDiffFunc)
- constraint->addConstraint (proj);
-
- g->insertLockedDofs (constraint);
- for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
- it != extraLockedDofs_.end (); ++it) {
- constraint->addConstraint (*it);
+ !extraNumericalConstraints_.empty ();
+ insertNumericalConstraints (proj);
+ to ()->insertNumericalConstraints (proj);
+ constraint->addConstraint (proj);
+
+ g->insertLockedJoints (proj);
+ for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
+ it != extraLockedJoints_.end (); ++it) {
+ proj->add (*it);
}
- insertLockedDofs (constraint);
- to ()->insertLockedDofs (constraint);
+ insertLockedJoints (proj);
+ to ()->insertLockedJoints (proj);
extraConstraints_->set (constraint);
}
return extraConstraints_->get ();
}
+ void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm)
+ {
+ extraNumericalConstraints_.push_back (nm);
+ }
+
void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
{
- extraNumericalFunctions_.push_back (DiffFuncAndIneqPair_t (function, ineq));
+ insertConfigConstraint (NumericalConstraint::create (function, ineq));
}
- void LevelSetEdge::insertConfigConstraint (const LockedJointPtr_t lockedDof)
+ void LevelSetEdge::insertConfigConstraint (const LockedJointPtr_t lockedJoint)
{
- extraLockedDofs_.push_back (lockedDof);
+ extraLockedJoints_.push_back (lockedJoint);
}
LevelSetEdge::LevelSetEdge (): extraConstraints_ (new Constraint_t()) {}
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index f9085f71..078f63d7 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -61,46 +61,46 @@ namespace hpp {
return os;
}
- void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t&)
+ void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm)
{
- assert (false);
+ numericalConstraints_.push_back(nm);
}
void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
{
- numericalConstraints_.push_back(DiffFuncAndIneqPair_t(function,ineq));
+ addNumericalConstraint (NumericalConstraint::create (function,ineq));
}
void GraphComponent::addLockedJointConstraint
(const LockedJointPtr_t& constraint)
{
- lockedDofConstraints_.push_back (constraint);
+ lockedJoints_.push_back (constraint);
}
bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
{
- for (DifferentiableFunctions_t::const_iterator it = numericalConstraints_.begin();
+ for (NumericalConstraints_t::const_iterator it = numericalConstraints_.begin();
it != numericalConstraints_.end(); ++it)
- proj->addFunction (it->first, it->second);
+ proj->add (*it);
return !numericalConstraints_.empty ();
}
- bool GraphComponent::insertLockedDofs (ConstraintSetPtr_t cs) const
+ bool GraphComponent::insertLockedJoints (ConfigProjectorPtr_t& cp) const
{
- for (LockedJoints_t::const_iterator it = lockedDofConstraints_.begin();
- it != lockedDofConstraints_.end(); ++it)
- cs->addConstraint (*it);
- return !lockedDofConstraints_.empty ();
+ for (LockedJoints_t::const_iterator it = lockedJoints_.begin();
+ it != lockedJoints_.end(); ++it)
+ cp->add (*it);
+ return !lockedJoints_.empty ();
}
- const DifferentiableFunctions_t& GraphComponent::numericalConstraints() const
+ const NumericalConstraints_t& GraphComponent::numericalConstraints() const
{
return numericalConstraints_;
}
- const LockedJoints_t& GraphComponent::lockedDofConstraints () const
+ const LockedJoints_t& GraphComponent::lockedJoints () const
{
- return lockedDofConstraints_;
+ return lockedJoints_;
}
void GraphComponent::parentGraph(const GraphWkPtr_t& parent)
diff --git a/src/graph/node.cc b/src/graph/node.cc
index bbdaf057..0e3ef411 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -95,14 +95,12 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
- // If at least one DifferentiableFunctionPtr_t was inserted, then add the projector.
- bool hasDiffFunc = g->insertNumericalConstraints (proj);
- hasDiffFunc = insertNumericalConstraints (proj) || hasDiffFunc;
- if (hasDiffFunc)
- constraint->addConstraint (proj);
+ g->insertNumericalConstraints (proj);
+ insertNumericalConstraints (proj);
+ constraint->addConstraint (proj);
- g->insertLockedDofs (constraint);
- insertLockedDofs (constraint);
+ g->insertLockedJoints (proj);
+ insertLockedJoints (proj);
configConstraints_->set (constraint);
}
return configConstraints_->get ();
diff --git a/src/path-projector.cc b/src/path-projector.cc
index 40a43443..d81d0b26 100644
--- a/src/path-projector.cc
+++ b/src/path-projector.cc
@@ -45,7 +45,7 @@ namespace hpp {
if (!sp) throw std::invalid_argument ("Unknow inherited class of Path");
success = impl_apply (sp, proj);
} else {
- PathVectorPtr_t res = PathVector::create (pv->outputSize());
+ PathVectorPtr_t res = PathVector::create (pv->outputSize (), pv->outputDerivativeSize ());
PathPtr_t part;
success = true;
for (size_t i = 0; i < pv->numberPaths (); i++) {
diff --git a/src/path-projector/dichotomy.cc b/src/path-projector/dichotomy.cc
index 5643d6c0..4cb27194 100644
--- a/src/path-projector/dichotomy.cc
+++ b/src/path-projector/dichotomy.cc
@@ -17,6 +17,7 @@
#include "hpp/manipulation/path-projector/dichotomy.hh"
#include <hpp/core/path-vector.hh>
+#include <hpp/core/config-projector.hh>
#include <queue>
#include <stack>
@@ -31,15 +32,16 @@ namespace hpp {
bool Dichotomy::impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const
{
ConstraintSetPtr_t constraints = path->constraints ();
+ const ConfigProjectorPtr_t& cp = constraints->configProjector ();
const StraightPath& sp = *path;
core::interval_t timeRange = sp.timeRange ();
const Configuration_t& q1 = sp(timeRange.first);
const Configuration_t& q2 = sp(timeRange.second);
- constraints->offsetFromConfig(q1);
+ if (cp) cp->rightHandSideFromConfig(q1);
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
- if (!constraints->configProjector ()) {
+ if (!cp) {
projection = path;
return true;
}
@@ -87,7 +89,7 @@ namespace hpp {
projection->constraints (constraints);
break;
default:
- core::PathVectorPtr_t pv = core::PathVector::create (sp.device ()->configSize ());
+ core::PathVectorPtr_t pv = core::PathVector::create (sp.outputSize (), sp.outputDerivativeSize ());
while (!paths.empty ()) {
paths.front ()->constraints (constraints);
pv->appendPath (paths.front ());
diff --git a/src/path-projector/progressive.cc b/src/path-projector/progressive.cc
index 906dbb7d..d046e392 100644
--- a/src/path-projector/progressive.cc
+++ b/src/path-projector/progressive.cc
@@ -17,6 +17,7 @@
#include "hpp/manipulation/path-projector/progressive.hh"
#include <hpp/core/path-vector.hh>
+#include <hpp/core/config-projector.hh>
#include <limits>
#include <queue>
@@ -32,15 +33,16 @@ namespace hpp {
bool Progressive::impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const
{
ConstraintSetPtr_t constraints = path->constraints ();
+ const ConfigProjectorPtr_t& cp = constraints->configProjector ();
const StraightPath& sp = *path;
core::interval_t timeRange = sp.timeRange ();
const Configuration_t& q1 = sp(timeRange.first);
const Configuration_t& q2 = sp(timeRange.second);
- constraints->offsetFromConfig(q1);
+ if (cp) cp->rightHandSideFromConfig(q1);
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
- if (!constraints->configProjector ()) {
+ if (!cp) {
projection = path;
return true;
}
@@ -95,7 +97,7 @@ namespace hpp {
projection->constraints (constraints);
break;
default:
- core::PathVectorPtr_t pv = core::PathVector::create (sp.device ()->configSize ());
+ core::PathVectorPtr_t pv = core::PathVector::create (sp.outputSize (), sp.outputDerivativeSize ());
qi = q1;
while (!paths.empty ()) {
assert ((qi - (*paths.front ())(paths.front ()->timeRange().first)).isZero ());
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index b0576ad2..864ae8d4 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -107,11 +107,10 @@ namespace hpp {
return constraintGraph_;
}
- LockedJointPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
+ LockedJointPtr_t ProblemSolver::lockedJoint (const std::string& name) const
{
- LockedDofConstraintMap_t::const_iterator it =
- lockedDofConstraintMap_.find (name);
- if (it == lockedDofConstraintMap_.end ()) {
+ LockedJointMap_t::const_iterator it = lockedJointMap_.find (name);
+ if (it == lockedJointMap_.end ()) {
throw std::runtime_error ("No LockedJoint constraint with this name");
}
return it->second;
--
GitLab