diff --git a/CMakeLists.txt b/CMakeLists.txt
index b113c989e0f693a728f9f12a0d3c1fe1ea94c566..640dbb05e9ddf55c21474d7ab7eb24c7ecec4517 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -86,8 +86,8 @@ set(${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/path-planner/end-effector-trajectory.hh
- include/hpp/manipulation/path-planner/in-state-path.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/serialization.hh
include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -123,6 +123,7 @@ set(${PROJECT_NAME}_SOURCES
src/path-optimization/enforce-transition-semantic.cc
src/path-planner/end-effector-trajectory.cc
src/path-planner/states-path-finder.cc
+ src/path-planner/transition-planner.cc
src/problem-target/state.cc
src/serialization.cc
src/steering-method/end-effector-trajectory.cc
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 59829be006f44220b0970a9e5ee6babf4aa3b671..6f257c787215fd10342e26c29dce8b0e32c45c75 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -104,6 +104,8 @@ HPP_PREDEF_CLASS(InStatePath);
typedef shared_ptr<InStatePath> InStatePathPtr_t;
HPP_PREDEF_CLASS(StateShooter);
typedef shared_ptr<StateShooter> StateShooterPtr_t;
+HPP_PREDEF_CLASS(TransitionPlanner);
+typedef shared_ptr<TransitionPlanner> TransitionPlannerPtr_t;
} // namespace pathPlanner
HPP_PREDEF_CLASS(GraphPathValidation);
typedef shared_ptr<GraphPathValidation> GraphPathValidationPtr_t;
diff --git a/include/hpp/manipulation/path-planner/in-state-path.hh b/include/hpp/manipulation/path-planner/in-state-path.hh
deleted file mode 100644
index 8e53d2ba87b3f40287bc791b5cd0fcf5ec24fa91..0000000000000000000000000000000000000000
--- a/include/hpp/manipulation/path-planner/in-state-path.hh
+++ /dev/null
@@ -1,128 +0,0 @@
-// Copyright (c) 2021, Joseph Mirabel
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
-// Florent Lamiraux (florent.lamiraux@laas.fr)
-// Alexandre Thiault (athiault@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_PATH_PLANNER_IN_STATE_PATH_HH
-#define HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
-
-#include <hpp/core/collision-validation.hh>
-#include <hpp/core/config-projector.hh>
-#include <hpp/core/joint-bound-validation.hh>
-#include <hpp/core/path-planner.hh>
-#include <hpp/manipulation/config.hh>
-#include <hpp/manipulation/fwd.hh>
-#include <hpp/manipulation/problem.hh>
-#include <hpp/manipulation/steering-method/graph.hh>
-
-namespace hpp {
-namespace manipulation {
-namespace pathPlanner {
-/// \addtogroup path_planner
-/// \{
-
-///
-/// Path planner designed to build a path between two configurations
-/// on the same leaf of a given state graph edge
-class HPP_MANIPULATION_DLLAPI InStatePath : public core::PathPlanner {
- public:
- virtual ~InStatePath() {}
-
- static InStatePathPtr_t create(const core::ProblemConstPtr_t& problem);
-
- static InStatePathPtr_t createWithRoadmap(
- const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap);
-
- InStatePathPtr_t copy() const;
-
- void maxIterPlanner(const unsigned long& maxiter);
- void timeOutPlanner(const double& timeout);
- void resetRoadmap(const bool& resetroadmap);
- void plannerType(const std::string& plannertype);
- void addOptimizerType(const std::string& opttype);
- void resetOptimizerTypes();
-
- /// Set the edge along which q_init and q_goal will be linked.
- /// Use setEdge before setInit and setGoal.
- void setEdge(const graph::EdgePtr_t& edge);
- void setInit(const ConfigurationPtr_t& q);
- void setGoal(const ConfigurationPtr_t& q);
-
- virtual void oneStep() {}
- virtual core::PathVectorPtr_t solve();
-
- const core::RoadmapPtr_t& leafRoadmap() const;
- void mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const;
-
- protected:
- InStatePath(const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap)
- : PathPlanner(problem),
- problem_(HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
- leafRoadmap_(roadmap),
- constraints_(),
- weak_() {
- const core::DevicePtr_t& robot = problem_->robot();
- leafProblem_ = core::Problem::create(robot);
- leafProblem_->setParameter("kPRM*/numberOfNodes",
- core::Parameter((size_type)2000));
- leafProblem_->clearConfigValidations();
- leafProblem_->addConfigValidation(core::CollisionValidation::create(robot));
- leafProblem_->addConfigValidation(
- core::JointBoundValidation::create(robot));
- for (const core::CollisionObjectPtr_t& obs :
- problem_->collisionObstacles()) {
- leafProblem_->addObstacle(obs);
- }
- leafProblem_->pathProjector(problem->pathProjector());
- }
-
- InStatePath(const InStatePath& other)
- : PathPlanner(other.problem_),
- problem_(other.problem_),
- leafProblem_(other.leafProblem_),
- leafRoadmap_(other.leafRoadmap_),
- constraints_(other.constraints_),
- weak_() {}
-
- void init(InStatePathWkPtr_t weak) { weak_ = weak; }
-
- private:
- // a pointer to the problem used to create the InStatePath instance
- ProblemConstPtr_t problem_;
- // a new problem created for this InStatePath instance
- core::ProblemPtr_t leafProblem_;
- core::RoadmapPtr_t leafRoadmap_;
- ConstraintSetPtr_t constraints_;
-
- double timeOutPathPlanning_ = 0;
- unsigned long maxIterPathPlanning_ = 0;
- bool resetRoadmap_ = true;
- std::string plannerType_ = "BiRRT*";
- std::vector<std::string> optimizerTypes_;
-
- /// Weak pointer to itself
- InStatePathWkPtr_t weak_;
-
-}; // class InStatePath
-/// \}
-
-} // namespace pathPlanner
-} // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh
new file mode 100644
index 0000000000000000000000000000000000000000..b47aaf45ec474078dfb841aa4e4bd1a2100c51fe
--- /dev/null
+++ b/include/hpp/manipulation/path-planner/transition-planner.hh
@@ -0,0 +1,175 @@
+// 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.
+
+#ifndef HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
+#define HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
+
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/core/path-planner.hh>
+
+namespace hpp {
+namespace manipulation {
+namespace pathPlanner {
+
+ /// \addtogroup path_planner
+ /// \{
+
+ /// Plan paths in a leaf of a transition
+ ///
+ /// In many manipulation applications, the sequence of actions is knwown in
+ /// advance or computed by a task planner. There is a need then to connect
+ /// configurations that lie on the same leaf of a transition. This class
+ /// performs this computation.
+ ///
+ /// The constraint graph is stored in the Problem instance of the planner.
+ /// To select the transition, call method \link TransitionPlanner::setEdge
+ /// setEdge \endlink with the index of the transition.
+ ///
+ /// At construction, a core::Problem instance is created, as well as a
+ /// core::PathPlanner instance. They are respectively called the inner problem
+ /// and the inner planner.
+ ///
+ /// The leaf of the transition is defined by the initial configuration passed
+ /// to method \link TransitionPlanner::planPath planPath \endlink.
+ /// The right hand side of the inner problem constraints is initialized with
+ /// this configuration.
+ ///
+ /// The class stores path optimizers that are called when invoking method
+ /// \link TransitionPlanner::optimizePath optimizePath \endlink.
+ ///
+ /// Method \link TransitionPlanner::timeParameterization timeParameterization
+ /// \endlink computes a time parameterization of a given path.
+ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
+ public:
+ typedef core::PathPlannerPtr_t PathPlannerPtr_t;
+ typedef core::PathProjectorPtr_t PathProjectorPtr_t;
+ typedef core::PathPtr_t PathPtr_t;
+ typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
+ typedef core::PathVector PathVector;
+ typedef core::PathVectorPtr_t PathVectorPtr_t;
+ typedef core::Parameter Parameter;
+
+ /// Create instance and return share pointer
+ static TransitionPlannerPtr_t createWithRoadmap
+ (const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap);
+ /// Get the inner planner
+ PathPlannerPtr_t innerPlanner() const {
+ return innerPlanner_;
+ }
+ /// Set the inner planner
+ void innerPlanner(const PathPlannerPtr_t& planner) {
+ innerPlanner_ = planner;
+ }
+ /// Get the inner problem
+ core::ProblemPtr_t innerProblem() const {
+ return innerProblem_;
+ }
+ /// Initialize path planning
+ /// Set right hand side of problem constraints with initial configuration
+ virtual void startSolve();
+
+ /// One step of the planner
+ /// Calls the same method of the internally stored planner
+ virtual void oneStep();
+
+ /// Solve the problem defined by input configurations
+ /// \param qInit initial configuration,
+ /// \param qGoals, goal configurations,
+ /// \param resetRoadmap whether to reset the roadmap
+ PathVectorPtr_t planPath(const Configuration_t qInit,
+ matrixIn_t qGoals, bool resetRoadmap);
+ /// Call the steering method between two configurations
+ /// \param q1, q2 the start and end configurations,
+ /// \param validate whether resulting path should be tested for collision
+ /// \retval success True if path has been computed and validated
+ /// successfully
+ /// \retval status a message in case of failure.
+ /// If a path projector has been selected, the path is tested for
+ /// continuity.
+ PathPtr_t directPath(const Configuration_t& q1,
+ const Configuration_t& q2, bool validate, bool& success,
+ std::string& status);
+ /// Optimize path using the selected path optimizers
+ /// \param path input path
+ /// \return optimized path
+ PathVectorPtr_t optimizePath(const PathPtr_t& path);
+
+ /// Compute time parameterization of path
+ /// \param path input path
+ /// \return time parameterized trajectory
+ /// Uses core::pathOptimization::SimpleTimeParameterization.
+ PathVectorPtr_t timeParameterization(const PathVectorPtr_t& path);
+
+ /// Set transition along which we wish to plan a path
+ /// \param id index of the edge in the constraint graph
+ void setEdge(std::size_t id);
+
+ /// Create a Reeds and Shepp steering method and path it to the problem.
+ void setReedsAndSheppSteeringMethod(double turningRadius);
+
+ /// Set the path projector
+ void pathProjector(const PathProjectorPtr_t pathProjector);
+
+ /// Clear path optimizers
+ void clearPathOptimizers();
+
+ /// Add a path optimizer
+ void addPathOptimizer(const PathOptimizerPtr_t& pathOptimizer);
+
+ /// Set parameter to the inner problem
+ void setParameter(const std::string& key, const Parameter& value);
+
+ protected:
+ /// Constructor
+ /// Cast problem into manipulation::Problem and store shared pointer
+ ///
+ /// Create an inner problem and forward the following elements of the input
+ /// problem to the inner problem:
+ /// \li parameters,
+ /// \li collision obstacles.
+ /// Add instances of core::CollisionValidation and
+ /// core::JointBoundValidation to the inner problem.
+ TransitionPlanner(const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap);
+ /// store weak pointer to itself
+ void init(TransitionPlannerWkPtr_t weak);
+ private:
+ /// Pointer to the problem of the inner planner
+ core::ProblemPtr_t innerProblem_;
+ /// Pointer to the inner path planner
+ PathPlannerPtr_t innerPlanner_;
+ /// Vector of optimizers to call after solve
+ std::vector<PathOptimizerPtr_t> pathOptimizers_;
+ /// weak pointer to itself
+ TransitionPlannerWkPtr_t weakPtr_;
+ }; // class TransitionPlanner
+} // namespace hpp
+} // namespace manipulation
+} // namespace pathPlanner
+#endif // HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index b91c9c28e414c31ca09eb3ba995133e27aa5179f..4f79a8ea31f67aefeac4b7fbe5c2440675140153 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -36,7 +36,6 @@
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/state-selector.hh>
#include <hpp/manipulation/graph/state.hh>
-#include <hpp/manipulation/path-planner/in-state-path.hh>
#include <hpp/manipulation/path-planner/states-path-finder.hh>
#include <hpp/manipulation/roadmap.hh>
#include <hpp/pinocchio/configuration.hh>
diff --git a/src/path-planner/transition-planner.cc b/src/path-planner/transition-planner.cc
new file mode 100644
index 0000000000000000000000000000000000000000..66048cdbfe8541142fcfc0eb6310301d4e56f6d3
--- /dev/null
+++ b/src/path-planner/transition-planner.cc
@@ -0,0 +1,264 @@
+// 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/path-planner/bi-rrt-star.hh>
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/constraint-set.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/config-validations.hh>
+#include <hpp/core/distance/reeds-shepp.hh>
+#include <hpp/core/joint-bound-validation.hh>
+#include <hpp/core/path-optimization/simple-time-parameterization.hh>
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/core/path-projector.hh>
+#include <hpp/core/path-validation.hh>
+#include <hpp/core/path-validation-report.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/core/steering-method/reeds-shepp.hh>
+
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/graph/edge.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::startSolve()
+ {
+ // 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.");
+ innerProblem_->constraints()->configProjector()->rightHandSideFromConfig
+ (*(innerProblem_->initConfig()));
+ // Forward maximal number of iterations to inner planner
+ innerPlanner_->maxIterations(this->maxIterations());
+ // Forward timeout to inner planner
+ innerPlanner_->timeOut(this->timeOut());
+
+ // 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);
+ }
+ ConfigurationPtr_t q(new Configuration_t(qInit));
+ innerProblem_->initConfig(q);
+ innerProblem_->resetGoalConfigs();
+ for (size_type r=0; r < qGoals.rows(); ++r) {
+ ConfigurationPtr_t q(new Configuration_t(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();
+ }
+ PathVectorPtr_t path = innerPlanner_->solve();
+ path = optimizePath(path);
+ return path;
+ }
+
+ core::PathPtr_t TransitionPlanner::directPath
+ (const Configuration_t& q1, const Configuration_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;
+ }
+
+ 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)
+ {
+ core::PathOptimizerPtr_t tp
+ (core::pathOptimization::SimpleTimeParameterization::create
+ (innerProblem_));
+ return tp->optimize(path);
+ }
+
+ void TransitionPlanner::setEdge(std::size_t id)
+ {
+ 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());
+ }
+ 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);
+ }
+
+ 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);
+ }
+
+ void TransitionPlanner::init(TransitionPlannerWkPtr_t weak)
+ {
+ core::PathPlanner::init(weak);
+ weakPtr_ = weak;
+ }
+
+} // namespace hpp
+} // namespace manipulation
+} // namespace pathPlanner
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index ac5d640db353717333004905f07048c88e75c59d..73af6d7b5455d3da265b0e4d9667d064ca9b6d31 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -62,8 +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/in-state-path.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"
@@ -127,6 +127,8 @@ ProblemSolver::ProblemSolver() : core::ProblemSolver(), robot_(), problem_() {
pathPlanner::EndEffectorTrajectory::createWithRoadmap);
pathPlanners.add("StatesPathFinder",
pathPlanner::StatesPathFinder::createWithRoadmap);
+ pathPlanners.add("TransitionPlanner",
+ pathPlanner::TransitionPlanner::createWithRoadmap);
pathValidations.add("Graph-Discretized",
createDiscretizedCollisionGraphPathValidation);
pathValidations.add("Graph-DiscretizedCollision",