Start binding hpp::manipulation path planner

include/pyhpp/core/fwd.hh

@@ -59,6 +59,10 @@ void exposePathPlanner();
 void exposeParameter();
 void exposeProblem();
 void exposeProblemSolver();
+
+// forward declaration of some classes
+class PathPlanner;
+class Problem;
 }  // namespace core
 }  // namespace pyhpp
 

include/pyhpp/manipulation/fwd.hh

@@ -37,6 +37,7 @@ namespace manipulation {
 
 struct Device;
 struct Problem;
+struct PyWEdge;
 struct PyWGraph;
 struct GraphSteeringMethod;
 
@@ -49,6 +50,7 @@ typedef std::shared_ptr<GraphSteeringMethod> PyWGraphSteeringMethodPtr_t;
 void exposeDevice();
 void exposeHandle();
 void exposeGraph();
+void exposePathPlanners();
 void exposeProblem();
 void exposePathProjector();
 void exposeGraphSteeringMethod();

src/CMakeLists.txt

@@ -153,6 +153,8 @@ add_python_library(
   pyhpp/manipulation/device.cc
   pyhpp/manipulation/graph.cc
   pyhpp/manipulation/problem.cc
+  pyhpp/manipulation/path-planner.cc
+  pyhpp/manipulation/path-planner.hh
   pyhpp/manipulation/path-projector.cc
   pyhpp/manipulation/steering-method.cc
   LINK_LIBRARIES

src/pyhpp/core/problem-solver.cc

@@ -69,16 +69,6 @@ static PathVectorPtr_t path(ProblemSolver& ps, const std::size_t& i) {
   if (i > ps.paths().size()) throw std::invalid_argument("Out of range");
   return ps.paths()[i];
 }
-static tuple pathValidationType(const ProblemSolver& ps) {
-  value_type tol;
-  std::string type = ps.pathValidationType(tol);
-  return boost::python::make_tuple(type, tol);
-}
-static tuple pathProjectorType(const ProblemSolver& ps) {
-  value_type tol;
-  std::string type = ps.pathProjectorType(tol);
-  return boost::python::make_tuple(type, tol);
-}
 
 template <typename Type, typename TypePtr_t = hpp::shared_ptr<Type> >
 struct Builder {

src/pyhpp/manipulation/bindings.cc

@@ -42,4 +42,5 @@ BOOST_PYTHON_MODULE(bindings) {
   pyhpp::manipulation::exposeGraph();
   pyhpp::manipulation::exposePathProjector();
   pyhpp::manipulation::exposeGraphSteeringMethod();
+  pyhpp::manipulation::exposePathPlanners();
 }

src/pyhpp/manipulation/graph.cc

@@ -757,7 +757,6 @@ ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition,
   Configuration_t output(input);
   bool success(true);
 
-  value_type dist = 0;
   success = transition->obj->generateTargetConfig(q_rhs, output);
 
   hpp::core::ConfigProjectorPtr_t configProjector(

src/pyhpp/manipulation/graph.hh

@@ -36,6 +36,8 @@
 namespace pyhpp {
 namespace manipulation {
 
+typedef hpp::manipulation::Configuration_t Configuration_t;
+typedef hpp::manipulation::ConfigurationIn_t ConfigurationIn_t;
 typedef hpp::manipulation::size_type size_type;
 typedef hpp::manipulation::value_type value_type;
 typedef hpp::manipulation::graph::GraphComponent GraphComponent;

src/pyhpp/manipulation/path-planner.cc

@@ -0,0 +1,174 @@
+//
+// Copyright (c) 2025, 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 <../src/pyhpp/manipulation/graph.hh>
+#include <../src/pyhpp/manipulation/path-planner.hh>
+#include <hpp/manipulation/path-planner/transition-planner.hh>
+#include <hpp/pinocchio/configuration.hh>
+#include <pyhpp/core/path-planner.hh>
+#include <pyhpp/core/problem.hh>
+
+namespace pyhpp {
+namespace manipulation {
+
+void exposePathPlanners() {
+  boost::python::class_<TransitionPlanner,
+                        boost::python::bases<pyhpp::core::PathPlanner> >(
+      "TransitionPlanner", boost::python::init<const pyhpp::core::Problem&>())
+      .def("innerPlanner",
+           static_cast<pyhpp::core::PathPlanner (TransitionPlanner::*)() const>(
+               &TransitionPlanner::innerPlanner))
+      .def("innerPlanner", static_cast<void (TransitionPlanner::*)(
+                               const pyhpp::core::PathPlanner&)>(
+                               &TransitionPlanner::innerPlanner))
+      .def("innerProblem", &TransitionPlanner::innerProblem)
+      .def("planPath", &TransitionPlanner::planPath)
+      .def("directPath", &TransitionPlanner::directPath)
+      .def("validateConfiguration", &TransitionPlanner::validateConfiguration)
+      .def("optimizePath", &TransitionPlanner::optimizePath)
+      .def("timeParameterization", &TransitionPlanner::timeParameterization)
+      .def("setEdge", &TransitionPlanner::setEdge)
+      .def("setReedsAndSheppSteeringMethod",
+           &TransitionPlanner::setReedsAndSheppSteeringMethod)
+      .def("pathProjector", &TransitionPlanner::pathProjector)
+      .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers)
+      .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer);
+}
+
+TransitionPlanner::TransitionPlanner(const pyhpp::core::Problem& problem) {
+  obj = hpp::manipulation::pathPlanner::TransitionPlanner::createWithRoadmap(
+      problem.obj, hpp::core::Roadmap::create(problem.obj->distance(),
+                                              problem.obj->robot()));
+}
+
+hpp::manipulation::pathPlanner::TransitionPlannerPtr_t
+TransitionPlanner::trObj() const {
+  assert(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::pathPlanner::TransitionPlanner,
+                              obj));
+  return HPP_STATIC_PTR_CAST(hpp::manipulation::pathPlanner::TransitionPlanner,
+                             obj);
+}
+
+pyhpp::core::PathPlanner TransitionPlanner::innerPlanner() const {
+  pyhpp::core::PathPlanner pathPlanner;
+  pathPlanner.obj = trObj()->innerPlanner();
+  return pathPlanner;
+}
+
+void TransitionPlanner::innerPlanner(const pyhpp::core::PathPlanner& planner) {
+  trObj()->innerPlanner(planner.obj);
+}
+pyhpp::core::Problem TransitionPlanner::innerProblem() const {
+  return pyhpp::core::Problem(trObj()->innerProblem());
+}
+
+PathVectorPtr_t TransitionPlanner::planPath(ConfigurationIn_t qInit,
+                                            matrixIn_t qGoals,
+                                            bool resetRoadmap) {
+  // Check sizes of initial and goal configurations
+  if (qInit.rows() != obj->problem()->robot()->configSize()) {
+    std::ostringstream os;
+    os << "qInit = " << hpp::pinocchio::displayConfig(qInit)
+       << "should be of size " << obj->problem()->robot()->configSize() << ".";
+    throw std::logic_error(os.str().c_str());
+  }
+  if (qGoals.cols() != obj->problem()->robot()->configSize()) {
+    std::ostringstream os;
+    os << "qGoals = " << qGoals << "should have "
+       << obj->problem()->robot()->configSize() << " columns.";
+    throw std::logic_error(os.str().c_str());
+  }
+  if (qGoals.rows() < 1) {
+    std::ostringstream os;
+    os << "qGoals = " << qGoals << "should have at least one line.";
+    throw std::logic_error(os.str().c_str());
+  }
+  return trObj()->planPath(qInit, qGoals, resetRoadmap);
+}
+
+tuple TransitionPlanner::directPath(ConfigurationIn_t q1, ConfigurationIn_t q2,
+                                    bool validate) {
+  // Check sizes of initial and goal configurations
+  if (q1.rows() != obj->problem()->robot()->configSize()) {
+    std::ostringstream os;
+    os << "q1 = " << hpp::pinocchio::displayConfig(q1) << "should be of size "
+       << obj->problem()->robot()->configSize() << ".";
+    throw std::logic_error(os.str().c_str());
+  }
+  if (q2.rows() != obj->problem()->robot()->configSize()) {
+    std::ostringstream os;
+    os << "q2 = " << hpp::pinocchio::displayConfig(q2) << "should be of size "
+       << obj->problem()->robot()->configSize() << ".";
+    throw std::logic_error(os.str().c_str());
+  }
+  bool success;
+  std::string status;
+  PathPtr_t path = trObj()->directPath(q1, q2, validate, success, status);
+  return boost::python::make_tuple(success, path, status);
+}
+
+tuple TransitionPlanner::validateConfiguration(ConfigurationIn_t q,
+                                               std::size_t id) const {
+  hpp::core::ValidationReportPtr_t report;
+  bool res = trObj()->validateConfiguration(q, id, report);
+  return boost::python::make_tuple(res, report);
+}
+
+PathVectorPtr_t TransitionPlanner::optimizePath(const PathPtr_t& path) {
+  return trObj()->optimizePath(path);
+}
+
+PathVectorPtr_t TransitionPlanner::timeParameterization(
+    const PathVectorPtr_t& path) {
+  return trObj()->timeParameterization(path);
+}
+
+void TransitionPlanner::setEdge(const PyWEdge& transition) {
+  trObj()->setEdge(transition.obj);
+}
+
+void TransitionPlanner::setReedsAndSheppSteeringMethod(double turningRadius) {
+  trObj()->setReedsAndSheppSteeringMethod(turningRadius);
+}
+
+void TransitionPlanner::pathProjector(const PathProjectorPtr_t pathProjector) {
+  trObj()->pathProjector(pathProjector);
+}
+
+void TransitionPlanner::clearPathOptimizers() {
+  trObj()->clearPathOptimizers();
+}
+
+void TransitionPlanner::addPathOptimizer(
+    const PathOptimizerPtr_t& pathOptimizer) {
+  trObj()->addPathOptimizer(pathOptimizer);
+}
+
+}  // namespace manipulation
+}  // namespace pyhpp

src/pyhpp/manipulation/path-planner.hh

@@ -0,0 +1,69 @@
+//
+// Copyright (c) 2025, 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 PYHPP_MANIPULATION_PATH_PLANNER_HH
+#define PYHPP_MANIPULATION_PATH_PLANNER_HH
+
+#include <hpp/manipulation/fwd.hh>
+#include <pyhpp/core/path-planner.hh>
+
+namespace pyhpp {
+namespace manipulation {
+
+typedef boost::python::tuple tuple;
+typedef hpp::core::ConfigurationIn_t ConfigurationIn_t;
+typedef hpp::core::PathOptimizerPtr_t PathOptimizerPtr_t;
+typedef hpp::core::PathProjectorPtr_t PathProjectorPtr_t;
+typedef hpp::core::PathPtr_t PathPtr_t;
+typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
+typedef hpp::core::RoadmapPtr_t RoadmapPtr_t;
+typedef hpp::manipulation::matrixIn_t matrixIn_t;
+
+struct TransitionPlanner : public pyhpp::core::PathPlanner {
+  // Dynamic cast pointer into TransitionPlanner
+  hpp::manipulation::pathPlanner::TransitionPlannerPtr_t trObj() const;
+  TransitionPlanner(const pyhpp::core::Problem& problem);
+  pyhpp::core::PathPlanner innerPlanner() const;
+  void innerPlanner(const pyhpp::core::PathPlanner& planner);
+  pyhpp::core::Problem innerProblem() const;
+  PathVectorPtr_t planPath(ConfigurationIn_t qInit, matrixIn_t qGoals,
+                           bool resetRoadmap);
+  tuple directPath(ConfigurationIn_t q1, ConfigurationIn_t q2, bool validate);
+  tuple validateConfiguration(ConfigurationIn_t q, std::size_t id) const;
+  PathVectorPtr_t optimizePath(const PathPtr_t& path);
+  PathVectorPtr_t timeParameterization(const PathVectorPtr_t& path);
+  void setEdge(const PyWEdge& transition);
+  void setReedsAndSheppSteeringMethod(double turningRadius);
+  void pathProjector(const PathProjectorPtr_t pathProjector);
+  void clearPathOptimizers();
+  void addPathOptimizer(const PathOptimizerPtr_t& pathOptimizer);
+};  // struct TransitionPlanner
+}  // namespace manipulation
+}  // namespace pyhpp
+#endif  // PYHPP_MANIPULATION_PATH_PLANNER_HH

src/pyhpp/manipulation/steering-method.hh

@@ -16,8 +16,8 @@
 // received a copy of the GNU Lesser General Public License along with
 // hpp-python. If not, see <http://www.gnu.org/licenses/>.
 
-#ifndef PYHPP_STEERING_METHOD_HH
-#define PYHPP_STEERING_METHOD_HH
+#ifndef PYHPP_MANIPULATION_STEERING_METHOD_HH
+#define PYHPP_MANIPULATION_STEERING_METHOD_HH
 
 #include <hpp/core/steering-method.hh>
 #include <hpp/manipulation/steering-method/graph.hh>
@@ -38,4 +38,4 @@ struct GraphSteeringMethod {
 }  // namespace manipulation
 }  // namespace pyhpp
 
-#endif  // PYHPP_STEERING_METHOD_HH
+#endif  // PYHPP_MANIPULATION_STEERING_METHOD_HH