mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
core docs, mwe monitoring generator
This commit is contained in:
parent
65bc0a8703
commit
f7c2fadde6
@ -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"
|
||||
}
|
||||
|
||||
@ -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)
|
||||
;
|
||||
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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.
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user