Expose MultiPlanner to Python (#474)

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
This commit is contained in:
VideoSystemsTech 2024-05-27 16:19:09 +02:00 committed by Robert Haschke
parent 3b4ea48c18
commit 227d475282
4 changed files with 119 additions and 17 deletions

View File

@ -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())

View File

@ -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

View File

@ -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

68
demo/scripts/multi_planner.py Executable file
View File

@ -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)