From f7c2fadde6c949af3e7976b72f1f448787ba92e5 Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Wed, 13 Oct 2021 18:09:24 +0200 Subject: [PATCH] core docs, mwe monitoring generator --- core/doc/conf.py | 4 +- core/python/bindings/src/core.cpp | 91 ++++++++++++++++++++++++++-- core/python/bindings/src/solvers.cpp | 26 ++++++++ core/python/bindings/src/stages.cpp | 2 + 4 files changed, 117 insertions(+), 6 deletions(-) diff --git a/core/doc/conf.py b/core/doc/conf.py index 909dd3f7..8915635e 100644 --- a/core/doc/conf.py +++ b/core/doc/conf.py @@ -225,4 +225,6 @@ htmlhelp_basename = "mtcdoc" intersphinx_mapping = {"https://docs.python.org/3": None} # Default options for generating documentation. -autodoc_default_options = {"exclude-members": "ContainerBase, InitStageError"} +autodoc_default_options = { + "exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward" +} diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 1acab786..ee56c913 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -138,10 +138,26 @@ void export_core(pybind11::module& m) { py::keep_alive<0, 1>()) ; - py::classh(m, "InterfaceState") + py::classh(m, "InterfaceState", R"pbdoc( + InterfaceState(self, scene) + + Args: + scene (obj): Desired Planning Scene at the interface. + + 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. + + )pbdoc") .def(py::init(), py::arg("scene")) - .def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties)) - .def_property_readonly("scene", &InterfaceState::scene) + .def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), R"pbdoc( + PropertyMap: Get access to the PropertyMap of the stage. + Notice that this is a read-only property. + )pbdoc") + .def_property_readonly("scene", &InterfaceState::scene, R"pbdoc( + PlanningScene: Get access to the planning scene of the interface stage. + Notice that this is a read-only property. + )pbdoc") ; py::classh(m, "MoveItErrorCode") @@ -253,9 +269,74 @@ void export_core(pybind11::module& m) { )pbdoc") ; - properties::class_>(m, "MonitoringGenerator") + properties::class_>(m, "MonitoringGenerator", R"pbdoc( + MonitoringGenerator(self, name) + + Args: + name (str): Name of the stage. + + 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. + + You may derive from this stage to implement your own stage generation logic. + + :: + + class PyMonitoringGenerator(core.MonitoringGenerator): + """ Implements a custom 'MonitoringGenerator' stage.""" + + solution_multiplier = 2 + + def __init__(self, name="MonitoringGenerator"): + core.MonitoringGenerator.__init__(self, name) + self.reset() + + def reset(self): + core.MonitoringGenerator.reset(self) + self.upstream_solutions = list() + + def onNewSolution(self, sol): + self.upstream_solutions.append(sol) + + def canCompute(self): + return bool(self.upstream_solutions) + + 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: + + :: + + jointspace = core.JointInterpolationPlanner() + + task = core.Task() + current = stages.CurrentState("current") + task.add(current) + + connect = stages.Connect(planners=[('panda_arm', jointspace)]) + task.add(connect) + + mg = PyMonitoringGenerator("generator") + task.add(mg) + + task["generator"].setMonitoredStage(task["current"]) + + )pbdoc") .def(py::init(), py::arg("name") = std::string("generator")) - .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the reference to the Monitored Stage.") + .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, R"pbdoc( + setMonitoredStage(self, stage) + + Args: + stage (obj): Monitor solutions of this stage. + + Set the reference to the Monitored Stage. + )pbdoc") .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution) ; diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 9ac85b3d..75f07742 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -58,7 +58,33 @@ void export_solvers(py::module& m) { py::return_value_policy::reference_internal); properties::class_(m, "PipelinePlanner", R"pbdoc( + PipelinePlanner(self) + Use MoveIt's PlanningPipeline to plan a trajectory between to scenes. + + :: + + # create and configure a planner instance + pipelinePlanner = core.PipelinePlanner() + pipelinePlanner.planner = 'PRMkConfigDefault' + pipelinePlanner.num_planning_attempts = 10 + + # specify planning group + group = "panda_arm" + + # create a task + task = core.Task() + + # get the current robot state + currentState = stages.CurrentState("current state") + task.add(currentState) + + # moveTo named posture, using the pipeline planner interplation + move = stages.MoveTo("moveTo ready", pipelinePlanner) + move.group = group + move.setGoal("extended") + task.add(move) + )pbdoc") .property("planner", R"pbdoc( str: Planner ID. diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 4eb8a57a..54303ac8 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -367,6 +367,8 @@ void export_stages(pybind11::module& m) { .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( + setGoal(self, goal) + Args: goal (PointStamped_): Desired configuration.