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 *.swp
*.pyc *.pyc
__pycache__/ __pycache__/
*/doc/html/
*/doc/manifest.yaml

View File

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

View File

@ -30,6 +30,7 @@ extensions = [
"sphinx.ext.autosummary", "sphinx.ext.autosummary",
"sphinx.ext.napoleon", "sphinx.ext.napoleon",
"sphinx_rtd_theme", "sphinx_rtd_theme",
"sphinx.ext.extlinks",
] ]
autosummary_generate = True autosummary_generate = True
@ -214,3 +215,13 @@ intersphinx_mapping = {"python": ("https://docs.python.org/3", None)}
# Default options for generating documentation. # Default options for generating documentation.
autodoc_default_options = {"exclude-members": "ContainerBase, ParallelContainerBase, WrapperBase"} 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 { namespace {
// utility function to extract index from python object // utility function to normalize index: negative indeces reference from the end
// also handles negative indexes referencing from the end size_t normalize_index(size_t size, long index) {
size_t convert_index(size_t size, const py::object& pyindex) {
long index = pyindex.cast<size_t>();
if (index < 0) if (index < 0)
index += size; index += size;
if (index >= long(size) || index < 0) if (index >= long(size) || index < 0)
@ -63,9 +61,9 @@ size_t convert_index(size_t size, const py::object& pyindex) {
// implement operator[](index) // implement operator[](index)
template <typename T> 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(); auto it = container.begin();
std::advance(it, convert_index(container.size(), index)); std::advance(it, normalize_index(container.size(), index));
return *it; return *it;
} }
@ -110,119 +108,110 @@ void export_core(pybind11::module& m) {
} }
}); });
// clang-format off py::classh<SolutionBase>(m, "Solution", "Abstract base class for solutions of a stage")
py::classh<SolutionBase>(m, "Solution", "Solution class encapsulates a particular solution") .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost associated with the solution")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost,"float: Cost that is associated with a particular solution") .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment,
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment that is associated with a particular solution") "str: Comment associated with the solution")
.def("toMsg", [](const SolutionBasePtr& s) { .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; moveit_task_constructor_msgs::Solution msg;
s->fillMessage(msg); s->fillMessage(msg);
return msg; return msg;
}, "Convert a solution object into a ros message type") },
; "Convert to the ROS message ``Solution``");
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.
.. _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>; using Solutions = ordered<SolutionBaseConstPtr>;
py::classh<Solutions>(m, "Solutions", "Encapsulates multiple solutions") py::classh<Solutions>(m, "Solutions", "Cost-ordered list of solutions")
.def("__len__", &Solutions::size) .def("__len__", &Solutions::size)
.def("__getitem__", &get_item<Solutions>) .def("__getitem__", &get_item<Solutions>)
.def("__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); }, .def(
py::keep_alive<0, 1>()) "__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
; py::keep_alive<0, 1>());
py::classh<InterfaceState>(m, "InterfaceState", R"( py::classh<InterfaceState>(m, "InterfaceState",
InterfaceState describes a potential start or goal state "Describes a potential start or goal state of a Stage. "
for a planning stage. A start or goal state for planning "It comprises a PlanningScene as well as a PropertyMap.")
is essentially defined by the state of a planning scene.)")
.def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a) .def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a)
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), .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") "PropertyMap: PropertyMap of the state (read-only).")
.def_property_readonly("scene", &InterfaceState::scene, .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") "PlanningScene: PlanningScene of the state (read-only).");
;
py::classh<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode", "Encapsulates moveit error code message") 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_readonly("val", &moveit::core::MoveItErrorCode::val, ":moveit_msgs:`MoveItErrorCodes`: error code")
.def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) { .def(PYBIND11_BOOL_ATTR,
return pybind11::cast(static_cast<bool>(err)); [](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast<bool>(err)); });
});
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"( auto stage =
Stage base type. All other stages derive from this type. properties::class_<Stage, PyStage<>>(m, "Stage", "Abstract base class of all stages.")
Object is not instantiable and should not be added to the task hierarchy .property<double>("timeout", "float: Maximally allowed time [s] per computation step")
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") .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") .def_property("forwarded_properties", getForwardedProperties, setForwardedProperties,
// expose name as writeable property "list: set of properties forwarded from input to output InterfaceState")
.def_property("name", &Stage::name, &Stage::setName, "str: Get / set the name of the stage") .def_property("name", &Stage::name, &Stage::setName, "str: name of the stage displayed e.g. in rviz")
// read-only access to properties + solutions .def_property_readonly("properties", py::overload_cast<>(&Stage::properties),
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties), "PropertyMap: Return the property map of the stage. Readonly property") "PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Stage::solutions, "Solutions: Get the solutions of a stage") .def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Stage::failures, "Solutions: Get the failed compuations of a stage") .def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def("reset", &Stage::reset, "Reset the stage, clearing all solutions, interfaces and inherited properties") .def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties")
.def("init", &Stage::init, R"( .def("init", &Stage::init,
Initialize the stage once before planning. When called, properties configured for "Initialize the stage once before planning. "
initialization from parent are already defined. Push interfaces are not yet defined! "Will setup properties configured for initialization from parent.",
)", "robot_model"_a); "robot_model"_a);
py::enum_<Stage::PropertyInitializerSource>(stage, "PropertyInitializerSource", R"( py::enum_<Stage::PropertyInitializerSource>(
Define, from where properties should be initialized when using the `configureInitFrom()` stage, "PropertyInitializerSource",
functions from the PropertyMap class. "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 in task hierarchy") .value("PARENT", Stage::PARENT, "Inherit properties from parent stage")
.value("INTERFACE", Stage::INTERFACE, "Inherit properties from interface, i.e. preceeding stage in the task hierarchy") .value("INTERFACE", Stage::INTERFACE, "Inherit properties from the input InterfaceState");
;
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay", "Base class for solution forwarding in both directions") m, "PropagatingEitherWay", "Base class for propagator-like stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay")) .def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay"))
.def("restrictDirection", &PropagatingEitherWay::restrictDirection, "Explicitly specify computation direction") .def("restrictDirection", &PropagatingEitherWay::restrictDirection,
"Explicitly specify computation direction")
.def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward") .def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward")
.def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward") .def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
//.def("sendForward", &PropagatingEitherWay::sendForward) //.def("sendForward", &PropagatingEitherWay::sendForward)
//.def("sendBackward", &PropagatingEitherWay::sendBackward) //.def("sendBackward", &PropagatingEitherWay::sendBackward)
; ;
py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction") py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction", "Propagation direction")
.value("AUTO", PropagatingEitherWay::AUTO) .value("AUTO", PropagatingEitherWay::AUTO)
.value("FORWARD", PropagatingEitherWay::FORWARD) .value("FORWARD", PropagatingEitherWay::FORWARD, "Propagating forwards from start to end")
.value("BACKWARD", PropagatingEitherWay::BACKWARD); .value("BACKWARD", PropagatingEitherWay::BACKWARD, "Propagating backwards from end to start");
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward", "Base class for forward solution propagation") py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(
.def(py::init<const std::string&>(), "name"_a = std::string("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 solution propagation") py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(
.def(py::init<const std::string&>(), "name"_a = std::string("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"( properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"(
Derive from this stage to implement a custom stage Base class for generator-like stages
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``.
:: 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): class MyGenerator(core.Generator):
""" Implements a custom 'Generator' stage with 3 compute() calls.""" """Implements a custom 'Generator' stage that produces maximally 3 solutions."""
max_calls = 3
def __init__(self, name="Generator"): def __init__(self, name="Generator"):
core.Generator.__init__(self, name) core.Generator.__init__(self, name)
@ -233,34 +222,27 @@ void export_core(pybind11::module& m) {
def reset(self): def reset(self):
core.Generator.reset(self) core.Generator.reset(self)
self.num = self.max_calls
def canCompute(self): def canCompute(self):
return self.num > 0 return len(self.solutions) < 3 # maximally produce 3 solutions
def compute(self): def compute(self):
self.num = self.num - 1 self.spawn(core.InterfaceState(self.ps), cost=len(self.solutions))
self.spawn(core.InterfaceState(self.ps), self.num)
)") )")
.def(py::init<const std::string&>(), "name"_a = std::string("generator")) .def(py::init<const std::string&>(), "name"_a = std::string("Generator"))
.def("canCompute", &Generator::canCompute, "Guard for the ``compute()`` function") .def("canCompute", &Generator::canCompute, "Return ``True`` if the stage can still produce solutions.")
.def("compute", &Generator::compute, "Implement the stage's logic here. To interface with other stages, spawn an ``InterfaceState``") .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); }, .def(
"Spawn an interface state that gets forwarded to the next stage", "spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); },
"state"_a, "cost"_a) "Spawn an ``InterfaceState`` to both, start and end interface", "state"_a, "cost"_a);
;
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"( properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"(
Generator that monitors solutions of another stage to make reuse of them Base class for monitoring generator stages
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.
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): class PyMonitoringGenerator(core.MonitoringGenerator):
""" Implements a custom 'MonitoringGenerator' stage.""" """ Implements a custom 'MonitoringGenerator' stage."""
@ -273,22 +255,22 @@ void export_core(pybind11::module& m) {
def reset(self): def reset(self):
core.MonitoringGenerator.reset(self) core.MonitoringGenerator.reset(self)
self.upstream_solutions = list() self.pending = []
def onNewSolution(self, sol): def onNewSolution(self, sol):
self.upstream_solutions.append(sol) self.pending.append(sol)
def canCompute(self): def canCompute(self):
return bool(self.upstream_solutions) return bool(self.pending)
def compute(self): def compute(self):
scene = self.upstream_solutions.pop(0).end.scene # 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): for i in range(self.solution_multiplier):
self.spawn(core.InterfaceState(scene), i) self.spawn(core.InterfaceState(scene), i)
Upon creation of the stage, assign the monitored stage: Upon creation of the stage, assign the monitored stage as follows::
::
jointspace = core.JointInterpolationPlanner() jointspace = core.JointInterpolationPlanner()
@ -300,143 +282,140 @@ void export_core(pybind11::module& m) {
task.add(connect) task.add(connect)
mg = PyMonitoringGenerator("generator") mg = PyMonitoringGenerator("generator")
mg.setMonitoredStage(task["current"])
task.add(mg) task.add(mg)
task["generator"].setMonitoredStage(task["current"])
)") )")
.def(py::init<const std::string&>(), "name"_a = std::string("generator")) .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("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the monitored ``Stage``", "stage"_a)
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution) .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution);
;
py::classh<ContainerBase, Stage>(m, "ContainerBase", "Base class for containers that implements utility functionality") py::classh<ContainerBase, Stage>(m, "ContainerBase", R"(
.def("add", &ContainerBase::add, "Add a stage to the container") Abstract base class for container stages
.def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1, "Insert a stage before given index into container") 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<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
.def("clear", &ContainerBase::clear, "Clear the stages of the container") .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("__len__", &ContainerBase::numChildren)
.def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* { .def(
"__getitem__",
[](const ContainerBase& c, const std::string& name) -> Stage* {
Stage* child = c.findChild(name); Stage* child = c.findChild(name);
if (!child) if (!child)
throw py::index_error(); throw py::index_error();
return child; return child;
}, py::return_value_policy::reference_internal) },
.def("__iter__", [](const ContainerBase &c) { py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const ContainerBase& c) {
const auto& children = c.pimpl()->children(); const auto& children = c.pimpl()->children();
return py::make_iterator(children.begin(), children.end()); return py::make_iterator(children.begin(), children.end());
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives },
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") py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Container implementing a linear planning sequence")
.def(py::init<const std::string&>(), "name"_a = std::string("serial container")); .def(py::init<const std::string&>(), "name"_a = std::string("SerialContainer"));
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"( py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"(
Plan for different alternatives in parallel. Plan for different alternatives in parallel
Solution of all children are reported - sorted by cost. Solution of all children are considered simultaneously.
.. literalinclude:: ./../../../demo/scripts/alternatives.py .. literalinclude:: ./../../../demo/scripts/alternatives.py
:language: python :lines: 22-60
)") )")
.def(py::init<const std::string&>(), "name"_a = std::string("alternatives")) .def(py::init<const std::string&>(), "name"_a = std::string("Alternatives"));
;
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"( py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"(
Plan for different alternatives in sequence. Plan for different alternatives in sequence
Try to find feasible solutions using first child. Try to find feasible solutions using the children in sequence. The behaviour slightly differs for the indivual stage types:
Only if this fails, proceed to the next child trying
an alternative planning strategy. - Generator: Proceed to next child if currently active one exhausted its solution, i.e. returns ``canCompute() == False``.
All solutions of the last active child are reported. - 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.
.. literalinclude:: ./../../../demo/scripts/fallbacks.py .. literalinclude:: ./../../../demo/scripts/fallbacks.py
:language: python :lines: 21-38
)") )")
.def(py::init<const std::string&>(), "name"_a = std::string("fallbacks")) .def(py::init<const std::string&>(), "name"_a = std::string("Fallbacks"));
;
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"( py::classh<Merger, ParallelContainerBase>(m, "Merger", R"(
Plan for different sub tasks in parallel and finally merge Plan for different sub tasks in parallel and eventually merge all sub solutions into a single trajectory
all sub solutions into a single trajectory This requires all children to operate on disjoint ``JointModelGroups``.
.. literalinclude:: ./../../../demo/scripts/merger.py .. literalinclude:: ./../../../demo/scripts/merger.py
:language: python
)") )")
.def(py::init<const std::string&>(), "name"_a = std::string("merger")) .def(py::init<const std::string&>(), "name"_a = std::string("merger"));
;
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"( py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"(
A wrapper wraps a single child stage, which can be Base class for wrapping containers, which can be used to filter or modify solutions generated by the single child.
accessed via ``wrapped()``. Implementations of this Implementations of this interface need to implement ``onNewSolution()`` to process a solution generated by the child.
interface need to implement ``onNewSolution()``, which The wrapper may reject the solution or create one or multiple derived solutions, potentially adapting the cost,
is called when the child has generated a new solution. the trajectory and output ``InterfaceStates``.
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.
)")
;
py::classh<Task>(m, "Task", R"( py::classh<Task>(m, "Task", R"(Root stage of a planning pipeline.
A task is the root of a tree of stages by A task stage usually wraps a single container (by default ``SerialContainer``) stage.
wrapping a single container The class provides methods to ``plan()`` for the configured pipeline and retrieve full solutions.)")
(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.
.. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python
)")
.def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true) .def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), .def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), "ns"_a = std::string(),
"ns"_a = std::string(), "introspection"_a = true, "container"_a) "introspection"_a = true, "container"_a)
// read-only access to properties + solutions
.def_property_readonly("properties", py::overload_cast<>(&Task::properties), .def_property_readonly("properties", py::overload_cast<>(&Task::properties),
"PropertyMap: Access the property map of the task") "PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Task::solutions, .def_property_readonly("solutions", &Task::solutions, "Successful Solutions of the stage (read-only)")
"Solutions: Access the solutions of the task, once the sub stage hierarchy has been traversed and planned") .def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def_property_readonly("failures", &Task::failures, .def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz")
"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",
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", "Load robot model from given parameter") "Load robot model from given ROS parameter")
.def("getRobotModel", &Task::getRobotModel) .def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, "Enable introspection publish for use with `rviz`") .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true,
.def("clear", &Task::clear, "Reset the stage hierarchy") "Enable publishing intermediate results for inspection in ``rviz``")
.def("add", &Task::add, "Add a stage to the task hierarchy", "stage"_a) .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("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* { .def(
"__getitem__",
[](const Task& t, const std::string& name) -> Stage* {
Stage* child = t.stages()->findChild(name); Stage* child = t.stages()->findChild(name);
if (!child) if (!child)
throw py::index_error(); throw py::index_error();
return child; return child;
}, py::return_value_policy::reference_internal) },
.def("__iter__", [](const Task &t) { py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const Task& t) {
const auto& children = t.stages()->pimpl()->children(); const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end()); 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") py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def("init", py::overload_cast<>(&Task::init), "Initialize all stages with a given scene") .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"( .def("plan", &Task::plan, "max_solutions"_a = 0, R"(
Reset, initialize scene (if not yet done), and initialize all Reset, init, and plan. Planning is limited to ``max_allowed_solutions``.
stages, then start planning with ``max_allowed_solutions``. Returns if planning Returns if planning was successful.)")
was successful.
)")
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)") .def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
.def("publish", [](Task& self, const SolutionBasePtr& solution) { .def(
self.introspection().publishSolution(*solution); "publish",
}, "solution"_a, "Publish a given solution to the solution ros topic") [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); },
.def("execute", [](const Task& self, const SolutionBasePtr& solution) { "solution"_a, "Publish the given solution to the ROS topic ``solution``")
moveit::planning_interface::PlanningSceneInterface psi; .def(
moveit::planning_interface::MoveGroupInterface "execute",
mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]); [](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; MoveGroupInterface::Plan plan;
moveit_task_constructor_msgs::Solution serialized; moveit_task_constructor_msgs::Solution serialized;
solution->fillMessage(serialized); solution->fillMessage(serialized);
@ -451,8 +430,8 @@ void export_core(pybind11::module& m) {
psi.applyPlanningScene(traj.scene_diff); psi.applyPlanningScene(traj.scene_diff);
} }
ROS_INFO("Executed successfully"); ROS_INFO("Executed successfully");
}, "solution"_a, "Execute Solution"); },
// clang-format on "solution"_a, "Send given solution to ``move_group`` node for execution");
} }
} // namespace python } // namespace python
} // namespace moveit } // namespace moveit

View File

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