diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh index 85bd8bae314aff606bcbccfa60aa64c4698e3844..9866dfaf8e3fadc975e8129db24b6e4086beb266 100644 --- a/include/hpp/manipulation/path-planner/transition-planner.hh +++ b/include/hpp/manipulation/path-planner/transition-planner.hh @@ -162,6 +162,8 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner { void init(TransitionPlannerWkPtr_t weak); private: + /// Check problem and forward maxIterations and timeout to inner problem. + void checkProblemAndForwardParameters(); /// Get pointer to edge from an id graph::EdgePtr_t getEdgeOrThrow(std::size_t id) const; /// Pointer to the problem of the inner planner diff --git a/src/path-planner/transition-planner.cc b/src/path-planner/transition-planner.cc index befd3333a5de60a94a0f48ef201612e231e59e14..a16c1e1683afb3120f0717d7f74e395f13a63d46 100644 --- a/src/path-planner/transition-planner.cc +++ b/src/path-planner/transition-planner.cc @@ -59,7 +59,7 @@ TransitionPlannerPtr_t TransitionPlanner::createWithRoadmap( return shPtr; } -void TransitionPlanner::startSolve() { +void TransitionPlanner::checkProblemAndForwardParameters() { // Check that edge has been selected // Initialize the planner if (!innerProblem_->constraints() || @@ -68,13 +68,24 @@ void TransitionPlanner::startSolve() { "TransitionPlanner::startSolve: inner problem has" " no constraints. You probably forgot to select " "the transition."); - innerProblem_->constraints()->configProjector()->rightHandSideFromConfig( - innerProblem_->initConfig()); + // 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(); } @@ -106,6 +117,7 @@ core::PathVectorPtr_t TransitionPlanner::planPath(const Configuration_t qInit, if (resetRoadmap) { roadmap()->clear(); } + checkProblemAndForwardParameters(); PathVectorPtr_t path = innerPlanner_->solve(); path = optimizePath(path); return path;