mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +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
|
*.swp
|
||||||
*.pyc
|
*.pyc
|
||||||
__pycache__/
|
__pycache__/
|
||||||
|
*/doc/html/
|
||||||
|
*/doc/manifest.yaml
|
||||||
|
|||||||
@ -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 %}
|
||||||
|
|||||||
@ -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", ""),
|
||||||
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user