From b34e71d970031f9718cbbda660475ca9cc819c9f Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Thu, 20 Jan 2022 09:49:44 +0000
Subject: [PATCH] [StatesPathFinder] Add missing header.
---
CMakeLists.txt | 1 +
.../path-planner/in-state-path.hh | 135 ++++++++++++++++++
2 files changed, 136 insertions(+)
create mode 100644 include/hpp/manipulation/path-planner/in-state-path.hh
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 35196031..0cf25ec7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -81,6 +81,7 @@ SET(${PROJECT_NAME}_HEADERS
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/problem-target/state.hh
diff --git a/include/hpp/manipulation/path-planner/in-state-path.hh b/include/hpp/manipulation/path-planner/in-state-path.hh
new file mode 100644
index 00000000..4395ed6d
--- /dev/null
+++ b/include/hpp/manipulation/path-planner/in-state-path.hh
@@ -0,0 +1,135 @@
+// 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/path-planner.hh>
+# include <hpp/core/config-projector.hh>
+# include <hpp/core/collision-validation.hh>
+# include <hpp/core/joint-bound-validation.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
--
GitLab