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)
moveit_task_constructor_msgs::Solution msg; .def_property_readonly("isFailure", &SolutionBase::isFailure,
s->fillMessage(msg); "bool: True if the trajectory is marked as a failure (read-only)")
return msg; .def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory (read-only)")
}, "Convert a solution object into a ros message type") .def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory (read-only)")
; .def_property_readonly(
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory", "A SubTrajectory connects interface states of compute stages") "markers", py::overload_cast<>(&SolutionBase::markers),
.def(py::init<>()) ":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)")
.def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory. Readonly property") .def(
.def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory. Readonly property") "toMsg",
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost of the solution") [](const SolutionBasePtr& s) {
.def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "msg"_a) moveit_task_constructor_msgs::Solution msg;
.def_property_readonly("isFailure", &SolutionBase::isFailure, "bool: True if the trajectory is marked as a failure. Readonly property") s->fillMessage(msg);
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment, which can be assigned to the trajectory") return msg;
.def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers), R"( },
Marker_: Markers that visualize the trajectory. Readonly property. "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>; 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: PropertyMap of the state (read-only).")
"PropertyMap: Get access to the PropertyMap of the stage. Notice that this is a read-only property") .def_property_readonly("scene", &InterfaceState::scene,
.def_property_readonly("scene", &InterfaceState::scene, "PlanningScene: PlanningScene of the state (read-only).");
"PlanningScene: Get access to the planning scene of the interface stage. Notice that this is a read-only property")
;
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 .property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage")
to implement custom logic. .def_property("forwarded_properties", getForwardedProperties, setForwardedProperties,
)") "list: set of properties forwarded from input to output InterfaceState")
.property<double>("timeout","float: Timeout of stage per computation") .def_property("name", &Stage::name, &Stage::setName, "str: name of the stage displayed e.g. in rviz")
.property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage") .def_property_readonly("properties", py::overload_cast<>(&Stage::properties),
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, "list: Get / set a list of forwarded properties") "PropertyMap: PropertyMap of the stage (read-only)")
// expose name as writeable property .def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)")
.def_property("name", &Stage::name, &Stage::setName, "str: Get / set the name of the stage") .def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)")
// read-only access to properties + solutions .def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties")
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties), "PropertyMap: Return the property map of the stage. Readonly property") .def("init", &Stage::init,
.def_property_readonly("solutions", &Stage::solutions, "Solutions: Get the solutions of a stage") "Initialize the stage once before planning. "
.def_property_readonly("failures", &Stage::failures, "Solutions: Get the failed compuations of a stage") "Will setup properties configured for initialization from parent.",
.def("reset", &Stage::reset, "Reset the stage, clearing all solutions, interfaces and inherited properties") "robot_model"_a);
.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);
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<>>(
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") py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction", "Propagation direction")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay")) .value("AUTO", PropagatingEitherWay::AUTO)
.def("restrictDirection", &PropagatingEitherWay::restrictDirection, "Explicitly specify computation direction") .value("FORWARD", PropagatingEitherWay::FORWARD, "Propagating forwards from start to end")
.def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward") .value("BACKWARD", PropagatingEitherWay::BACKWARD, "Propagating backwards from end to start");
.def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
//.def("sendForward", &PropagatingEitherWay::sendForward)
//.def("sendBackward", &PropagatingEitherWay::sendBackward)
;
py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction") py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(
.value("AUTO", PropagatingEitherWay::AUTO) m, "PropagatingForward", "Base class for forward-propagating stages")
.value("FORWARD", PropagatingEitherWay::FORWARD) .def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"));
.value("BACKWARD", PropagatingEitherWay::BACKWARD); py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(
m, "PropagatingBackward", "Base class for backward-propagating stages")
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward", "Base class for forward solution propagation") .def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"));
.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"))
;
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,226 +222,216 @@ 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):
""" Implements a custom 'MonitoringGenerator' stage."""
class PyMonitoringGenerator(core.MonitoringGenerator): solution_multiplier = 2
""" Implements a custom 'MonitoringGenerator' stage."""
solution_multiplier = 2 def __init__(self, name="MonitoringGenerator"):
core.MonitoringGenerator.__init__(self, name)
self.reset()
def __init__(self, name="MonitoringGenerator"): def reset(self):
core.MonitoringGenerator.__init__(self, name) core.MonitoringGenerator.reset(self)
self.reset() self.pending = []
def reset(self): def onNewSolution(self, sol):
core.MonitoringGenerator.reset(self) self.pending.append(sol)
self.upstream_solutions = list()
def onNewSolution(self, sol): def canCompute(self):
self.upstream_solutions.append(sol) return bool(self.pending)
def canCompute(self): def compute(self):
return bool(self.upstream_solutions) # 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): Upon creation of the stage, assign the monitored stage as follows::
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: 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() mg = PyMonitoringGenerator("generator")
current = stages.CurrentState("current") mg.setMonitoredStage(task["current"])
task.add(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)]) py::classh<ContainerBase, Stage>(m, "ContainerBase", R"(
task.add(connect) 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") py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Container implementing a linear planning sequence")
task.add(mg) .def(py::init<const std::string&>(), "name"_a = std::string("SerialContainer"));
task["generator"].setMonitoredStage(task["current"]) py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase",
"Abstract base class for parallel containers");
)")
.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<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.
All solutions of the last active child are reported.
.. literalinclude:: ./../../../demo/scripts/fallbacks.py - Generator: Proceed to next child if currently active one exhausted its solution, i.e. returns ``canCompute() == False``.
:language: python - 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
.def(py::init<const std::string&>(), "name"_a = std::string("fallbacks")) :lines: 21-38
; )")
.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), .def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
which serves as the root of all stages. .def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), "ns"_a = std::string(),
See below an exampel which creates a task with a "introspection"_a = true, "container"_a)
single stage in its hierarchy, fetching the current state
of the planning scene.
.. literalinclude:: ./../../../demo/scripts/current_state.py .def_property_readonly("properties", py::overload_cast<>(&Task::properties),
:language: python "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("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description",
.def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true) "Load robot model from given ROS parameter")
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), .def("getRobotModel", &Task::getRobotModel)
"ns"_a = std::string(), "introspection"_a = true, "container"_a) .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true,
// read-only access to properties + solutions "Enable publishing intermediate results for inspection in ``rviz``")
.def_property_readonly("properties", py::overload_cast<>(&Task::properties), .def("clear", &Task::clear, "Reset the stage task (and all its stages)")
"PropertyMap: Access the property map of the task") .def("add", &Task::add, "Append a stage to the task's top-level container", "stage"_a)
.def_property_readonly("solutions", &Task::solutions, .def("__len__", [](const Task& t) { t.stages()->numChildren(); })
"Solutions: Access the solutions of the task, once the sub stage hierarchy has been traversed and planned") .def(
.def_property_readonly("failures", &Task::failures, "__getitem__",
"Solutions: Inspect failures that occurred during the planning phase") [](const Task& t, const std::string& name) -> Stage* {
.def_property("name", &Task::name, &Task::setName, "str: Set the name property of the task") Stage* child = t.stages()->findChild(name);
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", "Load robot model from given parameter") if (!child)
.def("getRobotModel", &Task::getRobotModel) throw py::index_error();
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, "Enable introspection publish for use with `rviz`") return child;
.def("clear", &Task::clear, "Reset the stage hierarchy") },
.def("add", &Task::add, "Add a stage to the task hierarchy", "stage"_a) py::return_value_policy::reference_internal)
.def("__len__", [](const Task& t) { t.stages()->numChildren(); }) .def(
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* { "__iter__",
Stage* child = t.stages()->findChild(name); [](const Task& t) {
if (!child) const auto& children = t.stages()->pimpl()->children();
throw py::index_error(); return py::make_iterator(children.begin(), children.end());
return child; },
}, py::return_value_policy::reference_internal) py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def("__iter__", [](const Task &t) { .def("reset", &Task::reset, "Reset task (and all its stages)")
const auto& children = t.stages()->pimpl()->children(); .def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)")
return py::make_iterator(children.begin(), children.end()); .def("plan", &Task::plan, "max_solutions"_a = 0, R"(
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives Reset, init, and plan. Planning is limited to ``max_allowed_solutions``.
.def("reset", &Task::reset, "Reset all stages") Returns if planning was successful.)")
.def("init", py::overload_cast<>(&Task::init), "Initialize all stages with a given scene") .def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"( .def(
Reset, initialize scene (if not yet done), and initialize all "publish",
stages, then start planning with ``max_allowed_solutions``. Returns if planning [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); },
was successful. "solution"_a, "Publish the given solution to the ROS topic ``solution``")
)") .def(
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)") "execute",
.def("publish", [](Task& self, const SolutionBasePtr& solution) { [](const Task& self, const SolutionBasePtr& solution) {
self.introspection().publishSolution(*solution); using namespace moveit::planning_interface;
}, "solution"_a, "Publish a given solution to the solution ros topic") PlanningSceneInterface psi;
.def("execute", [](const Task& self, const SolutionBasePtr& solution) { MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
moveit::planning_interface::PlanningSceneInterface psi;
moveit::planning_interface::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);
for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) { for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) {
if (!traj.trajectory.joint_trajectory.points.empty()) { if (!traj.trajectory.joint_trajectory.points.empty()) {
plan.trajectory_ = traj.trajectory; plan.trajectory_ = traj.trajectory;
if (!mgi.execute(plan)) { if (!mgi.execute(plan)) {
ROS_ERROR("Execution failed! Aborting!"); ROS_ERROR("Execution failed! Aborting!");
return; return;
} }
} }
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);
} }