Expose MultiPlanner to Python (moveit#474)

Co-authored-by: Robert Haschke <[email protected]>

core/python/bindings/src/core.cpp

@@ -33,6 +33,7 @@
  *********************************************************************/
 
 #include "core.h"
+#include "utils.h"
 #include <pybind11/stl.h>
 #include <pybind11/functional.h>
 #include <moveit/python/task_constructor/properties.h>
@@ -52,23 +53,6 @@ namespace python {
 
 namespace {
 
-// utility function to normalize index: negative indeces reference from the end
-size_t normalize_index(size_t size, long index) {
-	if (index < 0)
-		index += size;
-	if (index >= long(size) || index < 0)
-		throw pybind11::index_error("Index out of range");
-	return index;
-}
-
-// implement operator[](index)
-template <typename T>
-typename T::value_type get_item(const T& container, long index) {
-	auto it = container.begin();
-	std::advance(it, normalize_index(container.size(), index));
-	return *it;
-}
-
 py::list getForwardedProperties(const Stage& self) {
 	py::list l;
 	for (const std::string& value : self.forwardedProperties())

core/python/bindings/src/solvers.cpp

@@ -36,7 +36,9 @@
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
+#include <moveit/task_constructor/solvers/multi_planner.h>
 #include <moveit_msgs/WorkspaceParameters.h>
+#include "utils.h"
 
 namespace py = pybind11;
 using namespace py::literals;
@@ -47,6 +49,7 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(PlannerInterface)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(PipelinePlanner)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(JointInterpolationPlanner)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(CartesianPath)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(MultiPlanner)
 
 namespace moveit {
 namespace python {
@@ -116,6 +119,27 @@ void export_solvers(py::module& m) {
 	        "This values specifies the fraction of mean acceptable joint motion per step.")
 	    .property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
 	    .def(py::init<>());
+
+	properties::class_<MultiPlanner, PlannerInterface>(m, "MultiPlanner", R"(
+			A meta planner that runs multiple alternative planners in sequence and returns the first found solution. ::
+
+				from moveit.task_constructor import core
+
+				# Instantiate MultiPlanner
+				multiPlanner = core.MultiPlanner()
+		)")
+	    .def("__len__", &MultiPlanner::size)
+	    .def("__getitem__", &get_item<MultiPlanner>)
+	    .def(
+	        "add",
+	        [](MultiPlanner& self, const py::args& args) {
+		        for (auto it = args.begin(), end = args.end(); it != end; ++it)
+			        self.push_back(it->cast<PlannerInterfacePtr>());
+	        },
+	        "Insert one or more planners")
+	    .def(
+	        "clear", [](MultiPlanner& self) { self.clear(); }, "Remove all planners")
+	    .def(py::init<>());
 }
 }  // namespace python
 }  // namespace moveit

core/python/bindings/src/utils.h

@@ -0,0 +1,26 @@
+#include <pybind11/pybind11.h>
+
+namespace moveit {
+namespace python {
+namespace {
+
+// utility function to normalize index: negative indeces reference from the end
+size_t normalize_index(size_t size, long index) {
+	if (index < 0)
+		index += size;
+	if (index >= long(size) || index < 0)
+		throw pybind11::index_error("Index out of range");
+	return index;
+}
+
+// implement operator[](index)
+template <typename T>
+typename T::value_type get_item(const T& container, long index) {
+	auto it = container.begin();
+	std::advance(it, normalize_index(container.size(), index));
+	return *it;
+}
+
+}  // namespace
+}  // namespace python
+}  // namespace moveit

demo/scripts/multi_planner.py

@@ -0,0 +1,68 @@
+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from moveit.task_constructor import core, stages
+from py_binding_tools import roscpp_init
+import time
+
+roscpp_init("mtc_tutorial")
+
+ompl_pipelinePlanner = core.PipelinePlanner("ompl")
+ompl_pipelinePlanner.planner = "RRTConnectkConfigDefault"
+pilz_pipelinePlanner = core.PipelinePlanner("pilz_industrial_motion_planner")
+pilz_pipelinePlanner.planner = "PTP"
+multiPlanner = core.MultiPlanner()
+multiPlanner.add(pilz_pipelinePlanner, ompl_pipelinePlanner)
+
+# Create a task
+task = core.Task()
+
+# Start from current robot state
+currentState = stages.CurrentState("current state")
+
+# Add the current state to the task hierarchy
+task.add(currentState)
+
+# The alternatives stage supports multiple execution paths
+alternatives = core.Alternatives("Alternatives")
+
+# goal 1
+goalConfig1 = {
+    "panda_joint1": 1.0,
+    "panda_joint2": -1.0,
+    "panda_joint3": 0.0,
+    "panda_joint4": -2.5,
+    "panda_joint5": 1.0,
+    "panda_joint6": 1.0,
+    "panda_joint7": 1.0,
+}
+
+# goal 2
+goalConfig2 = {
+    "panda_joint1": -3.0,
+    "panda_joint2": -1.0,
+    "panda_joint3": 0.0,
+    "panda_joint4": -2.0,
+    "panda_joint5": 1.0,
+    "panda_joint6": 2.0,
+    "panda_joint7": 0.5,
+}
+
+# First motion plan to compare
+moveTo1 = stages.MoveTo("Move To Goal Configuration 1", multiPlanner)
+moveTo1.group = "panda_arm"
+moveTo1.setGoal(goalConfig1)
+alternatives.insert(moveTo1)
+
+# Second motion plan to compare
+moveTo2 = stages.MoveTo("Move To Goal Configuration 2", multiPlanner)
+moveTo2.group = "panda_arm"
+moveTo2.setGoal(goalConfig2)
+alternatives.insert(moveTo2)
+
+# Add the alternatives stage to the task hierarchy
+task.add(alternatives)
+
+if task.plan():
+    task.publish(task.solutions[0])
+time.sleep(1)