core docs, mwe monitoring generator

This commit is contained in:
cpetersmeier 2021-10-13 18:09:24 +02:00 committed by Robert Haschke
parent 65bc0a8703
commit f7c2fadde6
4 changed files with 117 additions and 6 deletions

View File

@ -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"
}

View File

@ -138,10 +138,26 @@ void export_core(pybind11::module& m) {
py::keep_alive<0, 1>())
;
py::classh<InterfaceState>(m, "InterfaceState")
py::classh<InterfaceState>(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<const planning_scene::PlanningScenePtr&>(), 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<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode")
@ -253,9 +269,74 @@ void export_core(pybind11::module& m) {
)pbdoc")
;
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator")
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(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<const std::string&>(), 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)
;

View File

@ -58,7 +58,33 @@ void export_solvers(py::module& m) {
py::return_value_policy::reference_internal);
properties::class_<PipelinePlanner, PlannerInterface>(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<std::string>("planner", R"pbdoc(
str: Planner ID.

View File

@ -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<const geometry_msgs::PointStamped&>(&MoveTo::setGoal), R"pbdoc(
setGoal(self, goal)
Args:
goal (PointStamped_): Desired configuration.