diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6a617439d26f9b8d76a3872460dcdca9d678e45d..dc97934dcd142d54021a0cc9a7f434e708e03c03 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -65,6 +65,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/device.hh
+ include/hpp/manipulation/constraint-set.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
include/hpp/manipulation/manipulation-planner.hh
diff --git a/include/hpp/manipulation/constraint-set.hh b/include/hpp/manipulation/constraint-set.hh
new file mode 100644
index 0000000000000000000000000000000000000000..96da5079fcbfaff960ff62f5da0c045cf1ce8610
--- /dev/null
+++ b/include/hpp/manipulation/constraint-set.hh
@@ -0,0 +1,70 @@
+// Copyright (c) 2015 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-manipulation
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_CONSTRAINT_SET_HH
+# define HPP_MANIPULATION_CONSTRAINT_SET_HH
+
+# include <hpp/core/constraint-set.hh>
+
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+# include <hpp/manipulation/config.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup constraints
+ /// \{
+
+ /// a core::ConstraintSet remembering which edge created it
+ class HPP_MANIPULATION_DLLAPI ConstraintSet : public core::ConstraintSet
+ {
+ public:
+ typedef core::ConstraintSet Parent_t;
+
+ /// Return shared pointer to new object
+ static ConstraintSetPtr_t create (const DevicePtr_t& robot,
+ const std::string& name);
+
+ /// Return shared pointer to new object
+ static ConstraintSetPtr_t createCopy (const ConstraintSetPtr_t& cs);
+
+ /// return shared pointer to copy
+ virtual ConstraintPtr_t copy () const;
+
+ void edge (graph::EdgePtr_t edge);
+
+ graph::EdgePtr_t edge () const;
+
+ protected:
+ /// Constructor
+ ConstraintSet (const DevicePtr_t& robot, const std::string& name);
+ /// Copy constructor
+ ConstraintSet (const ConstraintSet& other);
+ /// Store weak pointer to itself.
+ void init (const ConstraintSetPtr_t& self);
+
+ virtual std::ostream& print (std::ostream& os) const;
+
+ private:
+ graph::EdgePtr_t edge_;
+ ConstraintSetWkPtr_t weak_;
+ }; // class ConstraintSet
+ /// \}
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_CONSTRAINT_SET_HH
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index a6f2509bb45a5ae9ea75dd2676be6c1d6fb3e570..3eed3da21452d2662ab0a7c0cf64e3fe8293c679 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -86,8 +86,8 @@ namespace hpp {
typedef core::NumericalConstraintPtr_t NumericalConstraintPtr_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
- typedef core::ConstraintSet ConstraintSet;
- typedef core::ConstraintSetPtr_t ConstraintSetPtr_t;
+ HPP_PREDEF_CLASS (ConstraintSet);
+ typedef boost::shared_ptr <ConstraintSet> ConstraintSetPtr_t;
typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 9bb19e3dff07f8f5bb0f7e2bc646c584946fa4ee..ad53938862435b4584b60e673a26677de624b36c 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -217,7 +217,7 @@ namespace hpp {
{
}
/// Initialization of the object.
- void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
+ void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
/// Print the object in a stream.
@@ -228,6 +228,8 @@ namespace hpp {
Waypoint waypoint_;
mutable Configuration_t config_, result_;
+
+ WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
/// Edge that find intersection of level set.
@@ -263,7 +265,7 @@ namespace hpp {
protected:
/// Initialization of the object.
- void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
+ void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
LevelSetEdge (const std::string& name,
@@ -291,6 +293,8 @@ namespace hpp {
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
+
+ LevelSetEdgeWkPtr_t wkPtr_;
}; // class LevelSetEdge
/// \}
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index e696146c660d6740298a60a3e62f60a8749f07a5..100ee43c222b3790b70045f85dbe551c4c381f9d 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -52,8 +52,6 @@ namespace hpp {
typedef hpp::core::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ConfigProjector ConfigProjector;
typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
- typedef hpp::core::ConstraintSet ConstraintSet;
- typedef hpp::core::ConstraintSetPtr_t ConstraintSetPtr_t;
typedef hpp::core::Equality Equality;
typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index 25e14837b2a612cb062cf1336013c1706583eb0a..fea288a333138a09be36675fd480ba92b0ea7c35 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -20,7 +20,6 @@
# include <hpp/util/debug.hh>
-# include <hpp/core/constraint-set.hh>
# include <hpp/statistics/bin.hh>
# include "hpp/manipulation/config.hh"
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index fc2f7f9fa35b4107f7fc1c740540eda3db116f9c..679752a65527a04c7745b883c95cbe354b9d2db4 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,6 +25,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
manipulation-planner.cc
problem-solver.cc
roadmap.cc
+ constraint-set.cc
roadmap-node.cc
device.cc
graph-path-validation.cc
diff --git a/src/constraint-set.cc b/src/constraint-set.cc
new file mode 100644
index 0000000000000000000000000000000000000000..70d7a791ec675f460f05486558e573ae3b1bb742
--- /dev/null
+++ b/src/constraint-set.cc
@@ -0,0 +1,79 @@
+// Copyright (c) 2015, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hpp/manipulation/constraint-set.hh"
+
+#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/graph/edge.hh"
+
+namespace hpp {
+ namespace manipulation {
+ ConstraintSetPtr_t ConstraintSet::create
+ (const DevicePtr_t& robot, const std::string& name)
+ {
+ ConstraintSet* ptr = new ConstraintSet (robot, name);
+ ConstraintSetPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ ConstraintSetPtr_t ConstraintSet::createCopy (const ConstraintSetPtr_t& cs)
+ {
+ ConstraintSet* ptr = new ConstraintSet (*cs);
+ ConstraintSetPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ ConstraintPtr_t ConstraintSet::copy () const
+ {
+ return createCopy (weak_.lock ());
+ }
+
+ void ConstraintSet::edge (graph::EdgePtr_t edge)
+ {
+ edge_ = edge;
+ }
+
+ graph::EdgePtr_t ConstraintSet::edge () const
+ {
+ return edge_;
+ }
+
+ ConstraintSet::ConstraintSet (const DevicePtr_t& robot, const std::string& name) :
+ Parent_t (robot, name),
+ edge_ ()
+ {}
+
+ ConstraintSet::ConstraintSet (const ConstraintSet& other) :
+ Parent_t (other),
+ edge_ (other.edge ())
+ {}
+
+ void ConstraintSet::init (const ConstraintSetPtr_t& self)
+ {
+ Parent_t::init (self);
+ weak_ = self;
+ }
+
+ std::ostream& ConstraintSet::print (std::ostream& os) const
+ {
+ Parent_t::print (os);
+ if (edge_) os << "Built by edge " << edge_->name() << std::endl;
+ return os;
+ }
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index afdc6ca2b9134d6de3ef41482fb71556d7c3cac0..ddc519b1a6fa9a21c1f3a4283f8071e80b11f59a 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -27,6 +27,7 @@
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/graph/statistics.hh"
+#include "hpp/manipulation/constraint-set.hh"
namespace hpp {
namespace manipulation {
@@ -126,6 +127,8 @@ namespace hpp {
g->insertLockedJoints (proj);
insertLockedJoints (proj);
to ()->insertLockedJoints (proj);
+
+ constraint->edge (wkPtr_.lock ());
return constraint;
}
@@ -155,6 +158,8 @@ namespace hpp {
g->insertLockedJoints (proj);
insertLockedJoints (proj);
node ()->insertLockedJoints (proj);
+
+ constraint->edge (wkPtr_.lock ());
return constraint;
}
@@ -209,10 +214,11 @@ namespace hpp {
return shPtr;
}
- void WaypointEdge::init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
+ void WaypointEdge::init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to)
{
Edge::init (weak, graph, from, to);
+ wkPtr_ = weak;
}
bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
@@ -396,10 +402,11 @@ namespace hpp {
return false;
}
- void LevelSetEdge::init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
+ void LevelSetEdge::init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to)
{
Edge::init (weak, graph, from, to);
+ wkPtr_ = weak;
}
LevelSetEdgePtr_t LevelSetEdge::create
@@ -433,6 +440,7 @@ namespace hpp {
proj->add (*it);
constraint->addConstraint (proj);
+ constraint->edge (wkPtr_.lock ());
hist_ = graph::LeafHistogramPtr_t (new graph::LeafHistogram (constraint));
}
@@ -470,6 +478,8 @@ namespace hpp {
}
insertLockedJoints (proj);
to ()->insertLockedJoints (proj);
+
+ constraint->edge (wkPtr_.lock ());
extraConstraints_->set (constraint);
}
return extraConstraints_->get ();
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 6ea4b14a9a27212204ee932507666e0d22791781..db6fa6e16276bab2478bd20ab2c8fa71bf2debcd 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -18,6 +18,7 @@
#include <hpp/util/assertion.hh>
+#include <hpp/manipulation/constraint-set.hh>
#include "hpp/manipulation/graph/node-selector.hh"
#include "hpp/manipulation/graph/node.hh"
#include "hpp/manipulation/graph/edge.hh"
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 9c9f90847299130a1cbd41a196a3e8519e65c806..6a1e3d855fe0b267351aa6a7d651d73d5adb1d37 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -21,6 +21,7 @@
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/constraint-set.hh"
namespace hpp {
namespace manipulation {
@@ -112,9 +113,9 @@ namespace hpp {
if (!*configConstraints_) {
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
- ConstraintSetPtr_t constraint = ConstraintSet::create ((const model::DevicePtr_t&)g->robot (), "Set " + n);
+ ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
- ConfigProjectorPtr_t proj = ConfigProjector::create((const model::DevicePtr_t&)g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
+ ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
constraint->addConstraint (proj);
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 8df01ea1830154c935121b10ccc624428a548ede..3bdde7c1621a54e0fbb516507685d75801c788e0 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -16,6 +16,8 @@
#include "hpp/manipulation/graph/statistics.hh"
+#include "hpp/manipulation/constraint-set.hh"
+
namespace hpp {
namespace manipulation {
namespace graph {
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 95be01f8c294eee02b522e8d4a08c0ca9932bc11..56b98429058705a74b31c9eae5b90bbcf8e5dd95 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -31,6 +31,7 @@
#include "hpp/manipulation/manipulation-planner.hh"
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/roadmap.hh"
+#include "hpp/manipulation/constraint-set.hh"
#include "hpp/manipulation/graph-path-validation.hh"
namespace hpp {