src/graph/helper.cc

@@ -41,8 +41,8 @@
 #include <hpp/util/debug.hh>
 #include <iterator>
 #include <pinocchio/multibody/model.hpp>
-#include <tr1/unordered_map>
-#include <tr1/unordered_set>
+#include <unordered_map>
+#include <unordered_set>
 
 #define CASE_TO_STRING(var, value) \
   ((var & value) ? std::string(#value) : std::string())
@@ -511,16 +511,16 @@ struct Result {
   ProblemSolverPtr_t ps;
   GraphPtr_t graph;
   typedef unsigned long stateid_type;
-  std::tr1::unordered_map<stateid_type, StateAndManifold_t> states;
+  std::unordered_map<stateid_type, StateAndManifold_t> states;
   typedef std::pair<stateid_type, stateid_type> edgeid_type;
   struct edgeid_hash {
-    std::tr1::hash<edgeid_type::first_type> first;
-    std::tr1::hash<edgeid_type::second_type> second;
+    std::hash<edgeid_type::first_type> first;
+    std::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::unordered_set<edgeid_type, edgeid_hash> edges;
   std::vector<std::array<ImplicitPtr_t, 3> > graspCs;
   index_t nG, nOH;
   GraspV_t dims;
@@ -953,7 +953,7 @@ GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
     std::get<2>(objects[i]) = i;
     std::get<1>(objects[i]).resize(od.handles.size());
     Handles_t::iterator it = std::get<1>(objects[i]).begin();
-    for (const std::string hn : od.handles) {
+    for (const std::string& hn : od.handles) {
       *it = robot.handles.get(hn);
       ++it;
     }

src/graph/state-selector.cc

@@ -90,8 +90,7 @@ StatePtr_t StateSelector::getState(ConfigurationIn_t config) const {
 }
 
 StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const {
-  if (!node->cacheUpToDate())
-    node->graphState(getState(*(node->configuration())));
+  if (!node->cacheUpToDate()) node->graphState(getState(node->configuration()));
   return node->graphState();
 }
 

src/graph/statistics.cc

@@ -153,8 +153,8 @@ LeafHistogram::LeafHistogram(const Foliation f) : f_(f), threshold_(0) {
 }
 
 void LeafHistogram::add(const RoadmapNodePtr_t& n) {
-  if (!f_.contains(*n->configuration())) return;
-  iterator it = insert(LeafBin(f_.parameter(*n->configuration()), &threshold_));
+  if (!f_.contains(n->configuration())) return;
+  iterator it = insert(LeafBin(f_.parameter(n->configuration()), &threshold_));
   it->push_back(n);
   if (numberOfObservations() % 10 == 0) {
     hppDout(info, *this);

src/handle.cc

@@ -74,6 +74,7 @@ bool isHandleOnFreeflyer(const Handle& handle) {
 }
 
 inline int maskSize(const std::vector<bool>& mask) {
+  assert(mask.size() == 6);
   std::size_t res(0);
   for (std::size_t i = 0; i < 6; ++i) {
     if (mask[i]) ++res;
@@ -98,6 +99,7 @@ inline bool is6Dmask(const std::vector<bool>& mask) {
 }
 
 inline std::vector<bool> complementMask(const std::vector<bool>& mask) {
+  assert(mask.size() == 6);
   std::vector<bool> m(6);
   for (std::size_t i = 0; i < 6; ++i) m[i] = !mask[i];
   return m;
@@ -202,8 +204,8 @@ ImplicitPtr_t Handle::createGraspAndComplement(const GripperPtr_t& gripper,
 ImplicitPtr_t Handle::createPreGrasp(const GripperPtr_t& gripper,
                                      const value_type& shift,
                                      std::string n) const {
-  Transform3f M = gripper->objectPositionInJoint() *
-                  Transform3f(I3, vector3_t(shift, 0, 0));
+  Transform3s M = gripper->objectPositionInJoint() *
+                  Transform3s(I3, vector3_t(shift, 0, 0));
   if (n.empty())
     n = "Pregrasp_ " + maskToStr(mask_) + "_" + name() + "_" + gripper->name();
   ImplicitPtr_t result(Implicit::create(

src/manipulation-planner.cc

@@ -75,7 +75,7 @@ graph::StatePtr_t getState(const graph::GraphPtr_t graph,
   if (mnode != NULL)
     return mnode->graphState();
   else
-    return graph->getState(*node->configuration());
+    return graph->getState(node->configuration());
 }
 
 core::PathPtr_t connect(const Configuration_t& q1, const Configuration_t& q2,
@@ -177,13 +177,13 @@ void ManipulationPlanner::oneStep() {
   core::Nodes_t newNodes;
   core::PathPtr_t path;
 
-  typedef std::tuple<core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
+  typedef std::tuple<core::NodePtr_t, Configuration_t, core::PathPtr_t>
       DelayedEdge_t;
   typedef std::vector<DelayedEdge_t> DelayedEdges_t;
   DelayedEdges_t delayedEdges;
 
   // Pick a random node
-  ConfigurationPtr_t q_rand = shooter_->shoot();
+  Configuration_t q_rand = shooter_->shoot();
 
   // Extend each connected component
   for (core::ConnectedComponents_t::const_iterator itcc =
@@ -210,12 +210,11 @@ void ManipulationPlanner::oneStep() {
         value_type t_final = path->timeRange().second;
         if (t_final != path->timeRange().first) {
           bool success;
-          ConfigurationPtr_t q_new(
-              new Configuration_t(path->eval(t_final, success)));
+          Configuration_t q_new(path->eval(t_final, success));
           assert(success);
           assert(!path->constraints() ||
-                 path->constraints()->isSatisfied(*q_new));
-          assert(problem_->constraintGraph()->getState(*q_new));
+                 path->constraints()->isSatisfied(q_new));
+          assert(problem_->constraintGraph()->getState(q_new));
           delayedEdges.push_back(DelayedEdge_t(near, q_new, path));
         }
       }
@@ -226,7 +225,7 @@ void ManipulationPlanner::oneStep() {
   // Insert delayed edges
   for (const auto& edge : delayedEdges) {
     const core::NodePtr_t& near = std::get<0>(edge);
-    const ConfigurationPtr_t& q_new = std::get<1>(edge);
+    Configuration_t q_new = std::get<1>(edge);
     const core::PathPtr_t& validPath = std::get<2>(edge);
     core::NodePtr_t newNode = roadmap()->addNode(q_new);
     roadmap()->addEdge(near, newNode, validPath);
@@ -263,21 +262,21 @@ void ManipulationPlanner::oneStep() {
 }
 
 bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
-                                 const ConfigurationPtr_t& q_rand,
+                                 ConfigurationIn_t q_rand,
                                  core::PathPtr_t& validPath) {
   graph::GraphPtr_t graph = problem_->constraintGraph();
   PathProjectorPtr_t pathProjector = problem_->pathProjector();
   pinocchio::DevicePtr_t robot(problem_->robot());
   value_type eps(graph->errorThreshold());
   // Select next node in the constraint graph.
-  const ConfigurationPtr_t q_near = n_near->configuration();
+  const Configuration_t q_near = n_near->configuration();
   HPP_START_TIMECOUNTER(chooseEdge);
   graph::EdgePtr_t edge = graph->chooseEdge(n_near);
   HPP_STOP_TIMECOUNTER(chooseEdge);
   if (!edge) {
     return false;
   }
-  qProj_ = *q_rand;
+  qProj_ = q_rand;
   HPP_START_TIMECOUNTER(generateTargetConfig);
   SuccessStatistics& es = edgeStat(edge);
   if (!edge->generateTargetConfig(n_near, qProj_)) {
@@ -286,7 +285,7 @@ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
     es.addFailure(reasons_[PROJECTION]);
     return false;
   }
-  if (pinocchio::isApprox(robot, qProj_, *q_near, eps)) {
+  if (pinocchio::isApprox(robot, qProj_, q_near, eps)) {
     es.addFailure(reasons_[FAILURE]);
     es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
     return false;
@@ -294,7 +293,7 @@ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
   HPP_STOP_TIMECOUNTER(generateTargetConfig);
   core::PathPtr_t path;
   HPP_START_TIMECOUNTER(buildPath);
-  if (!edge->build(path, *q_near, qProj_)) {
+  if (!edge->build(path, q_near, qProj_)) {
     HPP_STOP_TIMECOUNTER(buildPath);
     es.addFailure(reasons_[FAILURE]);
     es.addFailure(reasons_[STEERING_METHOD]);
@@ -396,7 +395,7 @@ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
   value_type distance;
   for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
        ++itn1) {
-    const Configuration_t& q1(*(*itn1)->configuration());
+    const Configuration_t& q1((*itn1)->configuration());
     graph::StatePtr_t s1 = getState(graph, *itn1);
     connectSucceed = false;
     for (core::ConnectedComponents_t::const_iterator itcc =
@@ -411,7 +410,7 @@ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
         bool _2to1 = (*itn1)->isInNeighbor(*itn2);
         assert(!_1to2 || !_2to1);
 
-        const Configuration_t& q2(*(*itn2)->configuration());
+        const Configuration_t& q2((*itn2)->configuration());
         graph::StatePtr_t s2 = getState(graph, *itn2);
         assert(q1 != q2);
 
@@ -445,7 +444,7 @@ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
   std::size_t nbConnection = 0;
   for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
        ++itn1) {
-    const Configuration_t& q1(*(*itn1)->configuration());
+    const Configuration_t& q1((*itn1)->configuration());
     graph::StatePtr_t s1 = getState(graph, *itn1);
 
     for (core::Nodes_t::const_iterator itn2 = std::next(itn1);
@@ -455,7 +454,7 @@ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
       bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
       bool _2to1 = (*itn1)->isInNeighbor(*itn2);
       assert(!_1to2 || !_2to1);
-      const Configuration_t& q2(*(*itn2)->configuration());
+      const Configuration_t& q2((*itn2)->configuration());
       graph::StatePtr_t s2 = getState(graph, *itn2);
       assert(q1 != q2);
 

src/path-planner/end-effector-trajectory.cc

@@ -73,7 +73,7 @@ void EndEffectorTrajectory::startSolve() {
     throw std::runtime_error(msg);
   }
 
-  if (!problem()->initConfig()) {
+  if (problem()->initConfig().size() == 0) {
     std::string msg("No init config in problem.");
     hppDout(error, msg);
     throw std::runtime_error(msg);
@@ -151,8 +151,10 @@ void EndEffectorTrajectory::oneStep() {
 
   core::interval_t timeRange(sm->timeRange());
 
+  // Generate a vector if configuration where the first one is the initial
+  // configuration and the following ones are random configurations
   std::vector<core::Configuration_t> qs(
-      configurations(*problem()->initConfig()));
+      configurations(problem()->initConfig()));
   if (qs.empty()) {
     hppDout(info, "Failed to generate initial configs.");
     return;
@@ -166,12 +168,23 @@ void EndEffectorTrajectory::oneStep() {
   vector_t times(nDiscreteSteps_ + 1);
   matrix_t steps(problem()->robot()->configSize(), nDiscreteSteps_ + 1);
 
+  // Discretize definition interval of the steering method into times
   times[0] = timeRange.first;
   for (int j = 1; j < nDiscreteSteps_; ++j)
     times[j] = timeRange.first +
                j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
   times[nDiscreteSteps_] = timeRange.second;
 
+  // For each random configuration,
+  //   - compute initial configuration of path by projecting the random
+  //     configuration (initial configuration for the first time),
+  //   - compute following samples by projecting current sample after
+  //     updating right hand side.
+  // If failure, try next random configuration.
+  // Failure can be due to
+  //   - projection,
+  //   - collision of final configuration,
+  //   - validation of path (for collision mainly).
   for (i = 0; i < qs.size(); ++i) {
     if (resetRightHandSide) {
       constraints->rightHandSideAt(times[0]);
@@ -200,6 +213,7 @@ void EndEffectorTrajectory::oneStep() {
     if (!success) continue;
     success = false;
 
+    // Test collision of final configuration.
     if (!cfgValidation->validate(steps.col(nDiscreteSteps_), cfgReport)) {
       hppDout(info, "Destination config is in collision.");
       continue;
@@ -220,10 +234,13 @@ void EndEffectorTrajectory::oneStep() {
       continue;
     }
 
-    roadmap()->initNode(make_shared<Configuration_t>(steps.col(0)));
+    // In case of success,
+    //   - insert q_init as initial configuration of the roadmap,
+    //   - insert final configuration as goal node in the roadmap,
+    //   - add a roadmap edge between them and stop.
+    roadmap()->initNode(steps.col(0));
     core::NodePtr_t init = roadmap()->initNode();
-    core::NodePtr_t goal = roadmap()->addGoalNode(
-        make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
+    core::NodePtr_t goal = roadmap()->addGoalNode(steps.col(nDiscreteSteps_));
     roadmap()->addEdge(init, goal, path);
     success = true;
     if (feasibilityOnly_) break;

src/path-planner/transition-planner.cc

+// Copyright (c) 2023 CNRS
+// Authors: Florent Lamiraux
+//
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// 1. Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//
+// 2. Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+// DAMAGE.
+
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/config-validations.hh>
+#include <hpp/core/constraint-set.hh>
+#include <hpp/core/distance/reeds-shepp.hh>
+#include <hpp/core/joint-bound-validation.hh>
+#include <hpp/core/path-optimization/rs-time-parameterization.hh>
+#include <hpp/core/path-optimization/simple-time-parameterization.hh>
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/core/path-planner/bi-rrt-star.hh>
+#include <hpp/core/path-projector.hh>
+#include <hpp/core/path-validation-report.hh>
+#include <hpp/core/path-validation.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/core/steering-method/reeds-shepp.hh>
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/path-planner/transition-planner.hh>
+#include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/roadmap.hh>
+
+namespace hpp {
+namespace manipulation {
+namespace pathPlanner {
+TransitionPlannerPtr_t TransitionPlanner::createWithRoadmap(
+    const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap) {
+  TransitionPlanner* ptr(new TransitionPlanner(problem, roadmap));
+  TransitionPlannerPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+void TransitionPlanner::checkProblemAndForwardParameters() {
+  // Check that edge has been selected
+  // Initialize the planner
+  if (!innerProblem_->constraints() ||
+      !innerProblem_->constraints()->configProjector())
+    throw std::logic_error(
+        "TransitionPlanner::startSolve: inner problem has"
+        " no constraints. You probably forgot to select "
+        "the transition.");
+  // Check that the initial configuration has been initialized
+  if (innerProblem_->initConfig().size() !=
+      innerProblem_->robot()->configSize()) {
+    std::ostringstream os;
+    os << "TransitionPlanner::startSolve: initial configuration size ("
+       << innerProblem_->initConfig().size()
+       << ") differs from robot configuration size ( "
+       << innerProblem_->robot()->configSize() << "). Did you initialize it ?";
+    throw std::logic_error(os.str().c_str());
+  }
+  // Forward maximal number of iterations to inner planner
+  innerPlanner_->maxIterations(this->maxIterations());
+  // Forward timeout to inner planner
+  innerPlanner_->timeOut(this->timeOut());
+}
+
+void TransitionPlanner::startSolve() {
+  checkProblemAndForwardParameters();
+  innerProblem_->constraints()->configProjector()->rightHandSideFromConfig(
+      innerProblem_->initConfig());
+  // Call parent implementation
+  core::PathPlanner::startSolve();
+}
+
+void TransitionPlanner::oneStep() { innerPlanner_->oneStep(); }
+
+core::PathVectorPtr_t TransitionPlanner::planPath(const Configuration_t qInit,
+                                                  matrixIn_t qGoals,
+                                                  bool resetRoadmap) {
+  ConfigProjectorPtr_t configProjector(
+      innerProblem_->constraints()->configProjector());
+  if (configProjector) {
+    configProjector->rightHandSideFromConfig(qInit);
+  }
+  Configuration_t q(qInit);
+  innerProblem_->initConfig(q);
+  innerProblem_->resetGoalConfigs();
+  for (size_type r = 0; r < qGoals.rows(); ++r) {
+    Configuration_t q(qGoals.row(r));
+    if (!configProjector->isSatisfied(q)) {
+      std::ostringstream os;
+      os << "hpp::manipulation::TransitionPlanner::computePath: "
+         << "goal configuration at rank " << r
+         << " does not satisfy the leaf constraint.";
+      throw std::logic_error(os.str().c_str());
+    }
+    innerProblem_->addGoalConfig(q);
+  }
+  if (resetRoadmap) {
+    roadmap()->clear();
+  }
+  checkProblemAndForwardParameters();
+  PathVectorPtr_t path = innerPlanner_->solve();
+  path = optimizePath(path);
+  return path;
+}
+
+core::PathPtr_t TransitionPlanner::directPath(ConfigurationIn_t q1,
+                                              ConfigurationIn_t q2,
+                                              bool validate, bool& success,
+                                              std::string& status) {
+  core::PathPtr_t res(innerProblem_->steeringMethod()->steer(q1, q2));
+  if (!res) {
+    success = false;
+    status = std::string("Steering method failed");
+    return res;
+  }
+  status = std::string("");
+  core::PathProjectorPtr_t pathProjector(innerProblem_->pathProjector());
+  bool success1 = true, success2 = true;
+  core::PathPtr_t projectedPath = res;
+  if (pathProjector) {
+    success1 = pathProjector->apply(res, projectedPath);
+    if (!success1) {
+      status += std::string("Failed to project the path. ");
+    }
+  }
+  core::PathPtr_t validPart = projectedPath;
+  core::PathValidationPtr_t pathValidation(innerProblem_->pathValidation());
+  if (pathValidation && validate) {
+    core::PathValidationReportPtr_t report;
+    success2 =
+        pathValidation->validate(projectedPath, false, validPart, report);
+    if (!success2) {
+      status += std::string("Failed to validate the path. ");
+    }
+  }
+  success = success1 && success2;
+  return validPart;
+}
+
+bool TransitionPlanner::validateConfiguration(
+    ConfigurationIn_t q, std::size_t id,
+    core::ValidationReportPtr_t& report) const {
+  graph::EdgePtr_t edge(getEdgeOrThrow(id));
+  return edge->pathValidation()->validate(q, report);
+}
+
+core::PathVectorPtr_t TransitionPlanner::optimizePath(const PathPtr_t& path) {
+  PathVectorPtr_t pv(HPP_DYNAMIC_PTR_CAST(PathVector, path));
+  if (!pv) {
+    pv = core::PathVector::create(path->outputSize(),
+                                  path->outputDerivativeSize());
+    pv->appendPath(path);
+  }
+  for (auto po : pathOptimizers_) {
+    pv = po->optimize(pv);
+  }
+  return pv;
+}
+
+core::PathVectorPtr_t TransitionPlanner::timeParameterization(
+    const PathVectorPtr_t& path) {
+  return timeParameterization_->optimize(path);
+}
+
+void TransitionPlanner::setEdge(std::size_t id) {
+  graph::EdgePtr_t edge(getEdgeOrThrow(id));
+  innerProblem_->constraints(edge->pathConstraint());
+  innerProblem_->pathValidation(edge->pathValidation());
+  innerProblem_->steeringMethod(edge->steeringMethod());
+}
+
+void TransitionPlanner::setReedsAndSheppSteeringMethod(double turningRadius) {
+  core::JointPtr_t root(innerProblem_->robot()->rootJoint());
+  core::SteeringMethodPtr_t sm(core::steeringMethod::ReedsShepp::create(
+      innerProblem_, turningRadius, root, root));
+  core::DistancePtr_t dist(core::distance::ReedsShepp::create(innerProblem_));
+  innerProblem_->steeringMethod(sm);
+  innerProblem_->distance(dist);
+  timeParameterization_ =
+      core::pathOptimization::RSTimeParameterization::create(innerProblem_);
+}
+
+void TransitionPlanner::pathProjector(const PathProjectorPtr_t pathProjector) {
+  innerProblem_->pathProjector(pathProjector);
+}
+
+void TransitionPlanner::clearPathOptimizers() { pathOptimizers_.clear(); }
+
+/// Add a path optimizer
+void TransitionPlanner::addPathOptimizer(
+    const core::PathOptimizerPtr_t& pathOptimizer) {
+  pathOptimizers_.push_back(pathOptimizer);
+}
+
+void TransitionPlanner::setParameter(const std::string& key,
+                                     const core::Parameter& value) {
+  innerProblem_->setParameter(key, value);
+}
+
+TransitionPlanner::TransitionPlanner(const core::ProblemConstPtr_t& problem,
+                                     const core::RoadmapPtr_t& roadmap)
+    : PathPlanner(problem, roadmap) {
+  ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+  if (!p)
+    throw std::invalid_argument(
+        "The problem should be of type hpp::manipulation::Problem.");
+  // create the inner problem
+  innerProblem_ = core::Problem::create(p->robot());
+  // Pass parameters from manipulation problem
+  std::vector<std::string> keys(
+      p->parameters.getKeys<std::vector<std::string> >());
+  for (auto k : keys) {
+    innerProblem_->setParameter(k, p->parameters.get(k));
+  }
+  // Initialize config validations
+  innerProblem_->clearConfigValidations();
+  innerProblem_->configValidations()->add(
+      hpp::core::CollisionValidation::create(p->robot()));
+  innerProblem_->configValidations()->add(
+      hpp::core::JointBoundValidation::create(p->robot()));
+  // Add obstacles to inner problem
+  innerProblem_->collisionObstacles(p->collisionObstacles());
+  // Create default path planner
+  innerPlanner_ = hpp::core::pathPlanner::BiRrtStar::createWithRoadmap(
+      innerProblem_, roadmap);
+  // Create default time parameterization
+  timeParameterization_ =
+      core::pathOptimization::SimpleTimeParameterization::create(innerProblem_);
+}
+
+void TransitionPlanner::init(TransitionPlannerWkPtr_t weak) {
+  core::PathPlanner::init(weak);
+  weakPtr_ = weak;
+}
+
+graph::EdgePtr_t TransitionPlanner::getEdgeOrThrow(std::size_t id) const {
+  ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem()));
+  assert(p);
+  graph::GraphComponentPtr_t comp(p->constraintGraph()->get(id).lock());
+  graph::EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(graph::Edge, comp));
+  if (!edge) {
+    std::ostringstream os;
+    os << "hpp::manipulation::pathPlanner::TransitionPlanner::setEdge: index "
+       << id << " does not correspond to any edge of the constraint graph.";
+    throw std::logic_error(os.str().c_str());
+  }
+  return edge;
+}
+
+}  // namespace pathPlanner
+}  // namespace manipulation
+}  // namespace hpp

src/problem-solver.cc

@@ -62,6 +62,8 @@
 #include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
 #include "hpp/manipulation/path-optimization/random-shortcut.hh"
 #include "hpp/manipulation/path-planner/end-effector-trajectory.hh"
+#include "hpp/manipulation/path-planner/states-path-finder.hh"
+#include "hpp/manipulation/path-planner/transition-planner.hh"
 #include "hpp/manipulation/problem-target/state.hh"
 #include "hpp/manipulation/problem.hh"
 #include "hpp/manipulation/roadmap.hh"
@@ -123,7 +125,10 @@ ProblemSolver::ProblemSolver() : core::ProblemSolver(), robot_(), problem_() {
   pathPlanners.add("M-RRT", ManipulationPlanner::create);
   pathPlanners.add("EndEffectorTrajectory",
                    pathPlanner::EndEffectorTrajectory::createWithRoadmap);
-
+  pathPlanners.add("StatesPathFinder",
+                   pathPlanner::StatesPathFinder::createWithRoadmap);
+  pathPlanners.add("TransitionPlanner",
+                   pathPlanner::TransitionPlanner::createWithRoadmap);
   pathValidations.add("Graph-Discretized",
                       createDiscretizedCollisionGraphPathValidation);
   pathValidations.add("Graph-DiscretizedCollision",

src/problem-target/astar.hh

@@ -144,13 +144,13 @@ class HPP_MANIPULATION_LOCAL Astar {
   }
 
   value_type heuristic(const RoadmapNodePtr_t node) const {
-    const ConfigurationPtr_t config = node->configuration();
+    const Configuration_t& config = node->configuration();
     value_type res = std::numeric_limits<value_type>::infinity();
     for (core::NodeVector_t::const_iterator itGoal =
              roadmap_->goalNodes().begin();
          itGoal != roadmap_->goalNodes().end(); ++itGoal) {
-      ConfigurationPtr_t goal = (*itGoal)->configuration();
-      value_type dist = (*distance_)(*config, *goal);
+      const Configuration_t& goal = (*itGoal)->configuration();
+      value_type dist = (*distance_)(config, goal);
       if (dist < res) {
         res = dist;
       }

src/roadmap-node.cc

@@ -32,7 +32,7 @@
 
 namespace hpp {
 namespace manipulation {
-RoadmapNode::RoadmapNode(const ConfigurationPtr_t& configuration,
+RoadmapNode::RoadmapNode(ConfigurationIn_t configuration,
                          ConnectedComponentPtr_t cc)
     : core::Node(configuration, cc), state_() {}
 }  // namespace manipulation

src/roadmap.cc

@@ -62,7 +62,7 @@ void Roadmap::clear() {
 
 void Roadmap::push_node(const core::NodePtr_t& n) {
   Parent::push_node(n);
-  const RoadmapNodePtr_t& node = static_cast<const RoadmapNodePtr_t>(n);
+  const RoadmapNodePtr_t& node = static_cast<RoadmapNodePtr_t>(n);
   statInsert(node);
   leafCCs_.insert(node->leafConnectedComponent());
 }
@@ -80,7 +80,7 @@ void Roadmap::insertHistogram(const graph::HistogramPtr_t hist) {
   histograms_.push_back(hist);
   core::Nodes_t::const_iterator _node;
   for (_node = nodes().begin(); _node != nodes().end(); ++_node)
-    hist->add(static_cast<const RoadmapNodePtr_t>(*_node));
+    hist->add(static_cast<RoadmapNodePtr_t>(*_node));
 }
 
 void Roadmap::constraintGraph(const graph::GraphPtr_t& graph) {
@@ -91,7 +91,7 @@ void Roadmap::constraintGraph(const graph::GraphPtr_t& graph) {
 }
 
 RoadmapNodePtr_t Roadmap::nearestNodeInState(
-    const ConfigurationPtr_t& configuration,
+    ConfigurationIn_t configuration,
     const ConnectedComponentPtr_t& connectedComponent,
     const graph::StatePtr_t& state, value_type& minDistance) const {
   core::NodePtr_t result = NULL;
@@ -103,7 +103,7 @@ RoadmapNodePtr_t Roadmap::nearestNodeInState(
   // 		<< std::endl;
   for (RoadmapNodes_t::const_iterator itNode = roadmapNodes.begin();
        itNode != roadmapNodes.end(); ++itNode) {
-    value_type d = (*distance())(*(*itNode)->configuration(), *configuration);
+    value_type d = (*distance())((*itNode)->configuration(), configuration);
     if (d < minDistance) {
       minDistance = d;
       result = *itNode;
@@ -112,7 +112,7 @@ RoadmapNodePtr_t Roadmap::nearestNodeInState(
   return static_cast<RoadmapNode*>(result);
 }
 
-core::NodePtr_t Roadmap::createNode(const ConfigurationPtr_t& q) const {
+core::NodePtr_t Roadmap::createNode(ConfigurationIn_t q) const {
   // call RoadmapNode constructor with new manipulation connected component
   RoadmapNodePtr_t node = new RoadmapNode(q, ConnectedComponent::create(weak_));
   LeafConnectedCompPtr_t sc = WeighedLeafConnectedComp::create(weak_.lock());
@@ -154,8 +154,8 @@ void Roadmap::merge(const LeafConnectedCompPtr_t& cc1,
 
 void Roadmap::impl_addEdge(const core::EdgePtr_t& edge) {
   Parent::impl_addEdge(edge);
-  const RoadmapNodePtr_t& f = static_cast<const RoadmapNodePtr_t>(edge->from());
-  const RoadmapNodePtr_t& t = static_cast<const RoadmapNodePtr_t>(edge->to());
+  const RoadmapNodePtr_t& f = static_cast<RoadmapNodePtr_t>(edge->from());
+  const RoadmapNodePtr_t& t = static_cast<RoadmapNodePtr_t>(edge->to());
   if (f->graphState() == t->graphState()) {
     LeafConnectedCompPtr_t cc1(f->leafConnectedComponent());
     LeafConnectedCompPtr_t cc2(t->leafConnectedComponent());

src/serialization.cc

@@ -27,6 +27,12 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
+#include <boost/serialization/version.hpp>
+#if BOOST_VERSION / 100 % 1000 == 74
+#include <boost/serialization/library_version_type.hpp>
+#endif
+// ref https://github.com/boostorg/serialization/issues/219
+
 #include <boost/serialization/list.hpp>
 #include <boost/serialization/serialization.hpp>
 #include <boost/serialization/set.hpp>
@@ -67,7 +73,7 @@ inline void ConnectedComponent::serialize(Archive& ar,
   if (!Archive::is_saving::value) {
     RoadmapPtr_t roadmap = roadmap_.lock();
     for (const core::NodePtr_t& node : nodes()) {
-      const RoadmapNodePtr_t& n = static_cast<const RoadmapNodePtr_t>(node);
+      const RoadmapNodePtr_t& n = static_cast<RoadmapNodePtr_t>(node);
       graphStateMap_[roadmap->getState(n)].push_back(n);
     }
   }

src/steering-method/cross-state-optimization.cc

@@ -534,7 +534,7 @@ bool CrossStateOptimization::solveOptimizationProblem(
             .intValue());
 
     while (status != Solver_t::SUCCESS && nbTry < nRandomConfigs) {
-      d.waypoint.col(j) = *(problem()->configurationShooter()->shoot());
+      d.waypoint.col(j) = problem()->configurationShooter()->shoot();
       status = solver.solve(d.waypoint.col(j),
                             constraints::solver::lineSearch::Backtracking());
       ++nbTry;

src/steering-method/end-effector-trajectory.cc

@@ -64,7 +64,7 @@ class FunctionFromPath : public constraints::DifferentiableFunction {
 
  protected:
   void impl_compute(core::LiegroupElementRef result, vectorIn_t arg) const {
-    bool success = (*path_)(result.vector(), arg[0]);
+    bool success = path_->eval(result.vector(), arg[0]);
     if (!success) {
       hppDout(warning, "Failed to evaluate path at param "
                            << arg[0] << incindent << iendl << *path_
@@ -140,14 +140,16 @@ PathPtr_t EndEffectorTrajectory::impl_compute(ConfigurationIn_t q1,
     return core::StraightPath::create(problem()->robot(), q1, q2, timeRange_,
                                       c);
   } catch (const std::exception& e) {
-    std::cout << timeRange_.first << ", " << timeRange_.second << '\n';
+    std::ostringstream os;
+    os << "steeringMethod::EndEffectorTrajectory failed: " << e.what()
+       << std::endl;
+    os << "time range: [" << timeRange_.first << ", " << timeRange_.second
+       << "]\n";
     if (eeTraj_)
-      std::cout << (*eeTraj_)(vector_t::Constant(1, timeRange_.first)) << '\n'
-                << (*eeTraj_)(vector_t::Constant(1, timeRange_.second))
-                << std::endl;
-    std::cout << *constraints() << std::endl;
-    std::cout << e.what() << std::endl;
-    throw;
+      os << (*eeTraj_)(vector_t::Constant(1, timeRange_.first)) << '\n'
+         << (*eeTraj_)(vector_t::Constant(1, timeRange_.second)) << std::endl;
+    if (constraints()) os << *constraints() << std::endl;
+    throw std::runtime_error(os.str().c_str());
   }
 }
 

src/weighed-distance.cc

@@ -86,7 +86,7 @@ value_type WeighedDistance::impl_distance(ConfigurationIn_t q1,
 
 value_type WeighedDistance::impl_distance(core::NodePtr_t n1,
                                           core::NodePtr_t n2) const {
-  Configuration_t &q1 = *n1->configuration(), q2 = *n2->configuration();
+  const Configuration_t &q1 = n1->configuration(), q2 = n2->configuration();
   value_type d = core::WeighedDistance::impl_distance(q1, q2);
 
   graph::Edges_t pes =

tests/CMakeLists.txt

@@ -4,31 +4,33 @@
 #
 
 # Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are
-# met:
+# modification, are permitted provided that the following conditions are met:
 #
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
+# 1. Redistributions of source code must retain the above copyright notice, this
+#   list of conditions and the following disclaimer.
 #
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in the
-# documentation and/or other materials provided with the distribution.
+# 1. Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
 #
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
-# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
-# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
-# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
-# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
-# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
-# DAMAGE.
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 # Make Boost.Test generates the main function in test cases.
-ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
+add_definitions(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
 
-ADD_UNIT_TEST(test-constraintgraph test-constraintgraph.cc)
-TARGET_LINK_LIBRARIES(test-constraintgraph ${PROJECT_NAME} Boost::unit_test_framework)
+add_unit_test(test-constraintgraph test-constraintgraph.cc)
+target_link_libraries(test-constraintgraph ${PROJECT_NAME}
+                      Boost::unit_test_framework)
+set_tests_properties(
+  test-constraintgraph
+  PROPERTIES ENVIRONMENT
+             "ROS_PACKAGE_PATH=${example-robot-data_DIR}/../../../share")

tests/test-constraintgraph.cc

@@ -80,12 +80,11 @@ void initialize(bool ur5) {
   hpp::manipulation::ProblemPtr_t problem(
       hpp::manipulation::Problem::create(robot));
   if (ur5) {
-    hpp::pinocchio::urdf::loadModel(
-        robot, 0, "ur5/", "anchor",
-        "package://example-robot-data/robots/ur_description/urdf/"
-        "ur5_joint_limited_robot.urdf",
-        "package://example-robot-data/robots/ur_description/srdf/"
-        "ur5_joint_limited_robot.srdf");
+    hpp::pinocchio::urdf::loadModel(robot, 0, "ur5/", "anchor",
+                                    "package://ur_description/urdf/"
+                                    "ur5_joint_limited_robot.urdf",
+                                    "package://ur_description/srdf/"
+                                    "ur5_joint_limited_robot.srdf");
   }
   SteeringMethodPtr_t sm(
       hpp::manipulation::steeringMethod::Graph::create(problem));