mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Rework core documentation
This commit is contained in:
parent
9103af2704
commit
e77fa83a42
2
.gitignore
vendored
2
.gitignore
vendored
@ -1,3 +1,5 @@
|
||||
*.swp
|
||||
*.pyc
|
||||
__pycache__/
|
||||
*/doc/html/
|
||||
*/doc/manifest.yaml
|
||||
|
@ -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 %}
|
||||
|
@ -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", ""),
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user