diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8b3c6d5d279faff95b6e37dec7e3e88142e81214..3f5a62a20e65ed4c8acb484f6d6f57d534b946f0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -75,6 +75,8 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
include/hpp/manipulation/connected-component.hh
+ include/hpp/manipulation/symbolic-component.hh
+ include/hpp/manipulation/symbolic-planner.hh
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-steering-method.hh
@@ -92,6 +94,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/small-steps.hh
include/hpp/manipulation/path-optimization/config-optimization.hh
+ include/hpp/manipulation/path-optimization/keypoints.hh
)
ADD_SUBDIRECTORY(src)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 8c4fb07869ca802a5eecba85ea4fced65655bac8..6a5d3ff7ae3fa288aa38c03347430eefbc1d317a 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -58,6 +58,11 @@ namespace hpp {
typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
HPP_PREDEF_CLASS (ConnectedComponent);
typedef boost::shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
+ HPP_PREDEF_CLASS (SymbolicComponent);
+ typedef boost::shared_ptr<SymbolicComponent> SymbolicComponentPtr_t;
+ typedef std::set<SymbolicComponentPtr_t> SymbolicComponents_t;
+ HPP_PREDEF_CLASS (WeighedSymbolicComponent);
+ typedef boost::shared_ptr<WeighedSymbolicComponent> WeighedSymbolicComponentPtr_t;
HPP_PREDEF_CLASS (WeighedDistance);
typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
@@ -75,6 +80,8 @@ namespace hpp {
typedef model::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS (ManipulationPlanner);
typedef boost::shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
+ HPP_PREDEF_CLASS (SymbolicPlanner);
+ typedef boost::shared_ptr < SymbolicPlanner > SymbolicPlannerPtr_t;
HPP_PREDEF_CLASS (GraphPathValidation);
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (GraphSteeringMethod);
@@ -128,6 +135,8 @@ namespace hpp {
namespace pathOptimization {
HPP_PREDEF_CLASS (SmallSteps);
typedef boost::shared_ptr < SmallSteps > SmallStepsPtr_t;
+ HPP_PREDEF_CLASS (Keypoints);
+ typedef boost::shared_ptr < Keypoints > KeypointsPtr_t;
} // namespace pathOptimization
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index f4c26d3a047d7a22af5a3dd1dc5fdd0228322f18..44762a26bef923b16d1b7b7e3f1945a199f3e3c8 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -19,6 +19,7 @@
#include <hpp/core/constraint-set.hh>
#include <hpp/core/steering-method.hh>
+#include <hpp/core/relative-motion.hh>
#include <hpp/core/path.hh>
#include "hpp/manipulation/config.hh"
@@ -84,6 +85,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
{
public:
+ typedef core::RelativeMotion RelativeMotion;
+
/// Destructor
virtual ~Edge ();
@@ -160,6 +163,11 @@ namespace hpp {
return pathValidation_->get();
}
+ const RelativeMotion::matrix_type& relativeMotion () const
+ {
+ return relMotion_;
+ }
+
/// Get direction of the path compare to the edge
/// \return true is reverse
virtual bool direction (const core::PathPtr_t& path) const;
@@ -226,6 +234,7 @@ namespace hpp {
SteeringMethod_t* steeringMethod_;
/// Path validation associated to the edge
+ mutable RelativeMotion::matrix_type relMotion_;
PathValidation_t* pathValidation_;
/// Weak pointer to itself.
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 409286ba0749932a1d8b700762cbea89d11de730..502c8093d2aba6b831415406821b77951cbc5612 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -73,10 +73,16 @@ namespace hpp {
(const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
HPP_MANIPULATION_DEPRECATED;
+ /// Reset the numerical constraints stored in the component.
+ virtual void resetNumericalConstraints ();
+
/// Add core::LockedJoint constraint to the component.
virtual void addLockedJointConstraint
(const LockedJointPtr_t& constraint);
+ /// Reset the locked joint in the component.
+ virtual void resetLockedJoints ();
+
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one NumericalConstraintPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index 6ad3c141daf7e804c71e1dec49f4b902e2e5394b..fce9ee88a28a2bac94f3b9cd57002cdd0613a217 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -98,6 +98,20 @@ namespace hpp {
WithPrePlace = 1 << 4
};
+ struct Rule {
+ std::string gripper_;
+ std::string handle_;
+ bool link_;
+ Rule() : gripper_(""), handle_(""), link_(false) {}
+ Rule(std::string gripper, std::string handle, bool link) {
+ gripper_ = gripper;
+ handle_ = handle;
+ link_ = link;
+ }
+ };
+
+ typedef std::vector<Rule> Rules_t;
+
/// Create edges according to the case.
/// gCase is a logical OR combination of GraspingCase and PlacementCase
///
@@ -178,7 +192,8 @@ namespace hpp {
void graphBuilder (
const Objects_t& objects,
const Grippers_t& grippers,
- GraphPtr_t graph);
+ GraphPtr_t graph,
+ const Rules_t& rules = Rules_t ());
struct ObjectDef_t {
std::string name;
@@ -191,6 +206,7 @@ namespace hpp {
const StringList_t& griNames,
const std::list <ObjectDef_t>& objs,
const StringList_t& envNames,
+ const Rules_t& rules,
const value_type& prePlaceWidth = 0.05);
/// \}
} // namespace helper
diff --git a/include/hpp/manipulation/path-optimization/keypoints.hh b/include/hpp/manipulation/path-optimization/keypoints.hh
new file mode 100644
index 0000000000000000000000000000000000000000..3bb13f5cb274a0f9f470f54d89253c65f8b2f348
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/keypoints.hh
@@ -0,0 +1,78 @@
+// Copyright (c) 2016, 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/>.
+
+#ifndef HPP_MANIPULATION_PATHOPTIMIZATION_KEYPOINTS_HH
+#define HPP_MANIPULATION_PATHOPTIMIZATION_KEYPOINTS_HH
+
+# include <hpp/core/path-optimizer.hh>
+# include <hpp/core/path-optimizer.hh>
+
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/problem.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using hpp::core::Path;
+ using hpp::core::PathPtr_t;
+ using hpp::core::PathVector;
+ using hpp::core::PathVectorPtr_t;
+ /// \addtogroup path_optimization
+ /// \{
+
+ class HPP_MANIPULATION_DLLAPI Keypoints : public core::PathOptimizer
+ {
+ public:
+ static KeypointsPtr_t create (const core::Problem& problem) {
+ const Problem& p = dynamic_cast <const Problem&> (problem);
+ return KeypointsPtr_t (new Keypoints (p));
+ }
+
+ virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+
+ protected:
+ /// Constructor
+ Keypoints (const Problem& problem) :
+ PathOptimizer (problem), problem_ (problem) {}
+
+ private:
+ const Problem& problem_;
+ struct InterKeypointPath {
+ PathVectorPtr_t path;
+ bool isShort;
+ graph::EdgePtr_t edge;
+ };
+ typedef std::vector<InterKeypointPath> IKPvector_t;
+
+ IKPvector_t split (PathVectorPtr_t path) const;
+
+ PathVectorPtr_t shorten (const IKPvector_t& paths,
+ std::size_t i1, std::size_t i2) const;
+
+ IKPvector_t replaceInPath(const IKPvector_t& input,
+ const PathVectorPtr_t& shortcut,
+ std::size_t i1, std::size_t i2) const;
+ };
+
+ /// \}
+
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATHOPTIMIZATION_KEYPOINTS_HH
diff --git a/include/hpp/manipulation/roadmap-node.hh b/include/hpp/manipulation/roadmap-node.hh
index 430687d7d5e69a5cae050d6978ff6f2bb19e0e47..2fa8992e4b71af97c8aa60743e2b1e1b3163cb6e 100644
--- a/include/hpp/manipulation/roadmap-node.hh
+++ b/include/hpp/manipulation/roadmap-node.hh
@@ -80,10 +80,21 @@ namespace hpp {
/// \}
+ void symbolicComponent (const SymbolicComponentPtr_t& sc)
+ {
+ symbolicCC_ = sc;
+ }
+
+ const SymbolicComponentPtr_t& symbolicComponent () const
+ {
+ return symbolicCC_;
+ }
+
private:
CachingSystem cacheSystem_;
graph::NodePtr_t node_;
+ SymbolicComponentPtr_t symbolicCC_;
};
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index 68091f2391d5f80ea26415cecba89f417114e556..8d6096ea185ffb39a8f3dfb5d780821d4b01c1d6 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -62,6 +62,12 @@ namespace hpp {
/// Get graph node corresponding to given roadmap node
graph::NodePtr_t getNode(RoadmapNodePtr_t node);
+ /// Get the symbolic components
+ const SymbolicComponents_t& symbolicComponents () const
+ {
+ return symbolicCCs_;
+ }
+
protected:
/// Register a new configuration.
void statInsert (const RoadmapNodePtr_t& n);
@@ -76,7 +82,9 @@ namespace hpp {
{
weak_ = shPtr;
}
-
+
+ virtual void addEdge (const core::EdgePtr_t& edge);
+
private:
typedef std::list < graph::HistogramPtr_t > Histograms;
/// Keep track of the leaf that are explored.
@@ -84,6 +92,7 @@ namespace hpp {
Histograms histograms_;
graph::GraphPtr_t graph_;
RoadmapWkPtr_t weak_;
+ SymbolicComponents_t symbolicCCs_;
};
/// \}
} // namespace manipulation
diff --git a/include/hpp/manipulation/symbolic-component.hh b/include/hpp/manipulation/symbolic-component.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c80add3462c73bdf5838179941d6db8eee63aa3e
--- /dev/null
+++ b/include/hpp/manipulation/symbolic-component.hh
@@ -0,0 +1,120 @@
+//
+// Copyright (c) 2016 CNRS
+// 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/>.
+
+#ifndef HPP_MANIPULATION_SYMBOLIC_COMPONENT_HH
+#define HPP_MANIPULATION_SYMBOLIC_COMPONENT_HH
+
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
+
+#include <hpp/manipulation/roadmap-node.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// Set of configurations accessible to each others by a single edge,
+ /// with the same right hand side.
+ ///
+ /// This assumes the roadmap is not directed.
+ class HPP_MANIPULATION_DLLAPI SymbolicComponent
+ {
+ public:
+ typedef std::set<SymbolicComponentPtr_t> SymbolicComponents_t;
+
+ /// return a shared pointer to new instance
+ static SymbolicComponentPtr_t create (const RoadmapPtr_t& roadmap);
+
+ /// Merge two symbolic components
+ /// \param other manipulation symbolic component to merge into this one.
+ /// \note other will be empty after calling this method.
+ virtual void merge (SymbolicComponentPtr_t otherCC);
+
+ bool canMerge (const SymbolicComponentPtr_t& otherCC) const;
+
+ /// Add otherCC to the list of reachable components
+ ///
+ /// This also add this object to the list of ancestor of otherCC.
+ void canReach (const SymbolicComponentPtr_t& otherCC);
+
+ /// Add roadmap node to connected component
+ /// \param roadmap node to be added
+ void addNode (const RoadmapNodePtr_t& node);
+
+ virtual void setFirstNode (const RoadmapNodePtr_t& node);
+
+ core::ConnectedComponentPtr_t connectedComponent () const
+ {
+ assert (!nodes_.empty());
+ return nodes_.front()->connectedComponent();
+ };
+
+ const RoadmapNodes_t& nodes() const
+ {
+ return nodes_;
+ }
+
+ protected:
+ SymbolicComponent(const RoadmapPtr_t& r)
+ : roadmap_(r) {}
+
+ void init (const SymbolicComponentWkPtr_t& shPtr)
+ {
+ weak_ = shPtr;
+ }
+
+ graph::NodePtr_t state_;
+ RoadmapNodes_t nodes_;
+
+ private:
+ RoadmapPtr_t roadmap_;
+ SymbolicComponents_t to_, from_;
+ SymbolicComponentWkPtr_t weak_;
+ }; // class SymbolicComponent
+
+ class HPP_MANIPULATION_DLLAPI WeighedSymbolicComponent :
+ public SymbolicComponent
+ {
+ public:
+ void merge (SymbolicComponentPtr_t otherCC);
+
+ void setFirstNode (const RoadmapNodePtr_t& node);
+
+ static WeighedSymbolicComponentPtr_t create (const RoadmapPtr_t& roadmap);
+
+ std::size_t indexOf (const graph::EdgePtr_t e) const;
+
+ void normalizeProba ()
+ {
+ const value_type s = p_.sum();
+ p_ /= s;
+ }
+
+ value_type weight_;
+ /// Transition probabilities
+ vector_t p_;
+ std::vector<graph::EdgePtr_t> edges_;
+
+ protected:
+ WeighedSymbolicComponent(const RoadmapPtr_t& r)
+ : SymbolicComponent(r), weight_(1) {}
+
+ private:
+ }; // class SymbolicComponent
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_SYMBOLIC_COMPONENT_HH
diff --git a/include/hpp/manipulation/symbolic-planner.hh b/include/hpp/manipulation/symbolic-planner.hh
new file mode 100644
index 0000000000000000000000000000000000000000..556dcbe652d268287a8ff479072db171a6f3a47d
--- /dev/null
+++ b/include/hpp/manipulation/symbolic-planner.hh
@@ -0,0 +1,155 @@
+// Copyright (c) 2014, LAAS-CNRS
+// 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/>.
+
+#ifndef HPP_MANIPULATION_SYMBOLIC_PLANNER_HH
+# define HPP_MANIPULATION_SYMBOLIC_PLANNER_HH
+
+#include <hpp/core/path-planner.hh>
+
+#include <hpp/statistics/success-bin.hh>
+
+#include "hpp/manipulation/graph/statistics.hh"
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup path_planning
+ /// \{
+
+ class HPP_MANIPULATION_DLLAPI SymbolicPlanner :
+ public hpp::core::PathPlanner
+ {
+ public:
+ typedef std::list<std::size_t> ErrorFreqs_t;
+ struct ExtendStatus {
+ int status;
+ int info;
+ graph::EdgePtr_t edge;
+ PathValidationReportPtr_t validationReport;
+ };
+
+ /// Create an instance and return a shared pointer to the instance
+ static SymbolicPlannerPtr_t create (const core::Problem& problem,
+ const core::RoadmapPtr_t& roadmap);
+
+ /// One step of extension.
+ ///
+ /// A set of constraints is chosen using the graph of constraints.
+ /// A constraint extension is done using a chosen set.
+ ///
+ virtual void oneStep ();
+
+ /// 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),
+ /// resulting from the extension.
+ /// \retval status an integer
+ /// \return True if the returned path is valid.
+ bool extend (RoadmapNodePtr_t q_near, const ConfigurationPtr_t &q_rand,
+ core::PathPtr_t& validPath, ExtendStatus& status);
+
+ /// Get the number of occurrence of each errors.
+ ///
+ /// \sa ManipulationPlanner::errorList
+ ErrorFreqs_t getEdgeStat (const graph::EdgePtr_t& edge) const;
+
+ /// Get the list of possible outputs of the extension step.
+ ///
+ /// \sa ManipulationPlanner::getEdgeStat
+ static StringList_t errorList ();
+
+ protected:
+ /// Protected constructor
+ SymbolicPlanner (const Problem& problem,
+ const RoadmapPtr_t& roadmap);
+
+ /// Store weak pointer to itself
+ void init (const SymbolicPlannerWkPtr_t& weak);
+
+ private:
+ /// 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);
+
+ void updateWeightsAndProbabilities (
+ const RoadmapNodePtr_t& near, const RoadmapNodePtr_t& newN,
+ const ExtendStatus& es);
+
+ /// The transitions probabilities are updated as follow:
+ ///
+ /// \li let \f$p_{edge}\f$ be the edge probability before update,
+ /// \li the edge probability is multiplied by \f$\alpha\f$,
+ /// \li all other probabilities are multiplied by \f$ \frac{1 - \alpha * p_{edge}}{1 - p_{edge}} \f$
+ ///
+ /// \param alpha should be between 0 and 1.
+ ///
+ /// \note Theoretically, the described operation preserves the
+ /// normalization of the sum. WeighedSymbolicComponent::normalizeProba
+ /// is called to avoid numerical errors.
+ static void updateEdgeProba (
+ const graph::EdgePtr_t edge,
+ WeighedSymbolicComponentPtr_t wsc,
+ const value_type alpha);
+
+ /// Configuration shooter
+ ConfigurationShooterPtr_t shooter_;
+ /// Pointer to the problem
+ const Problem& problem_;
+ /// Pointer to the roadmap
+ RoadmapPtr_t roadmap_;
+ /// weak pointer to itself
+ SymbolicPlannerWkPtr_t weakPtr_;
+
+ /// Keep track of success and failure for method
+ /// extend.
+ typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
+ typedef ::hpp::statistics::SuccessBin SuccessBin;
+ typedef ::hpp::statistics::SuccessBin::Reason Reason;
+ SuccessStatistics& edgeStat (const graph::EdgePtr_t& edge);
+ std::vector<int> indexPerEdgeStatistics_;
+ std::vector<SuccessStatistics> perEdgeStatistics_;
+
+ /// A Reason is associated to each EdgePtr_t that generated a failure.
+ enum TypeOfFailure {
+ SUCCESS = -1,
+ PROJECTION = 0,
+ STEERING_METHOD = 1,
+ PATH_VALIDATION_ZERO = 2,
+ PATH_PROJECTION_ZERO = 3,
+ UNKNOWN = 4,
+ PATH_PROJECTION_SHORTER = 5,
+ PATH_VALIDATION_SHORTER = 6,
+ PARTLY_EXTENDED = 7,
+ PATH_PROJECTION_AND_VALIDATION_SHORTER = PATH_PROJECTION_SHORTER + PATH_VALIDATION_SHORTER
+ };
+ static const std::vector<Reason> reasons_;
+
+ const value_type extendStep_;
+
+ mutable Configuration_t qProj_;
+ };
+ /// \}
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_SYMBOLIC_PLANNER_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b9edbd26044ec15463b1a7e84dfa5a0030003975..98ea6c21ddfccfeab44ef184f608176eb32bfe93 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -22,10 +22,12 @@ SET(LIBRARY_NAME ${PROJECT_NAME})
SET(SOURCES
axial-handle.cc
handle.cc
+ symbolic-planner.cc
manipulation-planner.cc
problem-solver.cc
roadmap.cc
connected-component.cc
+ symbolic-component.cc
constraint-set.cc
roadmap-node.cc
device.cc
@@ -47,6 +49,7 @@ SET(SOURCES
graph/dot.cc
path-optimization/config-optimization.cc
+ path-optimization/keypoints.cc
)
IF(HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index c51297f0bc2f4df1e4846a4844ee171604959a8c..b5c133871af781b54bcbf1be74edc03163e3b900 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -260,10 +260,9 @@ namespace hpp {
// TODO this path validation will not contain obstacles added after
// its creation.
pathValidation_->set(problem->pathValidationFactory ());
- using core::RelativeMotion;
- RelativeMotion::matrix_type matrix (RelativeMotion::matrix (g->robot()));
- RelativeMotion::fromConstraint (matrix, g->robot(), constraint);
- pathValidation_->get()->filterCollisionPairs (matrix);
+ relMotion_ = RelativeMotion::matrix (g->robot());
+ RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
+ pathValidation_->get()->filterCollisionPairs (relMotion_);
return constraint;
}
@@ -287,7 +286,7 @@ namespace hpp {
if (constraints->isSatisfied (q1)) {
if (constraints->isSatisfied (q2)) {
path = (*steeringMethod_->get()) (q1, q2);
- return path;
+ return (bool)path;
}
hppDout(info, "q2 does not satisfy the constraints");
}
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index cadd90580c1b2e901d760880882064c59497cc6a..7d5214906bb9d0263ed7218d60e5d2854e48a58e 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -75,12 +75,23 @@ namespace hpp {
addNumericalConstraint (NumericalConstraint::create (function,ineq));
}
+ void GraphComponent::resetNumericalConstraints ()
+ {
+ numericalConstraints_.clear();
+ passiveDofs_.clear();
+ }
+
void GraphComponent::addLockedJointConstraint
(const LockedJointPtr_t& constraint)
{
lockedJoints_.push_back (constraint);
}
+ void GraphComponent::resetLockedJoints ()
+ {
+ lockedJoints_.clear();
+ }
+
bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
{
IntervalsContainer_t::const_iterator itpdof = passiveDofs_.begin ();
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index eb8a2ee49946b72fdecfaf91aed564f418eea6df..35ad3516ca13567da233d1e8db131ca955350ef9 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -16,7 +16,11 @@
#include <hpp/manipulation/graph/helper.hh>
+#include <tr1/unordered_map>
+#include <tr1/unordered_set>
+
#include <boost/array.hpp>
+#include <boost/regex.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp>
@@ -519,15 +523,47 @@ namespace hpp {
/// - the values correpond to the index of the handle (0..nbHandle-1), or
/// nbHandle to mean no handle.
typedef std::vector <index_t> GraspV_t;
+ struct CompiledRule {
+ enum Result {
+ Accept,
+ Refuse,
+ NoMatch,
+ Undefined
+ };
+ boost::regex gripper, handle;
+ bool link;
+ CompiledRule (const Rule& r) :
+ gripper (r.gripper_), handle (r.handle_), link (r.link_) {}
+ Result check (const std::string& g, const std::string& h) const
+ {
+ if (boost::regex_match(g, gripper))
+ if (boost::regex_match(h, handle))
+ return (link ? Accept : Refuse);
+ return NoMatch;
+ }
+ };
+ typedef std::vector<CompiledRule> CompiledRules_t;
struct Result {
GraphPtr_t graph;
- std::vector<NodeAndManifold_t> nodes;
+ typedef unsigned long nodeid_type;
+ std::tr1::unordered_map<nodeid_type, NodeAndManifold_t> nodes;
+ typedef std::pair<nodeid_type, nodeid_type> edgeid_type;
+ struct edgeid_hash {
+ std::tr1::hash<edgeid_type::first_type> first;
+ std::tr1::hash<edgeid_type::second_type> second;
+ std::size_t operator() (const edgeid_type& eid) const {
+ return first(eid.first) + second(eid.second);
+ }
+ };
+ std::tr1::unordered_set<edgeid_type, edgeid_hash> edges;
std::vector< boost::array<NumericalConstraintPtr_t,3> > graspCs;
index_t nG, nOH;
GraspV_t dims;
const Grippers_t& gs;
const Objects_t& ohs;
+ CompiledRules_t rules;
+ mutable Eigen::MatrixXi rulesCache;
Result (const Grippers_t& grippers, const Objects_t& objects, GraphPtr_t g) :
graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects)
@@ -539,16 +575,71 @@ namespace hpp {
dims[0] = nOH + 1;
for (index_t i = 1; i < nG; ++i)
dims[i] = dims[i-1] * (nOH + 1);
- nodes.resize (dims[nG-1] * (nOH + 1));
graspCs.resize (nG * nOH);
+ rulesCache = Eigen::MatrixXi::Constant(nG, nOH + 1, CompiledRule::Undefined);
+ }
+
+ void setRules (const Rules_t& r)
+ {
+ for (Rules_t::const_iterator _r = r.begin(); _r != r.end(); ++_r)
+ rules.push_back (CompiledRule(*_r));
+ }
+
+ bool graspIsAllowed (const GraspV_t& idxOH) const
+ {
+ assert (idxOH.size () == nG);
+ for (std::size_t i = 0; i < nG; ++i) {
+ const std::string& g = gs[i]->name(),
+ h = (idxOH[i] == nOH) ? "" : handle (idxOH[i])->name ();
+ if ((CompiledRule::Result)rulesCache(i, idxOH[i]) == CompiledRule::Undefined) {
+ CompiledRule::Result status = CompiledRule::Accept;
+ for (std::size_t r = 0; r < rules.size(); ++r) {
+ status = rules[r].check(g,h);
+ if (status == CompiledRule::Accept) break;
+ else if (status == CompiledRule::Refuse) break;
+ status = CompiledRule::Accept;
+ }
+ rulesCache(i, idxOH[i]) = status;
+ }
+ bool keep = ((CompiledRule::Result)rulesCache(i, idxOH[i]) == CompiledRule::Accept);
+ if (!keep) return false;
+ }
+ return true;
+ }
+
+ inline nodeid_type nodeid (const GraspV_t& iG)
+ {
+ nodeid_type iGOH = iG[0];
+ nodeid_type res;
+ for (index_t i = 1; i < nG; ++i) {
+ res = iGOH + dims[i] * (iG[i]);
+ if (res < iGOH) {
+ hppDout (info, "Node ID overflowed. There are too many states...");
+ }
+ iGOH = res;
+ // iGOH += dims[i] * (iG[i]);
+ }
+ return iGOH;
+ }
+
+ bool hasNode (const GraspV_t& iG)
+ {
+ return nodes.count(nodeid(iG)) > 0;
}
NodeAndManifold_t& operator() (const GraspV_t& iG)
{
- index_t iGOH = iG[0];
- for (index_t i = 1; i < nG; ++i)
- iGOH += dims[i] * (iG[i]);
- return nodes [iGOH];
+ return nodes [nodeid(iG)];
+ }
+
+ bool hasEdge (const GraspV_t& g1, const GraspV_t& g2)
+ {
+ return edges.count(edgeid_type(nodeid(g1), nodeid(g2))) > 0;
+ }
+
+ void addEdge (const GraspV_t& g1, const GraspV_t& g2)
+ {
+ edges.insert(edgeid_type(nodeid(g1), nodeid(g2)));
}
inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint (
@@ -712,6 +803,11 @@ namespace hpp {
const GraspV_t& gFrom, const GraspV_t& gTo,
const index_t iG, const int priority)
{
+ if (r.hasEdge(gFrom, gTo)) {
+ hppDout (warning, "Prevented creation of duplicated edge\nfrom "
+ << r.name (gFrom) << "\nto " << r.name (gTo));
+ return;
+ }
const NodeAndManifold_t& from = makeNode (r, gFrom, priority),
to = makeNode (r, gTo, priority+1);
const Object_t& o = r.object (gTo[iG]);
@@ -818,6 +914,7 @@ namespace hpp {
grasp.foliated () , place.foliated(),
submanifold);
}
+ r.addEdge(gFrom, gTo);
}
/// idx are the available grippers
@@ -828,6 +925,9 @@ namespace hpp {
if (idx_g.empty () || idx_oh.empty ()) return;
IndexV_t nIdx_g (idx_g.size() - 1);
IndexV_t nIdx_oh (idx_oh.size() - 1);
+ bool curGraspIsAllowed = r.graspIsAllowed(grasps);
+ if (curGraspIsAllowed) makeNode (r, grasps, depth);
+
for (IndexV_t::const_iterator itx_g = idx_g.begin ();
itx_g != idx_g.end (); ++itx_g) {
// Copy all element except itx_g
@@ -839,7 +939,12 @@ namespace hpp {
// Create the edge for the selected grasp
GraspV_t nGrasps = grasps;
nGrasps [*itx_g] = *itx_oh;
- makeEdge (r, grasps, nGrasps, *itx_g, depth);
+
+ bool nextGraspIsAllowed = r.graspIsAllowed(nGrasps);
+ if (nextGraspIsAllowed) makeNode (r, nGrasps, depth + 1);
+
+ if (curGraspIsAllowed && nextGraspIsAllowed)
+ makeEdge (r, grasps, nGrasps, *itx_g, depth);
// Copy all element except itx_oh
std::copy (boost::next (itx_oh), idx_oh.end (),
@@ -855,13 +960,15 @@ namespace hpp {
void graphBuilder (
const Objects_t& objects,
const Grippers_t& grippers,
- GraphPtr_t graph)
+ GraphPtr_t graph,
+ const Rules_t& rules)
{
if (!graph) throw std::logic_error ("The graph must be initialized");
NodeSelectorPtr_t ns = graph->nodeSelector ();
if (!ns) throw std::logic_error ("The graph does not have a NodeSelector");
Result r (grippers, objects, graph);
+ r.setRules (rules);
IndexV_t availG (r.nG), availOH (r.nOH);
for (index_t i = 0; i < r.nG; ++i) availG[i] = i;
@@ -870,6 +977,9 @@ namespace hpp {
GraspV_t iG (r.nG, r.nOH);
recurseGrippers (r, availG, availOH, iG, 0);
+
+ hppDout (info, "Created a graph with " << r.nodes.size() << " states "
+ "and " << r.edges.size() << " edges.");
}
GraphPtr_t graphBuilder (
@@ -878,6 +988,7 @@ namespace hpp {
const StringList_t& griNames,
const std::list <ObjectDef_t>& objs,
const StringList_t& envNames,
+ const std::vector <Rule>& rules,
const value_type& prePlaceWidth)
{
const Device& robot = *(ps->robot ());
@@ -934,7 +1045,7 @@ namespace hpp {
graph->maxIterations (ps->maxIterations ());
graph->errorThreshold (ps->errorThreshold ());
- graphBuilder (objects, grippers, graph);
+ graphBuilder (objects, grippers, graph, rules);
ps->constraintGraph (graph);
return graph;
}
diff --git a/src/path-optimization/keypoints.cc b/src/path-optimization/keypoints.cc
new file mode 100644
index 0000000000000000000000000000000000000000..f0b167136808a6d42b5fab82f2a7717a697b60fd
--- /dev/null
+++ b/src/path-optimization/keypoints.cc
@@ -0,0 +1,211 @@
+// Copyright (c) 2016, 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/path-optimization/keypoints.hh>
+
+#include <hpp/core/path.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/path-projector.hh>
+#include <hpp/core/path-validation.hh>
+
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph-steering-method.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using std::make_pair;
+
+ PathVectorPtr_t Keypoints::optimize (const PathVectorPtr_t& path)
+ {
+ PathVectorPtr_t input = PathVector::create (
+ path->outputSize(), path->outputDerivativeSize());
+ PathVectorPtr_t output = path;
+ path->flatten (input);
+ // Step 1: find the key waypoints
+ // paths is a vector of
+ IKPvector_t paths = split (input);
+
+ // Step 2: for each couple of consecutive path between keywaypoints,
+ // try to find a better intermediate key waypoint.
+ std::size_t i1 = 0;
+ bool success = false;
+ for (std::size_t i2 = 1; i2 < paths.size(); ++i2) {
+ if (paths[i2].isShort) continue; // So that i2 = i1 + 2
+ // Try to generate a new key waypoint
+ PathVectorPtr_t shortcut = shorten (paths, i1, i2);
+ if (shortcut) {
+ paths = replaceInPath (paths, shortcut, i1, i2);
+ success = true;
+ while (i2 < paths.size() && !paths[i2].isShort) ++i2;
+ }
+ i1 = i2;
+ }
+
+ if (success) {
+ // Build path
+ PathVectorPtr_t output = PathVector::create (path->outputSize (),
+ path->outputDerivativeSize ());
+ for (std::size_t j = 0; j < paths.size(); ++j)
+ output->concatenate (*paths[j].path);
+ }
+ return output;
+ }
+
+ Keypoints::IKPvector_t Keypoints::split (PathVectorPtr_t input) const
+ {
+ IKPvector_t paths;
+
+ ConstraintSetPtr_t c;
+ for (std::size_t i_s = 0; i_s < input->numberPaths ();) {
+ InterKeypointPath ikpp;
+ ikpp.path = PathVector::create (
+ input->outputSize(), input->outputDerivativeSize());
+ PathPtr_t current = input->pathAtRank (i_s);
+ ikpp.path->appendPath (current);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (c) ikpp.edge = c->edge ();
+ ikpp.isShort = ikpp.edge && ikpp.edge->isShort();
+ std::size_t i_e = i_s + 1;
+ for (; i_e < input->numberPaths (); ++i_e) {
+ current = input->pathAtRank (i_e);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (!c && ikpp.edge) {
+ hppDout(info, "No manipulation::ConstraintSet");
+ break;
+ }
+ if (c && ikpp.edge->node() != c->edge ()->node()) break;
+ if (ikpp.isShort != c->edge()->isShort()) // We do not optimize edges marked as short
+ break;
+ ikpp.path->appendPath (current);
+ }
+ hppDout(info, "Edge name: " << ikpp.edge->name());
+ i_s = i_e;
+ paths.push_back (ikpp);
+ }
+ return paths;
+ }
+
+ PathVectorPtr_t Keypoints::shorten (const IKPvector_t& paths,
+ std::size_t i1, std::size_t i2) const
+ {
+ PathVectorPtr_t result;
+ const std::size_t maxTrials = 10;
+ value_type cur_length = 0;
+ for (std::size_t i = i1; i < i2 + 1; ++i)
+ cur_length += paths[i].path->length();
+
+ const core::SteeringMethodPtr_t& sm (problem_.steeringMethod ());
+ PathProjectorPtr_t pathProjector (problem().pathProjector ());
+ core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
+
+ // Get all the possible edges of the graph.
+ const InterKeypointPath& ikpp1 = paths[i1];
+ const InterKeypointPath& ikpp2 = paths[i2];
+ graph::Edges_t edges;
+ graph::Graph& graph = *problem_.constraintGraph ();
+ try {
+ edges = graph.getEdges (ikpp1.edge->node(), ikpp2.edge->node());
+ } catch (const std::logic_error& e) {
+ hppDout (error, e.what ());
+ return PathVectorPtr_t ();
+ }
+
+ value_type t0, t1;
+ Configuration_t q (ikpp1.path->outputSize()),
+ q1 (ikpp1.path->outputSize()),
+ q4 (ikpp1.path->outputSize());
+
+ // Unused variables
+ PathValidationReportPtr_t report;
+ PathPtr_t unusedP;
+
+ for (std::size_t n1 = 0; n1 < maxTrials; ++n1) {
+ // Generate a random parameter on p1.
+ t0 = ikpp1.path->timeRange().first;
+ t1 = t0 + ikpp1.path->timeRange().second * rand ()/RAND_MAX;
+ // Get the configuration and project it.
+ if (!(*ikpp1.path) (q1, t1)) continue;
+ for (graph::Edges_t::const_iterator _edges = edges.begin();
+ _edges != edges.end(); ++_edges) {
+ // TODO: For level set edges, something different should be done.
+ // Maybe, we must add a virtual generateInIntersection (qfrom, qto, qrand)
+ // that would be specialized for each edge.
+ q = q1;
+ if ((*_edges)->applyConstraints (q1,q)) {
+ PathPtr_t short1, proj1;
+ if ((*_edges)->build (short1, q1, q)) {
+ if (!short1 || !pathProjector->apply(short1, proj1)) continue;
+ if (!pathValidation->validate (proj1, false, unusedP, report)) continue;
+
+ // short1 is a valid path leading to the target node.
+ // Try to connect this to ikpp2.path
+ for (std::size_t n2 = 0; n2 < maxTrials; ++n2) {
+ value_type t3, t4, t5;
+ t3 = ikpp2.path->timeRange().first;
+ t4 = t3 + ikpp2.path->timeRange().second * rand ()/RAND_MAX;
+ t5 = t3 + ikpp2.path->timeRange().second;
+ if (!(*ikpp2.path) (q4, t4)) continue;
+ PathPtr_t short2, proj2;
+ short2 = (*sm)(q, q4);
+ if (!short2 || !pathProjector->apply(short2, proj2)) continue;
+ if (!pathValidation->validate (proj2, false, unusedP, report)) continue;
+ // short2 is possible. Build the path and check it is shorter.
+
+ PathVectorPtr_t tmp = PathVector::create (ikpp1.path->outputSize (),
+ ikpp1.path->outputDerivativeSize ());
+ tmp->concatenate (*(ikpp1.path->extract
+ (make_pair (t0, t1))-> as <PathVector> ()));
+ tmp->appendPath (short1);
+ tmp->appendPath (short2);
+ tmp->concatenate (*(ikpp2.path->extract
+ (make_pair (t4, t5))-> as <PathVector> ()));
+
+ if (cur_length >= tmp->length()) {
+ cur_length = tmp->length();
+ result = tmp;
+ }
+ }
+ }
+ }
+ }
+ }
+ return result;
+ }
+
+ Keypoints::IKPvector_t Keypoints::replaceInPath(const IKPvector_t& input,
+ const PathVectorPtr_t& shortcut,
+ std::size_t i1, std::size_t i2) const
+ {
+ PathVectorPtr_t flat = PathVector::create (shortcut->outputSize (),
+ shortcut->outputDerivativeSize ());
+ shortcut->flatten(flat);
+
+ PathVectorPtr_t output = PathVector::create (shortcut->outputSize (),
+ shortcut->outputDerivativeSize ());
+ for (std::size_t j = 0; j < i1; ++j)
+ output->concatenate (*input[j].path);
+ output->concatenate (*flat);
+ for (std::size_t j = i2 + 1; j < input.size(); ++j)
+ output->concatenate (*input[j].path);
+
+ return split (output);
+ }
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 7bddf6e6608c09449272427a24edf44d964e8e23..80521aab768c7f7826fe4d84acf793c0b6434366 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -38,6 +38,7 @@
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/handle.hh"
#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/symbolic-planner.hh"
#include "hpp/manipulation/manipulation-planner.hh"
#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/constraint-set.hh"
@@ -46,6 +47,7 @@
#include "hpp/manipulation/graph-node-optimizer.hh"
#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/path-optimization/config-optimization.hh"
+#include "hpp/manipulation/path-optimization/keypoints.hh"
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
#include <hpp/wholebody-step/small-steps.hh>
@@ -80,6 +82,8 @@ namespace hpp {
{
parent_t::add<core::PathPlannerBuilder_t>
("M-RRT", ManipulationPlanner::create);
+ parent_t::add<core::PathPlannerBuilder_t>
+ ("SymbolicPlanner", SymbolicPlanner::create);
using core::PathOptimizerBuilder_t;
parent_t::add <PathOptimizerBuilder_t> ("Graph-RandomShortcut",
GraphOptimizer::create <core::RandomShortcut>);
@@ -99,6 +103,9 @@ namespace hpp {
parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
GraphSteeringMethod::create <core::SteeringMethodStraight>);
+ parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut",
+ pathOptimization::Keypoints::create);
+
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
parent_t::add <PathOptimizerBuilder_t>
("Walkgen", wholebodyStep::SmallSteps::create);
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 009a7dbdac660b2db0bd627a9835b76272cf0a24..fef69ea0c6c8e8841bcf49208c42101b8c4362a8 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -17,10 +17,14 @@
#include "hpp/manipulation/roadmap.hh"
#include <hpp/util/pointer.hh>
+
+#include <hpp/core/edge.hh>
#include <hpp/core/distance.hh>
-#include <hpp/manipulation/connected-component.hh>
+#include <hpp/manipulation/roadmap.hh>
+#include <hpp/manipulation/graph/node.hh>
#include <hpp/manipulation/roadmap-node.hh>
+#include <hpp/manipulation/symbolic-component.hh>
namespace hpp {
namespace manipulation {
@@ -46,8 +50,11 @@ namespace hpp {
void Roadmap::push_node (const core::NodePtr_t& n)
{
- statInsert (static_cast <RoadmapNodePtr_t> (n));
Parent::push_node (n);
+ const RoadmapNodePtr_t& node =
+ static_cast <const RoadmapNodePtr_t> (n);
+ statInsert (node);
+ symbolicCCs_.insert(node->symbolicComponent());
}
void Roadmap::statInsert (const RoadmapNodePtr_t& n)
@@ -99,7 +106,11 @@ namespace hpp {
core::NodePtr_t Roadmap::createNode (const ConfigurationPtr_t& q) const
{
// call RoadmapNode constructor with new manipulation connected component
- return RoadmapNodePtr_t (new RoadmapNode (q, ConnectedComponent::create(weak_)));
+ RoadmapNodePtr_t node = new RoadmapNode (q, ConnectedComponent::create(weak_));
+ SymbolicComponentPtr_t sc = WeighedSymbolicComponent::create (weak_.lock());
+ node->symbolicComponent (sc);
+ sc->setFirstNode(node);
+ return node;
}
graph::NodePtr_t Roadmap::getNode(RoadmapNodePtr_t node)
@@ -107,5 +118,23 @@ namespace hpp {
return graph_->getNode(node);
}
+ void Roadmap::addEdge (const core::EdgePtr_t& edge)
+ {
+ Parent::addEdge(edge);
+ const RoadmapNodePtr_t& f = static_cast <const RoadmapNodePtr_t> (edge->from());
+ const RoadmapNodePtr_t& t = static_cast <const RoadmapNodePtr_t> (edge->to());
+ SymbolicComponentPtr_t scf = f->symbolicComponent();
+ SymbolicComponentPtr_t sct = t->symbolicComponent();
+ scf->canReach(sct);
+ if (scf->canMerge(sct)) {
+ if (scf->nodes().size() > sct->nodes().size()) {
+ scf->merge(sct);
+ symbolicCCs_.erase(sct);
+ } else {
+ sct->merge(scf);
+ symbolicCCs_.erase(scf);
+ }
+ }
+ }
} // namespace manipulation
} // namespace hpp
diff --git a/src/symbolic-component.cc b/src/symbolic-component.cc
new file mode 100644
index 0000000000000000000000000000000000000000..d678f56c813a4ae8a16e427c923a86b6951bc176
--- /dev/null
+++ b/src/symbolic-component.cc
@@ -0,0 +1,126 @@
+// Copyright (c) 2016, 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/symbolic-component.hh>
+
+#include <hpp/manipulation/roadmap.hh>
+
+namespace hpp {
+ namespace manipulation {
+ SymbolicComponentPtr_t SymbolicComponent::create (const RoadmapPtr_t& roadmap)
+ {
+ SymbolicComponentPtr_t shPtr =
+ SymbolicComponentPtr_t(new SymbolicComponent(roadmap));
+ shPtr->init(shPtr);
+ return shPtr;
+ }
+
+ void SymbolicComponent::addNode (const RoadmapNodePtr_t& node)
+ {
+ assert(state_);
+ graph::NodePtr_t state = roadmap_->getNode(node);
+
+ // Sanity check
+ if (state_ == state) // Oops
+ throw std::runtime_error ("RoadmapNode of SymbolicComponent must be in"
+ " the same state");
+ nodes_.push_back(node);
+ }
+
+ void SymbolicComponent::setFirstNode (const RoadmapNodePtr_t& node)
+ {
+ assert(!state_);
+ state_ = roadmap_->getNode(node);
+ nodes_.push_back(node);
+ }
+
+ bool SymbolicComponent::canMerge (const SymbolicComponentPtr_t& otherCC) const
+ {
+ if (otherCC->state_ != state_) return false;
+ SymbolicComponents_t::const_iterator it = std::find
+ (to_.begin(), to_.end(), otherCC);
+ if (it == to_.end()) return false;
+ it = std::find
+ (from_.begin(), from_.end(), otherCC);
+ if (it == from_.end()) return false;
+ return true;
+ }
+
+ void SymbolicComponent::canReach (const SymbolicComponentPtr_t& otherCC)
+ {
+ to_.insert(otherCC);
+ otherCC->from_.insert(weak_.lock());
+ }
+
+ void SymbolicComponent::merge (SymbolicComponentPtr_t other)
+ {
+ assert (canMerge(other));
+ if (other == weak_.lock()) return;
+
+ // Tell other's nodes that they now belong to this connected component
+ for (RoadmapNodes_t::iterator itNode = other->nodes_.begin ();
+ itNode != other->nodes_.end (); ++itNode) {
+ (*itNode)->symbolicComponent (weak_.lock ());
+ }
+ // Add other's nodes to this list.
+ nodes_.insert (nodes_.end (), other->nodes_.begin(), other->nodes_.end());
+
+ from_.erase (other);
+ other->from_.erase (weak_.lock());
+ from_.insert (other->from_.begin(), other->from_.end());
+ to_.erase (other);
+ other->to_.erase (weak_.lock());
+ to_.insert (other->to_.begin(), other->to_.end());
+ }
+
+ WeighedSymbolicComponentPtr_t WeighedSymbolicComponent::create (const RoadmapPtr_t& roadmap)
+ {
+ WeighedSymbolicComponentPtr_t shPtr = WeighedSymbolicComponentPtr_t
+ (new WeighedSymbolicComponent(roadmap));
+ shPtr->init(shPtr);
+ return shPtr;
+ }
+
+ void WeighedSymbolicComponent::merge (SymbolicComponentPtr_t otherCC)
+ {
+ WeighedSymbolicComponentPtr_t other =
+ HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, otherCC);
+ value_type r = nodes_.size() / (nodes_.size() + other->nodes_.size());
+
+ SymbolicComponent::merge(otherCC);
+ weight_ *= other->weight_; // TODO a geometric mean would be more natural.
+ p_ = r * p_ + (1 - r) * other->p_;
+ }
+
+ void WeighedSymbolicComponent::setFirstNode (const RoadmapNodePtr_t& node)
+ {
+ SymbolicComponent::setFirstNode(node);
+ std::vector<value_type> p = state_->neighbors().probabilities();
+ p_.resize(p.size());
+ edges_ = state_->neighbors().values();
+ for (std::size_t i = 0; i < p.size(); ++i)
+ p_[i] = p[i];
+ }
+
+ std::size_t WeighedSymbolicComponent::indexOf (const graph::EdgePtr_t e) const
+ {
+ std::size_t i = 0;
+ for (; i < edges_.size(); ++i)
+ if (edges_[i] == e) break;
+ return i;
+ }
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/symbolic-planner.cc b/src/symbolic-planner.cc
new file mode 100644
index 0000000000000000000000000000000000000000..362ecf4ead1f111da5a78504c58a8056a38c5f50
--- /dev/null
+++ b/src/symbolic-planner.cc
@@ -0,0 +1,668 @@
+// Copyright (c) 2014, LAAS-CNRS
+// 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/symbolic-planner.hh"
+
+#include <boost/tuple/tuple.hpp>
+#include <boost/assign/list_of.hpp>
+
+#include <hpp/util/pointer.hh>
+#include <hpp/util/timer.hh>
+#include <hpp/util/assertion.hh>
+
+#include <hpp/model/configuration.hh>
+#include <hpp/model/collision-object.hh>
+
+#include <hpp/core/roadmap.hh>
+#include <hpp/core/distance.hh>
+#include <hpp/core/path-validation.hh>
+#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/core/basic-configuration-shooter.hh>
+#include <hpp/core/path-validation-report.hh>
+#include <hpp/core/collision-validation-report.hh>
+
+#include "hpp/manipulation/graph/statistics.hh"
+#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/roadmap.hh"
+#include "hpp/manipulation/roadmap-node.hh"
+#include "hpp/manipulation/symbolic-component.hh"
+#include "hpp/manipulation/graph/edge.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/graph/node-selector.hh"
+
+#define CastToWSC_ptr(var, scPtr) \
+ WeighedSymbolicComponentPtr_t var = \
+ HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent,scPtr)
+
+namespace hpp {
+ namespace manipulation {
+ using core::CollisionValidationReport;
+ using core::CollisionValidationReportPtr_t;
+ using core::CollisionObjectPtr_t;
+ typedef graph::Edge::RelativeMotion RelativeMotion;
+
+ namespace {
+ HPP_DEFINE_TIMECOUNTER(oneStep);
+ HPP_DEFINE_TIMECOUNTER(extend);
+ HPP_DEFINE_TIMECOUNTER(tryConnect);
+ HPP_DEFINE_TIMECOUNTER(nearestNeighbor);
+ HPP_DEFINE_TIMECOUNTER(delayedEdges);
+ HPP_DEFINE_TIMECOUNTER(tryConnectNewNodes);
+ HPP_DEFINE_TIMECOUNTER(tryConnectToRoadmap);
+ /// extend steps
+ HPP_DEFINE_TIMECOUNTER(chooseEdge);
+ HPP_DEFINE_TIMECOUNTER(applyConstraints);
+ HPP_DEFINE_TIMECOUNTER(buildPath);
+ HPP_DEFINE_TIMECOUNTER(projectPath);
+ HPP_DEFINE_TIMECOUNTER(validatePath);
+
+ struct WeighedSymbolicComponentComp {
+ bool operator() (const SymbolicComponentPtr_t& lhs, const SymbolicComponentPtr_t& rhs)
+ {
+ return HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, lhs)->weight_
+ > HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, rhs)->weight_;
+ }
+ };
+ typedef std::list<WeighedSymbolicComponentPtr_t> SymbolicComponentList_t;
+ SymbolicComponentList_t sorted_list (const SymbolicComponents_t& sc)
+ {
+ SymbolicComponentList_t l;
+ WeighedSymbolicComponentComp comp;
+ for (SymbolicComponents_t::const_iterator _sc = sc.begin();
+ _sc != sc.end(); ++_sc)
+ l.insert (std::upper_bound(l.begin(), l.end(), *_sc, comp),
+ HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, *_sc));
+ return l;
+ }
+
+ struct DistanceToConfiguration {
+ const Configuration_t& q_;
+ core::Distance& d_;
+ value_type mind_;
+ RoadmapNodePtr_t node_;
+ DistanceToConfiguration(const Configuration_t& q, core::Distance& d)
+ : q_(q), d_(d), mind_(std::numeric_limits<value_type>::max()) {}
+ void operator() (const RoadmapNodePtr_t& n)
+ {
+ value_type dist = d_ (*(n->configuration()), q_);
+ if (dist < mind_) {
+ node_ = n;
+ mind_ = dist;
+ }
+ }
+ };
+
+ bool belongs (const ConfigurationPtr_t& q, const core::Nodes_t& nodes)
+ {
+ for (core::Nodes_t::const_iterator itNode = nodes.begin ();
+ itNode != nodes.end (); ++itNode) {
+ if (*((*itNode)->configuration ()) == *q) return true;
+ }
+ return false;
+ }
+ }
+
+ const std::vector<SymbolicPlanner::Reason>
+ SymbolicPlanner::reasons_ = boost::assign::list_of
+ (SuccessBin::createReason ("[Fail] Projection"))
+ (SuccessBin::createReason ("[Fail] SteeringMethod"))
+ (SuccessBin::createReason ("[Fail] Path validation returned length 0"))
+ (SuccessBin::createReason ("[Fail] Path could not be projected"))
+ (SuccessBin::createReason ("[Fail] Unknow"))
+ (SuccessBin::createReason ("[Info] Path could not be fully projected"))
+ (SuccessBin::createReason ("[Info] Path could not be fully validated"))
+ (SuccessBin::createReason ("[Info] Extended partly"));
+
+ SymbolicPlannerPtr_t SymbolicPlanner::create (const core::Problem& problem,
+ const core::RoadmapPtr_t& roadmap)
+ {
+ SymbolicPlanner* ptr;
+ core::RoadmapPtr_t r2 = roadmap;
+ RoadmapPtr_t r;
+ try {
+ const Problem& p = dynamic_cast < const Problem& > (problem);
+ RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
+ ptr = new SymbolicPlanner (p, r);
+ } catch (std::exception&) {
+ if (!r)
+ throw std::invalid_argument ("The roadmap must be of type hpp::manipulation::Roadmap.");
+ else
+ throw std::invalid_argument ("The problem must be of type hpp::manipulation::Problem.");
+ }
+ SymbolicPlannerPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ SymbolicPlanner::ErrorFreqs_t SymbolicPlanner::getEdgeStat
+ (const graph::EdgePtr_t& edge) const
+ {
+ const std::size_t& id = edge->id ();
+ ErrorFreqs_t ret;
+ if (indexPerEdgeStatistics_.size() <= id ||
+ indexPerEdgeStatistics_[id] < 0) {
+ for (std::size_t i = 0; i < reasons_.size(); ++i) ret.push_back (0);
+ } else {
+ const SuccessStatistics& ss =
+ perEdgeStatistics_[indexPerEdgeStatistics_[id]];
+ ret.push_back (ss.nbSuccess ());
+ for (std::size_t i = 0; i < reasons_.size(); ++i)
+ ret.push_back (ss.nbFailure (reasons_[i]));
+ }
+ return ret;
+ }
+
+ StringList_t SymbolicPlanner::errorList ()
+ {
+ StringList_t ret;
+ ret.push_back ("Success");
+ for (std::size_t i = 0; i < reasons_.size(); ++i) ret.push_back (reasons_[i].what);
+ return ret;
+ }
+
+ void SymbolicPlanner::oneStep ()
+ {
+ HPP_START_TIMECOUNTER(oneStep);
+
+ // Get the robot
+ DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem ().robot ());
+ HPP_ASSERT(robot);
+ // Get the roadmap and the symbolic components
+ RoadmapPtr_t rdm = HPP_DYNAMIC_PTR_CAST(Roadmap, roadmap());
+ HPP_ASSERT(rdm);
+ SymbolicComponentList_t scs = sorted_list (rdm->symbolicComponents());
+
+ core::Nodes_t newNodes;
+ core::PathPtr_t path;
+
+ typedef boost::tuple <RoadmapNodePtr_t, ConfigurationPtr_t, core::PathPtr_t,
+ ExtendStatus>
+ DelayedEdge_t;
+ typedef std::vector <DelayedEdge_t> DelayedEdges_t;
+ DelayedEdges_t delayedEdges;
+
+ // Pick a random node
+ ConfigurationPtr_t q_rand = shooter_->shoot();
+
+ // Extend each connected component
+ for (core::ConnectedComponents_t::const_iterator itcc =
+ rdm->connectedComponents ().begin ();
+ itcc != rdm->connectedComponents ().end (); ++itcc) {
+ // Find the symbolic component in this connected component which has
+ // the highest value.
+ SymbolicComponentPtr_t sc;
+ for (SymbolicComponentList_t::iterator _sc = scs.begin();
+ _sc != scs.end(); ++_sc) {
+ sc = *_sc;
+ if (sc->connectedComponent() == *itcc) break;
+ }
+ if (!sc) {
+ hppDout (error, "There should always be a connected component corresponding to symbolic component");
+ continue;
+ }
+
+ // Find the nearest neighbor.
+ HPP_START_TIMECOUNTER(nearestNeighbor);
+ DistanceToConfiguration dtc (*q_rand, *rdm->distance());
+ RoadmapNodePtr_t near =
+ std::for_each(sc->nodes().begin(), sc->nodes().end(), dtc).node_;
+ HPP_STOP_TIMECOUNTER(nearestNeighbor);
+ HPP_DISPLAY_LAST_TIMECOUNTER(nearestNeighbor);
+ if (!near) continue;
+
+ // Extension
+ HPP_START_TIMECOUNTER(extend);
+ ExtendStatus status;
+ bool pathIsValid = extend (near, q_rand, path, status);
+ HPP_STOP_TIMECOUNTER(extend);
+ HPP_DISPLAY_LAST_TIMECOUNTER(extend);
+
+ // Insert new path to q_near in roadmap
+ if (pathIsValid) {
+ value_type t_final = path->timeRange ().second;
+ if (t_final != path->timeRange ().first) {
+ bool success;
+ ConfigurationPtr_t q_new (new Configuration_t
+ ((*path) (t_final, success)));
+ delayedEdges.push_back (DelayedEdge_t (near, q_new, path, status));
+ }
+ } else {
+ updateWeightsAndProbabilities (near, RoadmapNodePtr_t(), status);
+ }
+ }
+
+ // Insert delayed edges
+ HPP_START_TIMECOUNTER(delayedEdges);
+ for (DelayedEdges_t::const_iterator itEdge = delayedEdges.begin ();
+ itEdge != delayedEdges.end (); ++itEdge) {
+ const core::NodePtr_t& near = itEdge-> get <0> ();
+ const ConfigurationPtr_t& q_new = itEdge-> get <1> ();
+ const core::PathPtr_t& validPath = itEdge-> get <2> ();
+ core::NodePtr_t newNode = rdm->addNode (q_new);
+ roadmap()->addEdge (near, newNode, validPath);
+ core::interval_t timeRange = validPath->timeRange ();
+ roadmap()->addEdge (newNode, near, validPath->extract
+ (core::interval_t (timeRange.second ,
+ timeRange.first)));
+ updateWeightsAndProbabilities (
+ static_cast<RoadmapNode*>(near),
+ static_cast<RoadmapNode*>(newNode),
+ itEdge->get<3>());
+ }
+ HPP_STOP_TIMECOUNTER(delayedEdges);
+
+ // Try to connect the new nodes together
+ 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_TIMECOUNTER(oneStep);
+ HPP_DISPLAY_TIMECOUNTER(extend);
+ HPP_DISPLAY_TIMECOUNTER(tryConnect);
+ HPP_DISPLAY_TIMECOUNTER(tryConnectNewNodes);
+ HPP_DISPLAY_TIMECOUNTER(tryConnectToRoadmap);
+ HPP_DISPLAY_TIMECOUNTER(nearestNeighbor);
+ HPP_DISPLAY_TIMECOUNTER(delayedEdges);
+ HPP_DISPLAY_TIMECOUNTER(chooseEdge);
+ HPP_DISPLAY_TIMECOUNTER(applyConstraints);
+ HPP_DISPLAY_TIMECOUNTER(buildPath);
+ HPP_DISPLAY_TIMECOUNTER(projectPath);
+ HPP_DISPLAY_TIMECOUNTER(validatePath);
+ }
+
+ bool SymbolicPlanner::extend(
+ RoadmapNodePtr_t n_near,
+ const ConfigurationPtr_t& q_rand,
+ core::PathPtr_t& validPath,
+ ExtendStatus& status)
+ {
+ status.info = SUCCESS;
+ graph::GraphPtr_t graph = problem_.constraintGraph ();
+ // Select next node in the constraint graph.
+ const ConfigurationPtr_t q_near = n_near->configuration ();
+
+ HPP_START_TIMECOUNTER (chooseEdge);
+ // This code should go into a NodeSelector derived class.
+ WeighedSymbolicComponentPtr_t wscPtr = HPP_DYNAMIC_PTR_CAST
+ (WeighedSymbolicComponent, n_near->symbolicComponent());
+ if (wscPtr) {
+ WeighedSymbolicComponent wsc = *wscPtr;
+ value_type R = rand() / RAND_MAX;
+ std::size_t i = 0;
+ for (value_type sum = wsc.p_[0]; sum < R; sum += wsc.p_[i]) { ++i; }
+ assert(i < wsc.edges_.size());
+ status.edge = wsc.edges_[i];
+ } else status.edge = graph->chooseEdge (n_near);
+ HPP_STOP_TIMECOUNTER (chooseEdge);
+ if (!status.edge) {
+ status.status = UNKNOWN;
+ return false;
+ }
+
+ qProj_ = *q_rand;
+ HPP_START_TIMECOUNTER (applyConstraints);
+ SuccessStatistics& es = edgeStat (status.edge);
+ if (!status.edge->applyConstraints (n_near, qProj_)) {
+ HPP_STOP_TIMECOUNTER (applyConstraints);
+ es.addFailure (reasons_[PROJECTION]);
+ status.status = PROJECTION;
+ return false;
+ }
+ HPP_STOP_TIMECOUNTER (applyConstraints);
+
+ core::PathPtr_t path;
+ HPP_START_TIMECOUNTER (buildPath);
+ if (!status.edge->build (path, *q_near, qProj_)) {
+ HPP_STOP_TIMECOUNTER (buildPath);
+ es.addFailure (reasons_[STEERING_METHOD]);
+ status.status = STEERING_METHOD;
+ return false;
+ }
+ HPP_STOP_TIMECOUNTER (buildPath);
+
+ core::PathPtr_t projPath;
+ bool projShorter = false;
+ PathProjectorPtr_t pathProjector = problem_.pathProjector ();
+ if (pathProjector) {
+ HPP_START_TIMECOUNTER (projectPath);
+ projShorter = !pathProjector->apply (path, projPath);
+ if (projShorter) {
+ if (!projPath || projPath->length () == 0) {
+ HPP_STOP_TIMECOUNTER (projectPath);
+ es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
+ status.status = PATH_PROJECTION_ZERO;
+ return false;
+ }
+ status.info = PATH_PROJECTION_SHORTER;
+ es.addFailure (reasons_[PATH_PROJECTION_SHORTER]);
+ }
+ HPP_STOP_TIMECOUNTER (projectPath);
+ } else projPath = path;
+ GraphPathValidationPtr_t pathValidation (problem_.pathValidation ());
+ core::PathPtr_t fullValidPath;
+
+ HPP_START_TIMECOUNTER (validatePath);
+ bool fullyValid = false;
+ try {
+ fullyValid = pathValidation->validate
+ (projPath, false, fullValidPath, status.validationReport);
+ } catch (const core::projection_error& e) {
+ hppDout (error, e.what ());
+ es.addFailure (reasons_[PATH_VALIDATION_ZERO]);
+ status.status = PATH_VALIDATION_ZERO;
+ return false;
+ }
+ HPP_STOP_TIMECOUNTER (validatePath);
+
+ if (fullValidPath->length () == 0) {
+ es.addFailure (reasons_[PATH_VALIDATION_ZERO]);
+ validPath = fullValidPath;
+ status.status = PATH_PROJECTION_ZERO;
+ return false;
+ } else {
+ if (!fullyValid) {
+ es.addFailure (reasons_[PATH_VALIDATION_SHORTER]);
+ status.status = (projShorter?PATH_PROJECTION_AND_VALIDATION_SHORTER:PATH_VALIDATION_SHORTER);
+ }
+ if (extendStep_ == 1 || fullyValid) {
+ validPath = fullValidPath;
+ } else {
+ const value_type& length = fullValidPath->length();
+ const value_type& t_init = fullValidPath->timeRange ().first;
+ try {
+ validPath = fullValidPath->extract
+ (core::interval_t(t_init, t_init + length * extendStep_));
+ } catch (const core::projection_error& e) {
+ hppDout (error, e.what());
+ es.addFailure (reasons_[PATH_PROJECTION_SHORTER]);
+ status.status = UNKNOWN;
+ return false;
+ }
+ }
+ hppDout (info, "Extension:" << std::endl
+ << es);
+ }
+ if (!projShorter && fullValidPath) es.addSuccess ();
+ else es.addFailure (reasons_[PARTLY_EXTENDED]);
+ status.status = SUCCESS;
+ return true;
+ }
+
+ SymbolicPlanner::SuccessStatistics& SymbolicPlanner::edgeStat
+ (const graph::EdgePtr_t& edge)
+ {
+ const std::size_t& id = edge->id ();
+ if (indexPerEdgeStatistics_.size () <= id) {
+ indexPerEdgeStatistics_.resize (id + 1, -1);
+ }
+ if (indexPerEdgeStatistics_[id] < 0) {
+ indexPerEdgeStatistics_[id] = perEdgeStatistics_.size();
+ perEdgeStatistics_.push_back (SuccessStatistics (edge->name (), 2));
+ }
+ return perEdgeStatistics_[indexPerEdgeStatistics_[id]];
+ }
+
+ inline std::size_t SymbolicPlanner::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) {
+ ConfigurationPtr_t q1 ((*itn1)->configuration ());
+ 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) {
+ 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)));
+ }
+ connectSucceed = true;
+ break;
+ }
+ }
+ if (connectSucceed) break;
+ }
+ }
+ return nbConnection;
+ }
+
+ inline std::size_t SymbolicPlanner::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;
+ }
+
+ inline void SymbolicPlanner::updateWeightsAndProbabilities (
+ const RoadmapNodePtr_t& near, const RoadmapNodePtr_t& newN,
+ const ExtendStatus& extend)
+ {
+ // 0.99 ^ 26 * 1.3 ~= 1 => A new extension followed by 26 failures
+ // results in similar weights for the symbolic components.
+ const value_type weightInc = 1.3;
+ const value_type weightDec = 0.99;
+ CastToWSC_ptr (oldWSC, near->symbolicComponent());
+ CollisionValidationReportPtr_t colRep;
+ switch (extend.status) {
+ case SUCCESS:
+ {
+ CastToWSC_ptr (newWSC, newN->symbolicComponent());
+ switch (extend.info) {
+ case SUCCESS:
+ // If the corresponding edge is a loop, no adjustment.
+ // Otherwise, a new symbolic component has been created. This new SC
+ // should have higher priority than the old one.
+ if (oldWSC != newWSC) {
+ newWSC->weight_ = oldWSC->weight_ * weightInc;
+ // Maybe we should decrease the edge probability as well.
+ // A new connection with this edge should be tried whenever
+ // we have tried to extend the new SC and we could not.
+ }
+ oldWSC->weight_ *= weightDec;
+ updateEdgeProba(extend.edge, oldWSC, 0.99); // Rate of change should not be too big.
+ break;
+ case PATH_PROJECTION_SHORTER:
+ case PATH_PROJECTION_AND_VALIDATION_SHORTER:
+ // Hard to say what to do on this case...
+ break;
+ case PATH_VALIDATION_SHORTER:
+ // The path was made shorter, check the validation report to know
+ // why.
+ colRep = HPP_DYNAMIC_PTR_CAST(CollisionValidationReport,
+ extend.validationReport->configurationReport);
+ if (colRep) {
+ CollisionObjectPtr_t o1 = colRep->object1, o2 = colRep->object2;
+ // If it is a collision between the robot and an unactuated object,
+ // which cannot move in this state.
+ if (o1->joint() != NULL && o2->joint() != NULL) {
+ // TODO: Currently, the RelativeMotion matrix does not cover cases
+ // like ConvexShapeContact (2 function making a 6D constraints).
+ RelativeMotion::matrix_type m = extend.edge->relativeMotion();
+ size_type i0 = RelativeMotion::idx(NULL);
+ size_type i1 = RelativeMotion::idx(o1->joint());
+ size_type i2 = RelativeMotion::idx(o2->joint());
+ if (m(i0, i1) == RelativeMotion::Parameterized
+ || m(i0, i1) == RelativeMotion::Constrained) {
+ // Object1 should be moved out of the way first...
+ updateEdgeProba(extend.edge, oldWSC, 0.99); // Rate of change should not be too big.
+ }
+ if (m(i0, i2) == RelativeMotion::Parameterized
+ || m(i0, i2) == RelativeMotion::Constrained) {
+ // Object2 should be moved out of the way first...
+ updateEdgeProba(extend.edge, oldWSC, 0.99); // Rate of change should not be too big.
+ }
+ }
+ }
+ break;
+ }
+ }
+ break;
+ // For cases below, newN is a NULL pointer.
+ case PROJECTION:
+ case PATH_PROJECTION_ZERO: // unclear for PATH_PROJECTION_ZERO
+ // It was not possible to project. It may be that projection is not
+ // possible, or only that it is not yet possible.
+ // We decrease the probability of sampling this edge.
+ updateEdgeProba(extend.edge, oldWSC, 0.99); // Rate of change should not be too big.
+ oldWSC->weight_ *= weightDec;
+ break;
+ case PATH_VALIDATION_ZERO:
+ // The path validation report will say what's wrong.
+ // The only interesting thing are collisions with objects.
+ colRep = HPP_DYNAMIC_PTR_CAST(CollisionValidationReport,
+ extend.validationReport->configurationReport);
+ if (colRep) {
+ CollisionObjectPtr_t o1 = colRep->object1, o2 = colRep->object2;
+ // If it is a collision between the robot and an unactuated object,
+ // which cannot move in this state.
+ if (o1->joint() != NULL && o2->joint() != NULL) {
+ // TODO: Currently, the RelativeMotion matrix does not cover cases
+ // like ConvexShapeContact (2 function making a 6D constraints).
+ RelativeMotion::matrix_type m = extend.edge->relativeMotion();
+ size_type i0 = RelativeMotion::idx(NULL);
+ size_type i1 = RelativeMotion::idx(o1->joint());
+ size_type i2 = RelativeMotion::idx(o2->joint());
+ if (m(i0, i1) == RelativeMotion::Parameterized
+ || m(i0, i1) == RelativeMotion::Constrained) {
+ // Object1 should be moved out of the way first...
+ updateEdgeProba(extend.edge, oldWSC, 0.99); // Rate of change should not be too big.
+ }
+ if (m(i0, i2) == RelativeMotion::Parameterized
+ || m(i0, i2) == RelativeMotion::Constrained) {
+ // Object2 should be moved out of the way first...
+ updateEdgeProba(extend.edge, oldWSC, 0.99); // Rate of change should not be too big.
+ }
+ }
+ }
+ break;
+ case UNKNOWN:
+ case STEERING_METHOD:
+ // Hard to say what to do.
+ oldWSC->weight_ *= weightDec;
+ break;
+ }
+ }
+
+ void SymbolicPlanner::updateEdgeProba (
+ const graph::EdgePtr_t edge,
+ WeighedSymbolicComponentPtr_t wsc,
+ const value_type alpha) {
+ std::size_t i = wsc->indexOf (edge);
+ assert(i < wsc->p_.size());
+ const value_type pi = wsc->p_[i];
+ wsc->p_ *= ( 1 - alpha * pi ) / ( 1 - pi );
+ wsc->p_[i] = alpha * pi;
+ wsc->normalizeProba();
+ }
+
+ SymbolicPlanner::SymbolicPlanner (const Problem& problem,
+ const RoadmapPtr_t& roadmap) :
+ core::PathPlanner (problem, roadmap),
+ shooter_ (problem.configurationShooter()),
+ problem_ (problem), roadmap_ (roadmap),
+ extendStep_ (1),
+ qProj_ (problem.robot ()->configSize ())
+ {}
+
+ void SymbolicPlanner::init (const SymbolicPlannerWkPtr_t& weak)
+ {
+ core::PathPlanner::init (weak);
+ weakPtr_ = weak;
+ }
+ } // namespace manipulation
+} // namespace hpp