mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Expose MultiPlanner to Python (#474)
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
This commit is contained in:
parent
3b4ea48c18
commit
227d475282
@ -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())
|
||||
|
||||
@ -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
|
||||
|
||||
26
core/python/bindings/src/utils.h
Normal file
26
core/python/bindings/src/utils.h
Normal 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
68
demo/scripts/multi_planner.py
Executable 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)
|
||||
Loading…
Reference in New Issue
Block a user