Rework core documentation

This commit is contained in:
Robert Haschke 2022-01-23 14:40:34 +01:00
parent 9103af2704
commit e77fa83a42
5 changed files with 281 additions and 297 deletions

2
.gitignore vendored
View File

@ -1,3 +1,5 @@
*.swp
*.pyc
__pycache__/
*/doc/html/
*/doc/manifest.yaml

View File

@ -6,7 +6,7 @@
:members:
:show-inheritance:
:inherited-members:
:special-members: __call__, __add__, __mul__
:special-members: __len__, __getitem__, __iter__, __call__, __add__, __mul__
{% block methods %}
{% if methods %}

View File

@ -30,6 +30,7 @@ extensions = [
"sphinx.ext.autosummary",
"sphinx.ext.napoleon",
"sphinx_rtd_theme",
"sphinx.ext.extlinks",
]
autosummary_generate = True
@ -214,3 +215,13 @@ intersphinx_mapping = {"python": ("https://docs.python.org/3", None)}
# Default options for generating documentation.
autodoc_default_options = {"exclude-members": "ContainerBase, ParallelContainerBase, WrapperBase"}
ros_distro = "noetic"
ros_docs = "http://docs.ros.org/" + ros_distro + "/api/"
extlinks = {
"rosdocs": (ros_docs + "%s", ""),
"msgs": (ros_docs + "moveit_task_constructor/html/msg/%s.html", ""),
"moveit_msgs": (ros_docs + "moveit_msgs/html/msg/%s.html", ""),
"geometry_msgs": (ros_docs + "geometry_msgs/html/msg/%s.html", ""),
"visualization_msgs": (ros_docs + "visualization_msgs/html/msg/%s.html", ""),
}

View File

@ -50,10 +50,8 @@ namespace python {
namespace {
// utility function to extract index from python object
// also handles negative indexes referencing from the end
size_t convert_index(size_t size, const py::object& pyindex) {
long index = pyindex.cast<size_t>();
// 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)
@ -63,9 +61,9 @@ size_t convert_index(size_t size, const py::object& pyindex) {
// implement operator[](index)
template <typename T>
typename T::value_type get_item(const T& container, const py::object& index) {
typename T::value_type get_item(const T& container, long index) {
auto it = container.begin();
std::advance(it, convert_index(container.size(), index));
std::advance(it, normalize_index(container.size(), index));
return *it;
}
@ -110,119 +108,110 @@ void export_core(pybind11::module& m) {
}
});
// clang-format off
py::classh<SolutionBase>(m, "Solution", "Solution class encapsulates a particular solution")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost,"float: Cost that is associated with a particular solution")
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment that is associated with a particular solution")
.def("toMsg", [](const SolutionBasePtr& s) {
moveit_task_constructor_msgs::Solution msg;
s->fillMessage(msg);
return msg;
}, "Convert a solution object into a ros message type")
;
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory", "A SubTrajectory connects interface states of compute stages")
.def(py::init<>())
.def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory. Readonly property")
.def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory. Readonly property")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost of the solution")
.def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "msg"_a)
.def_property_readonly("isFailure", &SolutionBase::isFailure, "bool: True if the trajectory is marked as a failure. Readonly property")
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment, which can be assigned to the trajectory")
.def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers), R"(
Marker_: Markers that visualize the trajectory. Readonly property.
py::classh<SolutionBase>(m, "Solution", "Abstract base class for solutions of a stage")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost associated with the solution")
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment,
"str: Comment associated with the solution")
.def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "comment"_a)
.def_property_readonly("isFailure", &SolutionBase::isFailure,
"bool: True if the trajectory is marked as a failure (read-only)")
.def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory (read-only)")
.def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory (read-only)")
.def_property_readonly(
"markers", py::overload_cast<>(&SolutionBase::markers),
":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)")
.def(
"toMsg",
[](const SolutionBasePtr& s) {
moveit_task_constructor_msgs::Solution msg;
s->fillMessage(msg);
return msg;
},
"Convert to the ROS message ``Solution``");
.. _Marker: https://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html
)")
;
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory",
"Solution trajectory connecting two InterfaceStates of a stage")
.def(py::init<>())
.def_property("trajectory", &SubTrajectory::trajectory, &SubTrajectory::setTrajectory,
":moveit_msgs:`RobotTrajectory`: Actual robot trajectory");
using Solutions = ordered<SolutionBaseConstPtr>;
py::classh<Solutions>(m, "Solutions", "Encapsulates multiple solutions")
.def("__len__", &Solutions::size)
.def("__getitem__", &get_item<Solutions>)
.def("__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>())
;
py::classh<Solutions>(m, "Solutions", "Cost-ordered list of solutions")
.def("__len__", &Solutions::size)
.def("__getitem__", &get_item<Solutions>)
.def(
"__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>());
py::classh<InterfaceState>(m, "InterfaceState", R"(
InterfaceState describes a potential start or goal state
for a planning stage. A start or goal state for planning
is essentially defined by the state of a planning scene.)")
.def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a)
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties),
"PropertyMap: Get access to the PropertyMap of the stage. Notice that this is a read-only property")
.def_property_readonly("scene", &InterfaceState::scene,
"PlanningScene: Get access to the planning scene of the interface stage. Notice that this is a read-only property")
;
py::classh<InterfaceState>(m, "InterfaceState",
"Describes a potential start or goal state of a Stage. "
"It comprises a PlanningScene as well as a PropertyMap.")
.def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a)
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties),
"PropertyMap: PropertyMap of the state (read-only).")
.def_property_readonly("scene", &InterfaceState::scene,
"PlanningScene: PlanningScene of the state (read-only).");
py::classh<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode", "Encapsulates moveit error code message")
.def_readonly("val", &moveit::core::MoveItErrorCode::val, "int: Value of the error code")
.def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) {
return pybind11::cast(static_cast<bool>(err));
});
.def_readonly("val", &moveit::core::MoveItErrorCode::val, ":moveit_msgs:`MoveItErrorCodes`: error code")
.def(PYBIND11_BOOL_ATTR,
[](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast<bool>(err)); });
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"(
Stage base type. All other stages derive from this type.
Object is not instantiable and should not be added to the task hierarchy
in a standalone fashion. Rather, derive from generator or propagator stages
to implement custom logic.
)")
.property<double>("timeout","float: Timeout of stage per computation")
.property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage")
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, "list: Get / set a list of forwarded properties")
// expose name as writeable property
.def_property("name", &Stage::name, &Stage::setName, "str: Get / set the name of the stage")
// read-only access to properties + solutions
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties), "PropertyMap: Return the property map of the stage. Readonly property")
.def_property_readonly("solutions", &Stage::solutions, "Solutions: Get the solutions of a stage")
.def_property_readonly("failures", &Stage::failures, "Solutions: Get the failed compuations of a stage")
.def("reset", &Stage::reset, "Reset the stage, clearing all solutions, interfaces and inherited properties")
.def("init", &Stage::init, R"(
Initialize the stage once before planning. When called, properties configured for
initialization from parent are already defined. Push interfaces are not yet defined!
)", "robot_model"_a);
auto stage =
properties::class_<Stage, PyStage<>>(m, "Stage", "Abstract base class of all stages.")
.property<double>("timeout", "float: Maximally allowed time [s] per computation step")
.property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage")
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties,
"list: set of properties forwarded from input to output InterfaceState")
.def_property("name", &Stage::name, &Stage::setName, "str: name of the stage displayed e.g. in rviz")
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties),
"PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties")
.def("init", &Stage::init,
"Initialize the stage once before planning. "
"Will setup properties configured for initialization from parent.",
"robot_model"_a);
py::enum_<Stage::PropertyInitializerSource>(stage, "PropertyInitializerSource", R"(
Define, from where properties should be initialized when using the `configureInitFrom()`
functions from the PropertyMap class.
)")
.value("PARENT", Stage::PARENT, "Inherit properties from parent stage in task hierarchy")
.value("INTERFACE", Stage::INTERFACE, "Inherit properties from interface, i.e. preceeding stage in the task hierarchy")
;
py::enum_<Stage::PropertyInitializerSource>(
stage, "PropertyInitializerSource",
"OR-combinable flags defining a source to initialize a specific property from. "
"Used in :doc:`pymoveit_mtc.core.PropertyMap` ``configureInitFrom()``. ")
.value("PARENT", Stage::PARENT, "Inherit properties from parent stage")
.value("INTERFACE", Stage::INTERFACE, "Inherit properties from the input InterfaceState");
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(
m, "PropagatingEitherWay", "Base class for propagator-like stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay"))
.def("restrictDirection", &PropagatingEitherWay::restrictDirection,
"Explicitly specify computation direction")
.def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward")
.def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
//.def("sendForward", &PropagatingEitherWay::sendForward)
//.def("sendBackward", &PropagatingEitherWay::sendBackward)
;
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay", "Base class for solution forwarding in both directions")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay"))
.def("restrictDirection", &PropagatingEitherWay::restrictDirection, "Explicitly specify computation direction")
.def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward")
.def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
//.def("sendForward", &PropagatingEitherWay::sendForward)
//.def("sendBackward", &PropagatingEitherWay::sendBackward)
;
py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction", "Propagation direction")
.value("AUTO", PropagatingEitherWay::AUTO)
.value("FORWARD", PropagatingEitherWay::FORWARD, "Propagating forwards from start to end")
.value("BACKWARD", PropagatingEitherWay::BACKWARD, "Propagating backwards from end to start");
py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction")
.value("AUTO", PropagatingEitherWay::AUTO)
.value("FORWARD", PropagatingEitherWay::FORWARD)
.value("BACKWARD", PropagatingEitherWay::BACKWARD);
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward", "Base class for forward solution propagation")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"))
;
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(m, "PropagatingBackward", "Base class for backward solution propagation")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"))
;
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(
m, "PropagatingForward", "Base class for forward-propagating stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"));
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(
m, "PropagatingBackward", "Base class for backward-propagating stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"));
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"(
Derive from this stage to implement a custom stage
that spawns a solution. When traversing through the whole
task hierarchy, the ``compute()`` function of this generator stage
gets called as many times as the ``canCompute()`` returns ``true``.
Base class for generator-like stages
::
Derive from this stage to implement a custom generator stage that can produce new seed states w/o prior knowledge.
Implement the virtual methods as follows::
class PyGenerator(core.Generator):
""" Implements a custom 'Generator' stage with 3 compute() calls."""
max_calls = 3
class MyGenerator(core.Generator):
"""Implements a custom 'Generator' stage that produces maximally 3 solutions."""
def __init__(self, name="Generator"):
core.Generator.__init__(self, name)
@ -233,226 +222,216 @@ void export_core(pybind11::module& m) {
def reset(self):
core.Generator.reset(self)
self.num = self.max_calls
def canCompute(self):
return self.num > 0
return len(self.solutions) < 3 # maximally produce 3 solutions
def compute(self):
self.num = self.num - 1
self.spawn(core.InterfaceState(self.ps), self.num)
self.spawn(core.InterfaceState(self.ps), cost=len(self.solutions))
)")
.def(py::init<const std::string&>(), "name"_a = std::string("generator"))
.def("canCompute", &Generator::canCompute, "Guard for the ``compute()`` function")
.def("compute", &Generator::compute, "Implement the stage's logic here. To interface with other stages, spawn an ``InterfaceState``")
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); },
"Spawn an interface state that gets forwarded to the next stage",
"state"_a, "cost"_a)
;
.def(py::init<const std::string&>(), "name"_a = std::string("Generator"))
.def("canCompute", &Generator::canCompute, "Return ``True`` if the stage can still produce solutions.")
.def("compute", &Generator::compute, "Compute an actual solution and ``spawn`` an ``InterfaceState``")
.def(
"spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); },
"Spawn an ``InterfaceState`` to both, start and end interface", "state"_a, "cost"_a);
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"(
Generator that monitors solutions of another stage to make reuse of them
Sometimes its necessary to reuse a previously planned solution, e.g. to
traverse it in reverse order or to access the state of another generator.
To this end, the present stage hooks into the onNewSolution() method of
the monitored stage and forwards it to this' class onNewSolution() method.
Base class for monitoring generator stages
You may derive from this stage to implement your own stage generation logic.
To implement a generator stage that draws on some previously computed solution, you need to derive
from ``MonitoringGenerator`` - monitoring the solutions produced by another stage.
Each time, the monitored stage produces a new solution, the method ``onNewSolution()`` of the
MonitoringGenerator is called. Usually, you schedule this solution for later processing in ``compute()``::
::
class PyMonitoringGenerator(core.MonitoringGenerator):
""" Implements a custom 'MonitoringGenerator' stage."""
class PyMonitoringGenerator(core.MonitoringGenerator):
""" Implements a custom 'MonitoringGenerator' stage."""
solution_multiplier = 2
solution_multiplier = 2
def __init__(self, name="MonitoringGenerator"):
core.MonitoringGenerator.__init__(self, name)
self.reset()
def __init__(self, name="MonitoringGenerator"):
core.MonitoringGenerator.__init__(self, name)
self.reset()
def reset(self):
core.MonitoringGenerator.reset(self)
self.pending = []
def reset(self):
core.MonitoringGenerator.reset(self)
self.upstream_solutions = list()
def onNewSolution(self, sol):
self.pending.append(sol)
def onNewSolution(self, sol):
self.upstream_solutions.append(sol)
def canCompute(self):
return bool(self.pending)
def canCompute(self):
return bool(self.upstream_solutions)
def compute(self):
# fetch first pending upstream solution ...
scene = self.pending.pop(0).end.scene
# ... and generate new solutions derived from it
for i in range(self.solution_multiplier):
self.spawn(core.InterfaceState(scene), i)
def compute(self):
scene = self.upstream_solutions.pop(0).end.scene
for i in range(self.solution_multiplier):
self.spawn(core.InterfaceState(scene), i)
Upon creation of the stage, assign the monitored stage as follows::
Upon creation of the stage, assign the monitored stage:
jointspace = core.JointInterpolationPlanner()
::
task = core.Task()
current = stages.CurrentState("current")
task.add(current)
jointspace = core.JointInterpolationPlanner()
connect = stages.Connect(planners=[('panda_arm', jointspace)])
task.add(connect)
task = core.Task()
current = stages.CurrentState("current")
task.add(current)
mg = PyMonitoringGenerator("generator")
mg.setMonitoredStage(task["current"])
task.add(mg)
)")
.def(py::init<const std::string&>(), "name"_a = std::string("generator"))
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the monitored ``Stage``", "stage"_a)
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution);
connect = stages.Connect(planners=[('panda_arm', jointspace)])
task.add(connect)
py::classh<ContainerBase, Stage>(m, "ContainerBase", R"(
Abstract base class for container stages
Containers allow encapsulation and reuse of planning functionality in a hierachical fashion.
You can iterate of the children of a container and access them by name.)")
.def("add", &ContainerBase::add, "Insert a stage at the end of the current children list")
.def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1,
"Insert a stage before the given index into the children list")
.def("remove", py::overload_cast<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
.def("remove", py::overload_cast<Stage*>(&ContainerBase::remove), "Remove child stage by instance", "child"_a)
.def("clear", &ContainerBase::clear, "Remove all stages from the container")
.def("__len__", &ContainerBase::numChildren)
.def(
"__getitem__",
[](const ContainerBase& c, const std::string& name) -> Stage* {
Stage* child = c.findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const ContainerBase& c) {
const auto& children = c.pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
;
mg = PyMonitoringGenerator("generator")
task.add(mg)
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Container implementing a linear planning sequence")
.def(py::init<const std::string&>(), "name"_a = std::string("SerialContainer"));
task["generator"].setMonitoredStage(task["current"])
)")
.def(py::init<const std::string&>(), "name"_a = std::string("generator"))
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the reference to the Monitored Stage", "stage"_a)
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution)
;
py::classh<ContainerBase, Stage>(m, "ContainerBase", "Base class for containers that implements utility functionality")
.def("add", &ContainerBase::add, "Add a stage to the container")
.def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1, "Insert a stage before given index into container")
.def("remove", py::overload_cast<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
.def("clear", &ContainerBase::clear, "Clear the stages of the container")
.def("__len__", &ContainerBase::numChildren)
.def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* {
Stage* child = c.findChild(name);
if (!child)
throw py::index_error();
return child;
}, py::return_value_policy::reference_internal)
.def("__iter__", [](const ContainerBase &c) {
const auto& children = c.pimpl()->children();
return py::make_iterator(children.begin(), children.end());
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
;
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Base class for container containing a serial set of stages")
.def(py::init<const std::string&>(), "name"_a = std::string("serial container"));
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase", "Base class for parallel containers");
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase",
"Abstract base class for parallel containers");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"(
Plan for different alternatives in parallel.
Solution of all children are reported - sorted by cost.
Plan for different alternatives in parallel
Solution of all children are considered simultaneously.
.. literalinclude:: ./../../../demo/scripts/alternatives.py
:language: python
)")
.def(py::init<const std::string&>(), "name"_a = std::string("alternatives"))
;
.. literalinclude:: ./../../../demo/scripts/alternatives.py
:lines: 22-60
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Alternatives"));
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"(
Plan for different alternatives in sequence.
Try to find feasible solutions using first child.
Only if this fails, proceed to the next child trying
an alternative planning strategy.
All solutions of the last active child are reported.
Plan for different alternatives in sequence
Try to find feasible solutions using the children in sequence. The behaviour slightly differs for the indivual stage types:
.. literalinclude:: ./../../../demo/scripts/fallbacks.py
:language: python
- Generator: Proceed to next child if currently active one exhausted its solution, i.e. returns ``canCompute() == False``.
- Propagator: Forward an incoming ``InterfaceState`` to the next child if the current one ultimately failed on it.
- Connect: Only ``Connect`` stages are supported. Pairs of ``InterfaceStates`` are forward to the next child on failure of the current child.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("fallbacks"))
;
.. literalinclude:: ./../../../demo/scripts/fallbacks.py
:lines: 21-38
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Fallbacks"));
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"(
Plan for different sub tasks in parallel and finally merge
all sub solutions into a single trajectory
Plan for different sub tasks in parallel and eventually merge all sub solutions into a single trajectory
This requires all children to operate on disjoint ``JointModelGroups``.
.. literalinclude:: ./../../../demo/scripts/merger.py
:language: python
)")
.def(py::init<const std::string&>(), "name"_a = std::string("merger"))
;
.. literalinclude:: ./../../../demo/scripts/merger.py
)")
.def(py::init<const std::string&>(), "name"_a = std::string("merger"));
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"(
A wrapper wraps a single child stage, which can be
accessed via ``wrapped()``. Implementations of this
interface need to implement ``onNewSolution()``, which
is called when the child has generated a new solution.
The wrapper may reject the solution or create one or
multiple derived solutions, potentially adapting the cost,
as well as its start and end states.
)")
;
Base class for wrapping containers, which can be used to filter or modify solutions generated by the single child.
Implementations of this interface need to implement ``onNewSolution()`` to process a solution generated by the child.
The wrapper may reject the solution or create one or multiple derived solutions, potentially adapting the cost,
the trajectory and output ``InterfaceStates``.
)");
py::classh<Task>(m, "Task", R"(
A task is the root of a tree of stages by
wrapping a single container
(by default a SerialContainer),
which serves as the root of all stages.
See below an exampel which creates a task with a
single stage in its hierarchy, fetching the current state
of the planning scene.
py::classh<Task>(m, "Task", R"(Root stage of a planning pipeline.
A task stage usually wraps a single container (by default ``SerialContainer``) stage.
The class provides methods to ``plan()`` for the configured pipeline and retrieve full solutions.)")
.def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), "ns"_a = std::string(),
"introspection"_a = true, "container"_a)
.. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python
.def_property_readonly("properties", py::overload_cast<>(&Task::properties),
"PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Task::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz")
)")
.def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(),
"ns"_a = std::string(), "introspection"_a = true, "container"_a)
// read-only access to properties + solutions
.def_property_readonly("properties", py::overload_cast<>(&Task::properties),
"PropertyMap: Access the property map of the task")
.def_property_readonly("solutions", &Task::solutions,
"Solutions: Access the solutions of the task, once the sub stage hierarchy has been traversed and planned")
.def_property_readonly("failures", &Task::failures,
"Solutions: Inspect failures that occurred during the planning phase")
.def_property("name", &Task::name, &Task::setName, "str: Set the name property of the task")
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", "Load robot model from given parameter")
.def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, "Enable introspection publish for use with `rviz`")
.def("clear", &Task::clear, "Reset the stage hierarchy")
.def("add", &Task::add, "Add a stage to the task hierarchy", "stage"_a)
.def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* {
Stage* child = t.stages()->findChild(name);
if (!child)
throw py::index_error();
return child;
}, py::return_value_policy::reference_internal)
.def("__iter__", [](const Task &t) {
const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end());
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def("reset", &Task::reset, "Reset all stages")
.def("init", py::overload_cast<>(&Task::init), "Initialize all stages with a given scene")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(
Reset, initialize scene (if not yet done), and initialize all
stages, then start planning with ``max_allowed_solutions``. Returns if planning
was successful.
)")
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
.def("publish", [](Task& self, const SolutionBasePtr& solution) {
self.introspection().publishSolution(*solution);
}, "solution"_a, "Publish a given solution to the solution ros topic")
.def("execute", [](const Task& self, const SolutionBasePtr& solution) {
moveit::planning_interface::PlanningSceneInterface psi;
moveit::planning_interface::MoveGroupInterface
mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description",
"Load robot model from given ROS parameter")
.def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true,
"Enable publishing intermediate results for inspection in ``rviz``")
.def("clear", &Task::clear, "Reset the stage task (and all its stages)")
.def("add", &Task::add, "Append a stage to the task's top-level container", "stage"_a)
.def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def(
"__getitem__",
[](const Task& t, const std::string& name) -> Stage* {
Stage* child = t.stages()->findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const Task& t) {
const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def("reset", &Task::reset, "Reset task (and all its stages)")
.def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(
Reset, init, and plan. Planning is limited to ``max_allowed_solutions``.
Returns if planning was successful.)")
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
.def(
"publish",
[](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); },
"solution"_a, "Publish the given solution to the ROS topic ``solution``")
.def(
"execute",
[](const Task& self, const SolutionBasePtr& solution) {
using namespace moveit::planning_interface;
PlanningSceneInterface psi;
MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_task_constructor_msgs::Solution serialized;
solution->fillMessage(serialized);
MoveGroupInterface::Plan plan;
moveit_task_constructor_msgs::Solution serialized;
solution->fillMessage(serialized);
for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) {
if (!traj.trajectory.joint_trajectory.points.empty()) {
plan.trajectory_ = traj.trajectory;
if (!mgi.execute(plan)) {
ROS_ERROR("Execution failed! Aborting!");
return;
}
}
psi.applyPlanningScene(traj.scene_diff);
}
ROS_INFO("Executed successfully");
}, "solution"_a, "Execute Solution");
// clang-format on
for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) {
if (!traj.trajectory.joint_trajectory.points.empty()) {
plan.trajectory_ = traj.trajectory;
if (!mgi.execute(plan)) {
ROS_ERROR("Execution failed! Aborting!");
return;
}
}
psi.applyPlanningScene(traj.scene_diff);
}
ROS_INFO("Executed successfully");
},
"solution"_a, "Send given solution to ``move_group`` node for execution");
}
} // namespace python
} // namespace moveit

View File

@ -46,22 +46,14 @@ void export_stages(pybind11::module& m);
} // namespace moveit
PYBIND11_MODULE(pymoveit_mtc, m) {
auto msub = m.def_submodule("core", R"(
moveit.task_constructor.core
============================
This package provides wrappers of core MTC components.
)");
auto msub = m.def_submodule("core", "Provides wrappers for core C++ classes. "
"**Import as** :doc:`moveit.task_constructor.core`.");
moveit::python::export_properties(msub);
moveit::python::export_solvers(msub);
moveit::python::export_core(msub);
msub = m.def_submodule("stages", R"(
moveit.task_constructor.stages
==============================
This package provides wrappers of standard MTC stages.
)");
msub = m.def_submodule("stages", "Provides wrappers of standard MTC stages. "
"**Import as** :doc:`moveit.task_constructor.stages`.");
moveit::python::export_stages(msub);
}