diff --git a/CMakeLists.txt b/CMakeLists.txt
index 31ad63123cd7ec377642c3d401bcd3416a269f22..a054a8d2c8dcf440ddb74c30623317f052f11f5d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,6 +64,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/device.hh
+ include/hpp/manipulation/weighed-distance.hh
include/hpp/manipulation/constraint-set.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index c9d1de0a240bf9c06f134b9966e83f22aaa2ee9c..9251a26b2c5b35b63d63f66b4ef80c02fbb1ce19 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -55,6 +55,8 @@ namespace hpp {
typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS (RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
+ HPP_PREDEF_CLASS (WeighedDistance);
+ typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index 6c3dfc9da18294dbbb15eba1573103a90fb7c58a..92642de15f2670a7c3799592a1c25a2091735cad 100644
--- a/include/hpp/manipulation/graph-path-validation.hh
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -142,9 +142,11 @@ namespace hpp {
private:
/// Do validation regarding the constraint graph for PathVector
- bool impl_validate (const PathVectorPtr_t& path, bool reverse, PathPtr_t& validPart);
+ bool impl_validate (const PathVectorPtr_t& path, bool reverse,
+ PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// Do validation regarding the constraint graph for Path
- bool impl_validate (const PathPtr_t& path, bool reverse, PathPtr_t& validPart);
+ bool impl_validate (const PathPtr_t& path, bool reverse,
+ PathPtr_t& validPart, PathValidationReportPtr_t& report);
/// The encapsulated PathValidation.
PathValidationPtr_t pathValidation_;
/// Pointer to the constraint graph.
diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh
index eb5a132b3201e8fc7d05b9838749eb267f768596..22e8a6d206373e365db2a27b8cfa832055b8da19 100644
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ b/include/hpp/manipulation/graph-steering-method.hh
@@ -20,12 +20,14 @@
# include <hpp/manipulation/config.hh>
+# include "hpp/core/problem-solver.hh" // SteeringMethodBuilder_t
# include <hpp/core/steering-method.hh>
# include <hpp/core/weighed-distance.hh>
# include <hpp/model/device.hh>
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
+# include "hpp/manipulation/problem.hh"
namespace hpp {
namespace manipulation {
@@ -36,61 +38,73 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod
{
+ typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
+
public:
- /// Create instance and return shared pointer
- static GraphSteeringMethodPtr_t create (const model::DevicePtr_t& robot);
-
- /// Create copy and return shared pointer
- static GraphSteeringMethodPtr_t createCopy
- (const GraphSteeringMethodPtr_t& other);
- /// Copy instance and return shared pointer
- virtual core::SteeringMethodPtr_t copy () const
- {
- return createCopy (weak_.lock ());
- }
- /// \name Graph of constraints applicable to the robot.
- /// \{
-
- /// Set constraint graph
- void constraintGraph (const graph::GraphPtr_t& graph)
+ /// Create instance and return shared pointer
+ /// \warning core::ProblemPtr_t will be casted to ProblemPtr_t
+ static GraphSteeringMethodPtr_t create
+ (const core::ProblemPtr_t& problem);
+
+ template <typename T>
+ static GraphSteeringMethodPtr_t create
+ (const core::ProblemPtr_t& problem);
+
+ /// Create instance and return shared pointer
+ static GraphSteeringMethodPtr_t create (const ProblemPtr_t& problem);
+
+ /// Create copy and return shared pointer
+ static GraphSteeringMethodPtr_t createCopy
+ (const GraphSteeringMethodPtr_t& other);
+
+ /// Copy instance and return shared pointer
+ virtual core::SteeringMethodPtr_t copy () const
{
- graph_ = graph;
+ return createCopy (weak_.lock ());
}
- /// Get constraint graph
- const graph::GraphPtr_t& constraintGraph () const
+ const core::SteeringMethodPtr_t& innerSteeringMethod () const
{
- return graph_;
+ return steeringMethod_;
}
- /// \}
- const core::WeighedDistancePtr_t& distance () const;
+ void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
+ {
+ steeringMethod_ = sm;
+ }
protected:
/// Constructor
- GraphSteeringMethod (const model::DevicePtr_t& robot);
+ GraphSteeringMethod (const ProblemPtr_t& problem);
/// Copy constructor
GraphSteeringMethod (const GraphSteeringMethod&);
virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
- void init (GraphSteeringMethodWkPtr_t weak)
- {
- core::SteeringMethod::init (weak);
- weak_ = weak;
- }
+ void init (GraphSteeringMethodWkPtr_t weak)
+ {
+ core::SteeringMethod::init (weak);
+ weak_ = weak;
+ }
private:
- /// A pointer to the graph of constraint.
- graph::GraphPtr_t graph_;
- /// Pointer to the Robot.
- core::DeviceWkPtr_t robot_;
- /// Metric in configuration space.
- core::WeighedDistancePtr_t distance_;
- /// Weak pointer to itself
- GraphSteeringMethodWkPtr_t weak_;
+ /// A pointer to the problem
+ ProblemPtr_t problem_;
+ /// Weak pointer to itself
+ GraphSteeringMethodWkPtr_t weak_;
+ /// The encapsulated steering method
+ core::SteeringMethodPtr_t steeringMethod_;
};
+
+ template <typename T>
+ GraphSteeringMethodPtr_t GraphSteeringMethod::create
+ (const core::ProblemPtr_t& problem)
+ {
+ GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem);
+ gsm->innerSteeringMethod (T::create (problem));
+ return gsm;
+ }
/// \}
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 65098c2a962a1e0794f9a3e2a36b4fd881aea62d..4b5f32867a8e20d01064887f135d3ba5e49ead3f 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -18,7 +18,7 @@
# define HPP_MANIPULATION_GRAPH_EDGE_HH
#include <hpp/core/constraint-set.hh>
-#include <hpp/core/weighed-distance.hh>
+#include <hpp/core/steering-method.hh>
#include <hpp/core/path.hh>
#include "hpp/manipulation/config.hh"
@@ -65,7 +65,6 @@ namespace hpp {
/// Create a new empty Edge.
static EdgePtr_t create
(const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph,
const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
@@ -74,7 +73,10 @@ namespace hpp {
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
- virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
+ virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const;
/// Get the destination
NodePtr_t to () const;
@@ -97,7 +99,7 @@ namespace hpp {
/// Get steering method associated to the edge.
const core::SteeringMethodPtr_t& steeringMethod () const
{
- return steeringMethod_;
+ return steeringMethod_->get();
}
/// Get direction of the path compare to the edge
@@ -126,8 +128,7 @@ namespace hpp {
const NodeWkPtr_t& to);
/// Constructor
- Edge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod);
+ Edge (const std::string& name);
/// Constraint to project a path.
/// \return The initialized constraint.
@@ -144,6 +145,7 @@ namespace hpp {
private:
typedef Cache < ConstraintSetPtr_t > Constraint_t;
+ typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
/// See pathConstraint member function.
Constraint_t* pathConstraints_;
@@ -159,7 +161,7 @@ namespace hpp {
bool isInNodeFrom_;
/// Steering method used to create paths associated to the edge
- core::SteeringMethodPtr_t steeringMethod_;
+ SteeringMethod_t* steeringMethod_;
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
@@ -199,13 +201,14 @@ namespace hpp {
/// Create a new WaypointEdge.
static WaypointEdgePtr_t create
(const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
virtual bool direction (const core::PathPtr_t& path) const;
- virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
+ virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
@@ -229,9 +232,8 @@ namespace hpp {
NodePtr_t node () const;
protected:
- WaypointEdge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod) :
- Edge (name, steeringMethod)
+ WaypointEdge (const std::string& name) :
+ Edge (name)
{
}
/// Initialization of the object.
@@ -259,7 +261,6 @@ namespace hpp {
/// Create a new LevelSetEdge.
static LevelSetEdgePtr_t create
(const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
@@ -287,8 +288,7 @@ namespace hpp {
void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
- LevelSetEdge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod);
+ LevelSetEdge (const std::string& name);
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 3fb85bbefd7ea7f602876d023b50da494c09e274..86f1d0b5bd125a3598bac57e05a7b43e20247adb 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -45,9 +45,9 @@ namespace hpp {
/// Create a new Graph.
///
/// \param robot a manipulation robot
- /// \param sm a steering method to create paths from edges
+ /// \param problem a pointer to the problem
static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
- const core::SteeringMethodPtr_t& sm);
+ const ProblemPtr_t& problem);
/// Create and insert a NodeSelector inside the graph.
NodeSelectorPtr_t createNodeSelector (const std::string& name);
@@ -119,7 +119,7 @@ namespace hpp {
const DevicePtr_t& robot () const;
/// Get the steering Method
- const core::SteeringMethodPtr_t& steeringMethod () const;
+ const ProblemPtr_t& problem () const;
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -130,8 +130,8 @@ namespace hpp {
/// Constructor
/// \param sm a steering method to create paths from edges
- Graph (const std::string& name, const core::SteeringMethodPtr_t& sm) :
- GraphComponent (name), steeringMethod_ (sm)
+ Graph (const std::string& name, const ProblemPtr_t& problem) :
+ GraphComponent (name), problem_ (problem)
{}
/// Print the object in a stream.
@@ -160,7 +160,7 @@ namespace hpp {
typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
- core::SteeringMethodPtr_t steeringMethod_;
+ ProblemPtr_t problem_;
value_type errorThreshold_;
size_type maxIterations_;
}; // Class Graph
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index c59d0280724d528d9a04ff722290ef954691d5e5..fb78a4caca5d31ee7dfa0e82ea3b8d779cc6c3f0 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -45,7 +45,6 @@ namespace hpp {
public:
typedef boost::function < EdgePtr_t
(const std::string&,
- const core::SteeringMethodPtr_t&,
const GraphWkPtr_t&,
const NodeWkPtr_t&, const NodeWkPtr_t&) >
EdgeFactory;
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index d3acc285331ebadc8baf8e6ee6bceec7e35de811..429b7059f22358cdccf59b56d15ff7e7de06dca6 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -46,12 +46,12 @@ namespace hpp {
/// One step of extension.
///
- /// A set of constraints is choosen using the graph of constraints.
- /// A constraint extension is done using a choosen set.
+ /// A set of constraints is chosen using the graph of constraints.
+ /// A constraint extension is done using a chosen set.
///
virtual void oneStep ();
- /// Extend a the configuration q_near toward q_rand.
+ /// Extend configuration q_near toward q_rand.
/// \param q_near the configuration to be extended.
/// \param q_rand the configuration toward extension is performed.
/// \retval validPath the longest valid path (possibly of length 0),
@@ -69,8 +69,12 @@ namespace hpp {
void init (const ManipulationPlannerWkPtr_t& weak);
private:
- /// Try to connect configurations in a list.
- void tryConnect (const core::Nodes_t nodes);
+ /// Try to connect nodes of the roadmap to other connected components.
+ /// \return the number of connection made.
+ std::size_t tryConnectToRoadmap (const core::Nodes_t nodes);
+ /// Try to connect nodes in a list between themselves.
+ /// \return the number of connection made.
+ std::size_t tryConnectNewNodes (const core::Nodes_t nodes);
/// Configuration shooter
ConfigurationShooterPtr_t shooter_;
@@ -124,6 +128,8 @@ namespace hpp {
void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge);
+ const value_type extendStep_;
+
mutable Configuration_t qProj_;
};
/// \}
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 3860f3afbf672ee7df7ad61d2cb59a52cff7e065..f7384192ae5d18935f89a34d44737c86acd3c0c2 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -19,11 +19,10 @@
# include <hpp/core/problem.hh>
+# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/device.hh"
# include "hpp/manipulation/graph/graph.hh"
# include "hpp/manipulation/graph-path-validation.hh"
-# include "hpp/manipulation/graph-steering-method.hh"
-# include "hpp/manipulation/fwd.hh"
namespace hpp {
namespace manipulation {
@@ -36,20 +35,10 @@ namespace hpp {
typedef core::Problem Parent;
/// Constructor
- Problem (DevicePtr_t robot) : Parent (robot),
- graph_(), steeringMethod_ (GraphSteeringMethod::create (robot))
- {
- Parent::steeringMethod (steeringMethod_);
- }
+ Problem (DevicePtr_t robot);
/// Set the graph of constraints
- void constraintGraph (const graph::GraphPtr_t& graph)
- {
- graph_ = graph;
- steeringMethod_->constraintGraph (graph);
- if (pathValidation ())
- pathValidation ()->constraintGraph (graph);
- }
+ void constraintGraph (const graph::GraphPtr_t& graph);
/// Get the graph of constraints
graph::GraphPtr_t constraintGraph () const
@@ -70,22 +59,14 @@ namespace hpp {
}
/// Get the path validation as a GraphPathValidation
- GraphPathValidationPtr_t pathValidation () const
- {
- return HPP_DYNAMIC_PTR_CAST (GraphPathValidation, Parent::pathValidation());
- }
+ GraphPathValidationPtr_t pathValidation () const;
/// Get the steering method as a GraphSteeringMethod
- GraphSteeringMethodPtr_t steeringMethod () const
- {
- return steeringMethod_;
- }
+ GraphSteeringMethodPtr_t steeringMethod () const;
private:
/// The graph of constraints
graph::GraphPtr_t graph_;
- /// Steering method
- GraphSteeringMethodPtr_t steeringMethod_;
}; // class Problem
/// \}
} // namespace manipulation
diff --git a/include/hpp/manipulation/weighed-distance.hh b/include/hpp/manipulation/weighed-distance.hh
new file mode 100644
index 0000000000000000000000000000000000000000..472a6d57540c81538f1af640acd3d22e53541871
--- /dev/null
+++ b/include/hpp/manipulation/weighed-distance.hh
@@ -0,0 +1,75 @@
+// 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_DISTANCE_HH
+# define HPP_MANIPULATION_DISTANCE_HH
+
+# include <hpp/core/weighed-distance.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+# include <hpp/manipulation/config.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup steering_method
+ /// \{
+
+ /// Class for distance between configurations
+ class HPP_MANIPULATION_DLLAPI WeighedDistance : public core::WeighedDistance
+ {
+ public:
+ static WeighedDistancePtr_t create (const DevicePtr_t& robot,
+ const graph::GraphPtr_t& graph);
+
+ static WeighedDistancePtr_t createCopy
+ (const WeighedDistancePtr_t& distance);
+
+ virtual core::DistancePtr_t clone () const;
+
+ /// Set the graph of constraints
+ void constraintGraph (const graph::GraphPtr_t& graph)
+ {
+ graph_ = graph;
+ }
+
+ /// Get the graph of constraints
+ graph::GraphPtr_t constraintGraph () const
+ {
+ return graph_;
+ }
+
+ protected:
+ WeighedDistance (const DevicePtr_t& robot, const graph::GraphPtr_t graph);
+
+ WeighedDistance (const WeighedDistance& distance);
+
+ /// Derived class should implement this function
+ virtual value_type impl_distance (
+ ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+ virtual value_type impl_distance (
+ core::NodePtr_t n1, core::NodePtr_t n2) const;
+
+ void init (WeighedDistanceWkPtr_t self);
+
+ private:
+ graph::GraphPtr_t graph_;
+ WeighedDistanceWkPtr_t weak_;
+ }; // class Distance
+ /// \}
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_DISTANCE_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b15985e242af674bee8d3e497671ee1edfee77fb..f5920d83a9f4aeecdc332ef17b73f18e7ec52cb3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -28,6 +28,8 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
constraint-set.cc
roadmap-node.cc
device.cc
+ weighed-distance.cc
+ problem.cc
graph-path-validation.cc
graph-steering-method.cc
graph-optimizer.cc
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index 292dff7d9804f4aef07c1cb0a39423ba9fe1c487..46f24dc023f32a3096fc605aefacb34587ffa6a6 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -32,7 +32,8 @@ namespace hpp {
const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
{
assert (path);
- bool success = impl_validate (path, reverse, validPart);
+ PathValidationReportPtr_t report;
+ bool success = impl_validate (path, reverse, validPart, report);
assert (constraintGraph_);
assert (constraintGraph_->getNode (validPart->initial ()));
assert (constraintGraph_->getNode (validPart->end ()));
@@ -44,19 +45,20 @@ namespace hpp {
ValidationReport&)
{
assert (path);
- return impl_validate (path, reverse, validPart);
+ PathValidationReportPtr_t report;
+ return impl_validate (path, reverse, validPart, report);
}
bool GraphPathValidation::validate (const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
- PathValidationReportPtr_t&)
+ PathValidationReportPtr_t& report)
{
assert (path);
- return impl_validate (path, reverse, validPart);
+ return impl_validate (path, reverse, validPart, report);
}
- bool GraphPathValidation::impl_validate (
- const PathVectorPtr_t& path, bool reverse, PathPtr_t& validPart)
+ bool GraphPathValidation::impl_validate (const PathVectorPtr_t& path,
+ bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
{
PathPtr_t validSubPart;
if (reverse) {
@@ -64,7 +66,7 @@ namespace hpp {
assert (!reverse && "This has never been tested with reverse path");
for (long int i = path->numberPaths () - 1; i >= 0; i--) {
// We should stop at the first non valid subpath.
- if (!impl_validate (path->pathAtRank (i), true, validSubPart)) {
+ if (!impl_validate (path->pathAtRank (i), true, validSubPart, report)) {
PathVectorPtr_t p = PathVector::create
(path->outputSize(), path->outputDerivativeSize());
for (long int v = path->numberPaths () - 1; v > i; v--)
@@ -78,7 +80,7 @@ namespace hpp {
} else {
for (size_t i = 0; i != path->numberPaths (); i++) {
// We should stop at the first non valid subpath.
- if (!impl_validate (path->pathAtRank (i), false, validSubPart)) {
+ if (!impl_validate (path->pathAtRank (i), false, validSubPart, report)) {
PathVectorPtr_t p = PathVector::create
(path->outputSize(), path->outputDerivativeSize());
for (size_t v = 0; v < i; v++)
@@ -95,15 +97,14 @@ namespace hpp {
return true;
}
- bool GraphPathValidation::impl_validate (
- const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
+ bool GraphPathValidation::impl_validate (const PathPtr_t& path,
+ bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
{
PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path);
if (pathVector)
- return impl_validate (pathVector, reverse, validPart);
+ return impl_validate (pathVector, reverse, validPart, report);
PathPtr_t pathNoCollision;
- PathValidationReportPtr_t report;
if (pathValidation_->validate (path, reverse, pathNoCollision, report)) {
validPart = path;
return true;
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 10f18719b287d18044f12201de1ac1049fc10100..381d301a69b2866fcb128ae008dd4d994d546023 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -16,6 +16,9 @@
#include "hpp/manipulation/graph-steering-method.hh"
+#include <hpp/util/pointer.hh>
+
+#include <hpp/core/distance.hh>
#include <hpp/core/straight-path.hh>
#include "hpp/manipulation/graph/graph.hh"
@@ -24,9 +27,18 @@
namespace hpp {
namespace manipulation {
GraphSteeringMethodPtr_t GraphSteeringMethod::create
- (const model::DevicePtr_t& robot)
+ (const core::ProblemPtr_t& problem)
+ {
+ assert (dynamic_cast <const ProblemPtr_t> (problem) != NULL
+ && "Cast to const ProblemPtr_t failed");
+ const ProblemPtr_t& p = static_cast <const ProblemPtr_t> (problem);
+ return create (p);
+ }
+
+ GraphSteeringMethodPtr_t GraphSteeringMethod::create
+ (const ProblemPtr_t& problem)
{
- GraphSteeringMethod* ptr = new GraphSteeringMethod (robot);
+ GraphSteeringMethod* ptr = new GraphSteeringMethod (problem);
GraphSteeringMethodPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
@@ -41,40 +53,34 @@ namespace hpp {
return shPtr;
}
- GraphSteeringMethod::GraphSteeringMethod (const model::DevicePtr_t& robot) :
- SteeringMethod (), graph_ (), robot_ (robot),
- distance_ (core::WeighedDistance::create (robot)), weak_ ()
+ GraphSteeringMethod::GraphSteeringMethod (const ProblemPtr_t& problem) :
+ SteeringMethod (problem), problem_ (problem), weak_ ()
{
}
GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other):
- SteeringMethod (other), graph_ (other.graph_), robot_ (other.robot_),
- distance_ (other.distance_)
+ SteeringMethod (other), problem_ (other.problem_)
{
}
PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
graph::Edges_t possibleEdges;
+ graph::Graph& graph = *problem_->constraintGraph ();
try {
- possibleEdges = graph_->getEdges (graph_->getNode (q1), graph_->getNode (q2));
+ possibleEdges = graph.getEdges (graph.getNode (q1), graph.getNode (q2));
} catch (const std::logic_error& e) {
hppDout (error, e.what ());
return PathPtr_t ();
}
PathPtr_t path;
while (!possibleEdges.empty()) {
- if (possibleEdges.back ()->build (path, q1, q2, *distance_)) {
+ if (possibleEdges.back ()->build (path, q1, q2)) {
return path;
}
possibleEdges.pop_back ();
}
return PathPtr_t ();
}
-
- const core::WeighedDistancePtr_t& GraphSteeringMethod::distance () const
- {
- return distance_;
- }
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 84c6e4b874bd16bcb10e292b6d4ad53ef8ecf88a..1c2089f94d6c27c18424205f52b4539407ee7a15 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -18,7 +18,6 @@
#include <sstream>
-#include <hpp/core/steering-method.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/constraints/differentiable-function.hh>
@@ -26,24 +25,26 @@
#include <hpp/util/pointer.hh>
#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/graph/statistics.hh"
#include "hpp/manipulation/constraint-set.hh"
namespace hpp {
namespace manipulation {
namespace graph {
- Edge::Edge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod) :
+ Edge::Edge (const std::string& name) :
GraphComponent (name), isShort_ (false),
pathConstraints_ (new Constraint_t()),
configConstraints_ (new Constraint_t()),
- steeringMethod_ (steeringMethod->copy ())
+ steeringMethod_ (new SteeringMethod_t())
{}
Edge::~Edge ()
{
if (pathConstraints_ ) delete pathConstraints_;
if (configConstraints_) delete configConstraints_;
+ if (steeringMethod_ ) delete steeringMethod_;
}
NodePtr_t Edge::to () const
@@ -151,11 +152,10 @@ namespace hpp {
}
EdgePtr_t Edge::create (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph,
const NodeWkPtr_t& from, const NodeWkPtr_t& to)
{
- Edge* ptr = new Edge (name, steeringMethod);
+ Edge* ptr = new Edge (name);
EdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -226,7 +226,6 @@ namespace hpp {
if (!*pathConstraints_) {
ConstraintSetPtr_t pathConstraints (buildPathConstraint ());
pathConstraints_->set (pathConstraints);
- steeringMethod_->constraints (pathConstraints);
}
return pathConstraints_->get ();
}
@@ -249,11 +248,27 @@ namespace hpp {
node ()->insertLockedJoints (proj);
constraint->edge (wkPtr_.lock ());
+
+ // Build steering method
+ steeringMethod_->set(g->problem()->steeringMethod()
+ ->innerSteeringMethod()->copy());
+ steeringMethod_->get()->constraints (constraint);
return constraint;
}
+ bool Edge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2)
+ const
+ {
+ ConstraintSetPtr_t constraints = pathConstraint ();
+ constraints->configProjector ()->rightHandSideFromConfig(q1);
+ if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
+ return false;
+ }
+ return true;
+ }
+
bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
- ConfigurationIn_t q2, const core::WeighedDistance&)
+ ConfigurationIn_t q2)
const
{
ConstraintSetPtr_t constraints = pathConstraint ();
@@ -261,7 +276,7 @@ namespace hpp {
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
- path = (*steeringMethod_) (q1, q2);
+ path = (*steeringMethod_->get()) (q1, q2);
return true;
}
@@ -290,12 +305,11 @@ namespace hpp {
return false;
}
- WaypointEdgePtr_t WaypointEdge::create
- (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod,
+ WaypointEdgePtr_t WaypointEdge::create (const std::string& name,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to)
{
- WaypointEdge* ptr = new WaypointEdge (name, steeringMethod);
+ WaypointEdge* ptr = new WaypointEdge (name);
WaypointEdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -308,7 +322,13 @@ namespace hpp {
wkPtr_ = weak;
}
- bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
+ bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+ {
+ return waypoint_.first->canConnect (q1, q2) && Edge::canConnect (q1, q2);
+ }
+
+ bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const
{
assert (waypoint_.first);
core::PathPtr_t pathToWaypoint;
@@ -317,7 +337,7 @@ namespace hpp {
if (!result_.isApprox (q2)) config_ = q2;
if (!waypoint_.first->applyConstraints (q1, config_))
return false;
- if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
+ if (!waypoint_.first->build (pathToWaypoint, q1, config_))
return false;
core::PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (core::PathVector, pathToWaypoint);
if (!pv) {
@@ -329,7 +349,7 @@ namespace hpp {
path = pv;
core::PathPtr_t end;
- if (!Edge::build (end, config_, q2, d))
+ if (!Edge::build (end, config_, q2))
return false;
pv->appendPath (end);
return true;
@@ -358,12 +378,12 @@ namespace hpp {
ss.str (std::string ()); ss.clear ();
ss << bname << "_e" << d;
if (d == 0) {
- edge = Edge::create (ss.str (), steeringMethod (), graph_, from (),
+ edge = Edge::create (ss.str (), graph_, from (),
node);
edge->isInNodeFrom (isInNodeFrom ());
} else {
WaypointEdgePtr_t we = WaypointEdge::create
- (ss.str (), steeringMethod (), graph_, from (), node);
+ (ss.str (), graph_, from (), node);
we->createWaypoint (d-1, bname);
edge = we;
edge->isInNodeFrom (isInNodeFrom ());
@@ -499,11 +519,10 @@ namespace hpp {
}
LevelSetEdgePtr_t LevelSetEdge::create
- (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod,
- const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to)
+ (const std::string& name, const GraphWkPtr_t& graph,
+ const NodeWkPtr_t& from, const NodeWkPtr_t& to)
{
- LevelSetEdge* ptr = new LevelSetEdge (name, steeringMethod);
+ LevelSetEdge* ptr = new LevelSetEdge (name);
LevelSetEdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -592,9 +611,8 @@ namespace hpp {
}
LevelSetEdge::LevelSetEdge
- (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod) :
- Edge (name, steeringMethod), extraConstraints_ (new Constraint_t())
+ (const std::string& name) :
+ Edge (name), extraConstraints_ (new Constraint_t())
{
}
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index db6fa6e16276bab2478bd20ab2c8fa71bf2debcd..fc8385ec35615090cfc8401ac919bf15f6919e5b 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -27,9 +27,9 @@ namespace hpp {
namespace manipulation {
namespace graph {
GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
- const core::SteeringMethodPtr_t& sm)
+ const ProblemPtr_t& problem)
{
- Graph* ptr = new Graph (name, sm);
+ Graph* ptr = new Graph (name, problem);
GraphPtr_t shPtr (ptr);
ptr->init (shPtr, robot);
return shPtr;
@@ -80,9 +80,9 @@ namespace hpp {
return robot_;
}
- const core::SteeringMethodPtr_t& Graph::steeringMethod () const
+ const ProblemPtr_t& Graph::problem () const
{
- return steeringMethod_;
+ return problem_;
}
NodePtr_t Graph::getNode (ConfigurationIn_t config) const
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 6a1e3d855fe0b267351aa6a7d651d73d5adb1d37..598a43a755e9cf293eb8a25e79493a44ac5dbce2 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -53,8 +53,7 @@ namespace hpp {
const Weight_t& w, const bool& isInNodeFrom,
EdgeFactory create)
{
- EdgePtr_t newEdge = create(name, graph_.lock ()->steeringMethod (),
- graph_, wkPtr_, to);
+ EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
neighbors_.insert (newEdge, w);
newEdge->isInNodeFrom (isInNodeFrom);
return newEdge;
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 1057f65469655aa5d8fc394a1a385345f541c839..fcb8dbbcba421315ce66223a3c0532620e907a26 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -26,6 +26,7 @@
#include <hpp/core/connected-component.hh>
#include <hpp/core/path-projector.hh>
#include <hpp/core/projection-error.hh>
+#include <hpp/core/nearest-neighbor.hh>
#include "hpp/manipulation/graph/statistics.hh"
#include "hpp/manipulation/device.hh"
@@ -154,12 +155,18 @@ namespace hpp {
HPP_STOP_TIMECOUNTER(delayedEdges);
// Try to connect the new nodes together
- HPP_START_TIMECOUNTER(tryConnect);
- tryConnect (newNodes);
- HPP_STOP_TIMECOUNTER(tryConnect);
+ HPP_START_TIMECOUNTER(tryConnectNewNodes);
+ const std::size_t nbConn = tryConnectNewNodes (newNodes);
+ HPP_STOP_TIMECOUNTER(tryConnectNewNodes);
+ HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectNewNodes);
+ if (nbConn == 0) {
+ HPP_START_TIMECOUNTER(tryConnectToRoadmap);
+ tryConnectToRoadmap (newNodes);
+ HPP_STOP_TIMECOUNTER(tryConnectToRoadmap);
+ HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectToRoadmap);
+ }
HPP_STOP_TIMECOUNTER(oneStep);
HPP_DISPLAY_LAST_TIMECOUNTER(oneStep);
- HPP_DISPLAY_LAST_TIMECOUNTER(tryConnect);
HPP_DISPLAY_TIMECOUNTER(oneStep);
HPP_DISPLAY_TIMECOUNTER(extend);
HPP_DISPLAY_TIMECOUNTER(tryConnect);
@@ -195,10 +202,9 @@ namespace hpp {
return false;
}
HPP_STOP_TIMECOUNTER (applyConstraints);
- GraphSteeringMethodPtr_t sm = problem_.steeringMethod();
core::PathPtr_t path;
HPP_START_TIMECOUNTER (buildPath);
- if (!edge->build (path, *q_near, qProj_, *(sm->distance ()))) {
+ if (!edge->build (path, *q_near, qProj_)) {
HPP_STOP_TIMECOUNTER (buildPath);
addFailure (STEERING_METHOD, edge);
return false;
@@ -235,12 +241,18 @@ namespace hpp {
addFailure (PATH_VALIDATION, edge);
validPath = fullValidPath;
} else {
- if (fullyValid) validPath = fullValidPath;
+ if (extendStep_ == 1 || fullyValid) validPath = fullValidPath;
else {
const value_type& length = fullValidPath->length();
const value_type& t_init = fullValidPath->timeRange ().first;
- validPath = fullValidPath->extract
- (core::interval_t(t_init, t_init + length * 0.5));
+ try {
+ validPath = fullValidPath->extract
+ (core::interval_t(t_init, t_init + length * extendStep_));
+ } catch (const core::projection_error& e) {
+ hppDout (error, e.what());
+ addFailure (PATH_PROJECTION_SHORTER, edge);
+ return false;
+ }
}
extendStatistics_.addSuccess ();
hppDout (info, "Extension:" << std::endl
@@ -268,18 +280,30 @@ namespace hpp {
hppDout (info, "Extension failed." << std::endl << extendStatistics_);
}
- inline void ManipulationPlanner::tryConnect (const core::Nodes_t nodes)
+ inline std::size_t ManipulationPlanner::tryConnectToRoadmap (const core::Nodes_t nodes)
{
const core::SteeringMethodPtr_t& sm (problem ().steeringMethod ());
core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
PathProjectorPtr_t pathProjector (problem().pathProjector ());
core::PathPtr_t path, projPath, validPath;
graph::GraphPtr_t graph = problem_.constraintGraph ();
+ bool connectSucceed = false;
+ std::size_t nbConnection = 0;
+ const std::size_t K = 7;
+ value_type distance;
for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
- itn1 != nodes.end (); ++itn1) {
+ itn1 != nodes.end (); ++itn1) {
ConfigurationPtr_t q1 ((*itn1)->configuration ());
- for (core::Nodes_t::const_iterator itn2 = boost::next (itn1);
- itn2 != nodes.end (); ++itn2) {
+ connectSucceed = false;
+ for (core::ConnectedComponents_t::const_iterator itcc =
+ roadmap ()->connectedComponents ().begin ();
+ itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
+ if (*itcc == (*itn1)->connectedComponent ())
+ continue;
+ core::Nodes_t knearest = roadmap()->nearestNeighbor ()
+ ->KnearestSearch (q1, *itcc, K, distance);
+ for (core::Nodes_t::const_iterator itn2 = knearest.begin ();
+ itn2 != knearest.end (); ++itn2) {
bool _1to2 = (*itn1)->isOutNeighbor (*itn2);
bool _2to1 = (*itn1)->isInNeighbor (*itn2);
if (_1to2 && _2to1) {
@@ -295,6 +319,7 @@ namespace hpp {
} else projPath = path;
PathValidationReportPtr_t report;
if (pathValidation->validate (projPath, false, validPath, report)) {
+ nbConnection++;
if (!_1to2) roadmap ()->addEdge (*itn1, *itn2, projPath);
if (!_2to1) {
core::interval_t timeRange = projPath->timeRange ();
@@ -302,9 +327,58 @@ namespace hpp {
(core::interval_t (timeRange.second,
timeRange.first)));
}
+ connectSucceed = true;
+ break;
}
+ }
+ if (connectSucceed) break;
+ }
+ }
+ return nbConnection;
+ }
+
+ inline std::size_t ManipulationPlanner::tryConnectNewNodes (const core::Nodes_t nodes)
+ {
+ const core::SteeringMethodPtr_t& sm (problem ().steeringMethod ());
+ core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
+ PathProjectorPtr_t pathProjector (problem().pathProjector ());
+ core::PathPtr_t path, projPath, validPath;
+ graph::GraphPtr_t graph = problem_.constraintGraph ();
+ std::size_t nbConnection = 0;
+ for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
+ itn1 != nodes.end (); ++itn1) {
+ ConfigurationPtr_t q1 ((*itn1)->configuration ());
+ for (core::Nodes_t::const_iterator itn2 = boost::next (itn1);
+ itn2 != nodes.end (); ++itn2) {
+ if ((*itn1)->connectedComponent () == (*itn2)->connectedComponent ())
+ continue;
+ bool _1to2 = (*itn1)->isOutNeighbor (*itn2);
+ bool _2to1 = (*itn1)->isInNeighbor (*itn2);
+ if (_1to2 && _2to1) {
+ hppDout (info, "the two nodes are already connected");
+ continue;
+ }
+ ConfigurationPtr_t q2 ((*itn2)->configuration ());
+ assert (*q1 != *q2);
+ path = (*sm) (*q1, *q2);
+ if (!path) continue;
+ if (pathProjector) {
+ if (!pathProjector->apply (path, projPath)) continue;
+ } else projPath = path;
+ PathValidationReportPtr_t report;
+ if (pathValidation->validate (projPath, false, validPath, report)) {
+ nbConnection++;
+ if (!_1to2) roadmap ()->addEdge (*itn1, *itn2, projPath);
+ if (!_2to1) {
+ core::interval_t timeRange = projPath->timeRange ();
+ roadmap ()->addEdge (*itn2, *itn1, projPath->extract
+ (core::interval_t (timeRange.second,
+ timeRange.first)));
+ }
+ }
}
}
+ return nbConnection;
}
ManipulationPlanner::ManipulationPlanner (const Problem& problem,
@@ -312,6 +386,7 @@ namespace hpp {
core::PathPlanner (problem, roadmap),
shooter_ (problem.configurationShooter()),
problem_ (problem), roadmap_ (roadmap),
+ extendStep_ (1),
qProj_ (problem.robot ()->configSize ())
{}
diff --git a/src/path-optimization/config-optimization.cc b/src/path-optimization/config-optimization.cc
index 74f026602c5e1926d2058dc3c2aa5fc6181df90e..ffa166338f9798c34c9ec8db5060efc20a2e417d 100644
--- a/src/path-optimization/config-optimization.cc
+++ b/src/path-optimization/config-optimization.cc
@@ -43,7 +43,7 @@ namespace hpp {
/// path->end(). In that case, path->end () should be in edge->node().
/// (obviously, in this case, we have edge->isInNodeFrom_ = true)
const bool reverseB = setB->edge ()->direction(before);
- const bool reverseA = setA->edge ()->direction(after);
+ //const bool reverseA = setA->edge ()->direction(after);
reverse = reverseB;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 9a9ed0de25122f3967e886856cdee4211c65adad..8772e0097366fa19ecd4aa361f208f7fb5b7365f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -17,6 +17,8 @@
#include "hpp/manipulation/problem-solver.hh"
+#include <boost/bind.hpp>
+
#include <hpp/util/pointer.hh>
#include <hpp/util/debug.hh>
@@ -29,17 +31,19 @@
#include <hpp/core/continuous-collision-checking/progressive.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/roadmap.hh>
+#include <hpp/core/steering-method-straight.hh>
+#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/handle.hh"
#include "hpp/manipulation/graph/graph.hh"
#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-optimizer.hh"
#include "hpp/manipulation/graph-path-validation.hh"
#include "hpp/manipulation/graph-node-optimizer.hh"
+#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/path-optimization/config-optimization.hh"
namespace hpp {
@@ -90,8 +94,13 @@ namespace hpp {
GraphConfigOptimizationTraits
<pathOptimization::ConfigOptimizationTraits>
>);
+ using core::SteeringMethodBuilder_t;
+ add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
+ GraphSteeringMethod::create <core::SteeringMethodStraight>);
+
pathPlannerType ("M-RRT");
pathValidationType ("Graph-Discretized", 0.05);
+ steeringMethodType ("Graph-SteeringMethodStraight");
}
ProblemSolverPtr_t ProblemSolver::create ()
diff --git a/src/problem.cc b/src/problem.cc
new file mode 100644
index 0000000000000000000000000000000000000000..7758f09ceeb9da50dd09f0bd1e2247d46469bd04
--- /dev/null
+++ b/src/problem.cc
@@ -0,0 +1,52 @@
+// 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/problem.hh>
+#include <hpp/manipulation/weighed-distance.hh>
+#include <hpp/manipulation/graph-steering-method.hh>
+
+namespace hpp {
+ namespace manipulation {
+ Problem::Problem (DevicePtr_t robot)
+ : Parent (robot), graph_()
+ {
+ Parent::steeringMethod (GraphSteeringMethod::create (this));
+ distance (WeighedDistance::create (robot, graph_));
+ }
+
+ void Problem::constraintGraph (const graph::GraphPtr_t& graph)
+ {
+ graph_ = graph;
+ if (pathValidation ())
+ pathValidation ()->constraintGraph (graph);
+ WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,
+ distance ());
+ if (d) d->constraintGraph (graph);
+ }
+
+ GraphPathValidationPtr_t Problem::pathValidation () const
+ {
+ return HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
+ Parent::pathValidation());
+ }
+
+ GraphSteeringMethodPtr_t Problem::steeringMethod () const
+ {
+ return HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod,
+ Parent::steeringMethod());
+ }
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 0458686bd953e7ef02c86ea489621822eb846cc8..2bc24768495ea70b96d3d9ed08ad8252891aaea8 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -17,6 +17,7 @@
#include "hpp/manipulation/roadmap.hh"
#include <hpp/util/pointer.hh>
+#include <hpp/core/distance.hh>
#include <hpp/core/connected-component.hh>
#include <hpp/manipulation/roadmap-node.hh>
diff --git a/src/weighed-distance.cc b/src/weighed-distance.cc
new file mode 100644
index 0000000000000000000000000000000000000000..4709eb4a1368a591596c061a30ef77236dc302a4
--- /dev/null
+++ b/src/weighed-distance.cc
@@ -0,0 +1,101 @@
+// 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/weighed-distance.hh>
+
+#include <hpp/util/debug.hh>
+
+#include <hpp/manipulation/roadmap-node.hh>
+#include <hpp/manipulation/device.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/graph/edge.hh>
+
+namespace hpp {
+ namespace manipulation {
+ WeighedDistancePtr_t WeighedDistance::create
+ (const DevicePtr_t& robot, const graph::GraphPtr_t& graph)
+ {
+ WeighedDistance* ptr = new WeighedDistance (robot, graph);
+ WeighedDistancePtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ WeighedDistancePtr_t WeighedDistance::createCopy
+ (const WeighedDistancePtr_t& distance)
+ {
+ WeighedDistance* ptr = new WeighedDistance (*distance);
+ WeighedDistancePtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ core::DistancePtr_t WeighedDistance::clone () const
+ {
+ return createCopy (weak_.lock ());
+ }
+
+ WeighedDistance::WeighedDistance (const DevicePtr_t& robot,
+ const graph::GraphPtr_t graph) :
+ core::WeighedDistance (robot), graph_ (graph)
+ {
+ }
+
+ WeighedDistance::WeighedDistance (const WeighedDistance& distance) :
+ core::WeighedDistance (distance), graph_ (distance.graph_)
+ {
+ }
+
+ void WeighedDistance::init (WeighedDistanceWkPtr_t self)
+ {
+ weak_ = self;
+ }
+
+ value_type WeighedDistance::impl_distance (ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const
+ {
+ value_type d = core::WeighedDistance::impl_distance (q1, q2);
+ return d;
+
+ // graph::Edges_t pes = graph_->getEdges
+ // (graph_->getNode (q1), graph_->getNode (q2));
+ // while (!pes.empty ()) {
+ // if (pes.back ()->canConnect (q1, q2))
+ // return d;
+ // pes.pop_back ();
+ // }
+ // return d + 100;
+ }
+
+ value_type WeighedDistance::impl_distance (core::NodePtr_t n1,
+ core::NodePtr_t n2) const
+ {
+ Configuration_t& q1 = *n1->configuration(),
+ q2 = *n2->configuration();
+ value_type d = core::WeighedDistance::impl_distance (q1, q2);
+
+ graph::Edges_t pes = graph_->getEdges (
+ graph_->getNode (static_cast <RoadmapNodePtr_t>(n1)),
+ graph_->getNode (static_cast <RoadmapNodePtr_t>(n2)));
+ while (!pes.empty ()) {
+ if (pes.back ()->canConnect (q1, q2))
+ return d;
+ pes.pop_back ();
+ }
+ return d + 100;
+ }
+ } // namespace manipulation
+} // namespace hpp