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

@@ -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());

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

@@ -30,3 +30,7 @@ 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)
+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));