diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index a9f017ca1..c1b488482 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -33,6 +33,7 @@ *********************************************************************/ #include "core.h" +#include "utils.h" #include #include #include @@ -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::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()) diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index a1d3b405f..ef60fb822 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -36,7 +36,9 @@ #include #include #include +#include #include +#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("min_fraction", "float: Fraction of overall distance required to succeed.") .def(py::init<>()); + + properties::class_(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) + .def( + "add", + [](MultiPlanner& self, const py::args& args) { + for (auto it = args.begin(), end = args.end(); it != end; ++it) + self.push_back(it->cast()); + }, + "Insert one or more planners") + .def( + "clear", [](MultiPlanner& self) { self.clear(); }, "Remove all planners") + .def(py::init<>()); } } // namespace python } // namespace moveit diff --git a/core/python/bindings/src/utils.h b/core/python/bindings/src/utils.h new file mode 100644 index 000000000..14786d885 --- /dev/null +++ b/core/python/bindings/src/utils.h @@ -0,0 +1,26 @@ +#include + +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::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 diff --git a/demo/scripts/multi_planner.py b/demo/scripts/multi_planner.py new file mode 100755 index 000000000..4b26cef84 --- /dev/null +++ b/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)