From 705cfdaa8738ffb0cd482a2df589f6f57ae783b4 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Fri, 22 Sep 2023 15:54:47 +0000
Subject: [PATCH] [pre-commit.ci] auto fixes from pre-commit.com hooks
for more information, see https://pre-commit.ci
---
.../path-planner/transition-planner.hh | 270 ++++++------
src/path-planner/transition-planner.cc | 389 +++++++++---------
2 files changed, 318 insertions(+), 341 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh
index b47aaf45..6e41cc97 100644
--- a/include/hpp/manipulation/path-planner/transition-planner.hh
+++ b/include/hpp/manipulation/path-planner/transition-planner.hh
@@ -29,147 +29,143 @@
#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>
+#include <hpp/manipulation/fwd.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.
+/// \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
///
- /// 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
+ /// 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 pathPlanner
+} // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
diff --git a/src/path-planner/transition-planner.cc b/src/path-planner/transition-planner.cc
index 66048cdb..53f415d4 100644
--- a/src/path-planner/transition-planner.cc
+++ b/src/path-planner/transition-planner.cc
@@ -26,24 +26,23 @@
// 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/constraint-set.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-planner/bi-rrt-star.hh>
#include <hpp/core/path-projector.hh>
-#include <hpp/core/path-validation.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/graph.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>
@@ -51,214 +50,196 @@
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();
+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);
}
-
- 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) {
+ 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::pathPlanner::TransitionPlanner::setEdge: index "
- << id << " does not correspond to any edge of the constraint graph.";
+ 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_->constraints(edge->pathConstraint());
- innerProblem_->pathValidation(edge->pathValidation());
- innerProblem_->steeringMethod(edge->steeringMethod());
+ innerProblem_->addGoalConfig(q);
}
-
- 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);
+ if (resetRoadmap) {
+ roadmap()->clear();
}
-
- void TransitionPlanner::pathProjector(const PathProjectorPtr_t pathProjector)
- {
- innerProblem_->pathProjector(pathProjector);
+ 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;
}
-
- void TransitionPlanner::clearPathOptimizers()
- {
- pathOptimizers_.clear();
+ 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. ");
+ }
}
-
- /// Add a path optimizer
- void TransitionPlanner::addPathOptimizer
- (const core::PathOptimizerPtr_t& pathOptimizer)
- {
- pathOptimizers_.push_back(pathOptimizer);
+ 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. ");
+ }
}
-
- void TransitionPlanner::setParameter(const std::string& key,
- const core::Parameter& value)
- {
- innerProblem_->setParameter(key, value);
+ 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);
}
-
- 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);
+ for (auto po : pathOptimizers_) {
+ pv = po->optimize(pv);
}
-
- void TransitionPlanner::init(TransitionPlannerWkPtr_t weak)
- {
- core::PathPlanner::init(weak);
- weakPtr_ = weak;
+ 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());
}
-
-} // namespace hpp
-} // namespace manipulation
-} // namespace pathPlanner
+ 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 pathPlanner
+} // namespace manipulation
+} // namespace hpp
--
GitLab