expand core docs, add detailed examples

This commit is contained in:
cpetersmeier 2021-10-08 19:41:09 +02:00 committed by Robert Haschke
parent 4f53663756
commit 65bc0a8703
3 changed files with 302 additions and 20 deletions

View File

@ -223,3 +223,6 @@ htmlhelp_basename = "mtcdoc"
# Example configuration for intersphinx: refer to the Python standard library. # Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {"https://docs.python.org/3": None} intersphinx_mapping = {"https://docs.python.org/3": None}
# Default options for generating documentation.
autodoc_default_options = {"exclude-members": "ContainerBase, InitStageError"}

View File

@ -190,11 +190,67 @@ void export_core(pybind11::module& m) {
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingBackward")) .def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingBackward"))
; ;
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator") properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"pbdoc(
Generator(self, name)
Args:
name (str): Name of the stage.
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``.
::
class PyGenerator(core.Generator):
""" Implements a custom 'Generator' stage with 3 compute() calls."""
max_calls = 3
def __init__(self, name="Generator"):
core.Generator.__init__(self, name)
self.reset()
def init(self, robot_model):
self.ps = PlanningScene(robot_model)
def reset(self):
core.Generator.reset(self)
self.num = self.max_calls
def canCompute(self):
return self.num > 0
def compute(self):
self.num = self.num - 1
self.spawn(core.InterfaceState(self.ps), self.num)
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("generator")) .def(py::init<const std::string&>(), py::arg("name") = std::string("generator"))
.def("canCompute", &Generator::canCompute) .def("canCompute", &Generator::canCompute, R"pbdoc(
.def("compute", &Generator::compute) canCompute(self)
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); })
Returns:
bool: Should ``compute()`` be called?
Guard for the ``compute()`` function.
)pbdoc")
.def("compute", &Generator::compute, R"pbdoc(
compute(self)
Implement the stage's logic here. To interface with other stages,
spwan an ``InterfaceState``.
)pbdoc")
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); }, R"pbdoc(
spawn(self, state, cost)
Args:
state (obj): The Interface State that should be used.
cost (double): The cost associated with that solution.
Spawn an interface state that gets forwarded to the next stage.
)pbdoc")
; ;
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator") properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator")
@ -206,7 +262,9 @@ void export_core(pybind11::module& m) {
py::classh<ContainerBase, Stage>(m, "ContainerBase") py::classh<ContainerBase, Stage>(m, "ContainerBase")
.def("add", &ContainerBase::add) .def("add", &ContainerBase::add)
.def("insert", &ContainerBase::insert, py::arg("stage"), py::arg("before") = -1) .def("insert", &ContainerBase::insert, py::arg("stage"), py::arg("before") = -1)
.def("remove", py::overload_cast<int>(&ContainerBase::remove), "Remove child stage by index") .def("remove", py::overload_cast<int>(&ContainerBase::remove), R"pbdoc(
Remove child stage by index.
)pbdoc")
.def("clear", &ContainerBase::clear) .def("clear", &ContainerBase::clear)
.def("__len__", &ContainerBase::numChildren) .def("__len__", &ContainerBase::numChildren)
.def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* { .def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* {
@ -227,31 +285,227 @@ void export_core(pybind11::module& m) {
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase"); py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"pbdoc( py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"pbdoc(
Alternatives(self, name)
Args:
name (str): Name of the stage.
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 reported - sorted by cost.
::
from moveit.task_constructor import core, stages
import rospy
import time
import numpy as np
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
jointPlanner = core.JointInterpolationPlanner()
task = core.Task()
# start from current robot state
currentState = stages.CurrentState("current state")
task.add(currentState)
alternatives = core.Alternatives('Alternatives')
goalConfig1 = {
'panda_joint1': 1.0,
'panda_joint2': -1.0,
'panda_joint3': 0.0,
'panda_joint4': -2.5,
'panda_joint5': 1.0,
'panda_joint6': 1.0,
'panda_joint7': 1.0,
}
goalConfig2 = {
'panda_joint1': -3.0,
'panda_joint2': -1.0,
'panda_joint3': 0.0,
'panda_joint4': -2.0,
'panda_joint5': 1.0,
'panda_joint6': 2.0,
'panda_joint7': 0.5,
}
# first motion plan to compare
moveTo1 = stages.MoveTo('Move To Goal Configuration 1', jointPlanner)
moveTo1.group = 'panda_arm'
moveTo1.setGoal(goalConfig1)
alternatives.insert(moveTo1)
# second motion plan to compare
moveTo2 = stages.MoveTo('Move To Goal Configuration 2', jointPlanner)
moveTo2.group = 'panda_arm'
moveTo2.setGoal(goalConfig2)
alternatives.insert(moveTo2)
task.add(alternatives)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
)pbdoc") )pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives"), R"pbdoc( .def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives"));
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"pbdoc(
Fallbacks(self, name)
Args: Args:
Name (str): Name of the object name (str): Name of the stage.
)pbdoc");
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks") 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.
::
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import rospy
import time
roscpp_init("mtc_tutorial")
# use cartesian and joint interpolation planners
cartesianPlanner = core.CartesianPath()
jointPlanner = core.JointInterpolationPlanner()
# initialize the mtc task
task = core.Task()
# add the current planning scene state to the task hierarchy
currentState = stages.CurrentState("Current State")
task.add(currentState)
# create a fallback container to fall back to a different planner
# if motion generation fails with the primary one
fallbacks = core.Fallbacks('Fallbacks')
# primary motion plan
moveTo1 = stages.MoveTo('Move To Goal Configuration 1', cartesianPlanner)
moveTo1.group = 'panda_arm'
moveTo1.setGoal('extended')
fallbacks.insert(moveTo1)
# fallback motion plan
moveTo2 = stages.MoveTo('Move To Goal Configuration 2', jointPlanner)
moveTo2.group = 'panda_arm'
moveTo2.setGoal('extended')
fallbacks.insert(moveTo2)
# add the fallback container to the task hierarchy
task.add(fallbacks)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fallbacks")); .def(py::init<const std::string&>(), py::arg("name") = std::string("fallbacks"));
py::classh<Merger, ParallelContainerBase>(m, "Merger") py::classh<Merger, ParallelContainerBase>(m, "Merger", R"pbdoc(
Merger(self, name)
Args:
name (str): Name of the stage.
Plan for different sub tasks in parallel and finally merge
all sub solutions into a single trajectory
::
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import rospy
import time
roscpp_init("mtc_tutorial")
# use the joint interpolation planner
planner = core.JointInterpolationPlanner()
# the task will contain our stages
task = core.Task()
# start from current robot state
currentState = stages.CurrentState("current state")
task.add(currentState)
# the merger plans for two parallel execution paths
merger = core.Merger('Merger')
# first simultaneous execution
moveTo1 = stages.MoveTo('Move To Home', planner)
moveTo1.group = 'hand'
moveTo1.setGoal('close')
merger.insert(moveTo1)
# second simultaneous execution
moveTo2 = stages.MoveTo('Move To Ready', planner)
moveTo2.group = 'panda_arm'
moveTo2.setGoal('extended')
merger.insert(moveTo2)
# add the merger stage to the task hierarchy
task.add(merger)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("merger")); .def(py::init<const std::string&>(), py::arg("name") = std::string("merger"));
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase"); py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"pbdoc(
WrapperBase(self, name, child)
Args:
name (str): Name of the stage.
child (obj): Wrapped stage.
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.
)pbdoc");
py::classh<Task>(m, "Task", R"pbdoc( py::classh<Task>(m, "Task", R"pbdoc(
A Task is the root of a tree of stages. Task(self, ns, introspection)
Implementation detail: Args:
A tasks wraps a single container ns (str): Namespace of the task.
introspection (bool): Enable introspection.
A task is the root of a tree of stages by
wrapping a single container
(by default a SerialContainer), (by default a SerialContainer),
which serves as the root of all stages. which serves as the root of all stages.
::
# Create a task
task = core.Task()
# add some stages e.g. currentState
currentState = stages.CurrentState("current state")
task.add(currentState)
# compute the stages that the task contains.
if task.plan():
# publish to the ros network
task.publish(task.solutions[0])
)pbdoc") )pbdoc")
.def(py::init<const std::string&, bool>(), py::arg("ns") = std::string(), py::arg("introspection") = true) .def(py::init<const std::string&, bool>(), py::arg("ns") = std::string(), py::arg("introspection") = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), .def(py::init<const std::string&, bool, ContainerBase::pointer&&>(),

View File

@ -87,26 +87,51 @@ void export_solvers(py::module& m) {
.def(py::init<>()); .def(py::init<>());
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"pbdoc( properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"pbdoc(
JointInterpolationPlanner(self)
Interpolate a trajectory between states in joint space. Interpolate a trajectory between states in joint space.
Fails if direct joint space interpolation fails. Fails if direct joint space interpolation fails.
::
from moveit.task_constructor import core
# Instantiate joint interpolation planner
jointPlanner = core.JointInterpolationPlanner()
jointPlanner.max_step = 0.1
)pbdoc") )pbdoc")
.property<double>("max_step", R"pbdoc( .property<double>("max_step", R"pbdoc(
double: Maximum joint step. double: Prevent large movements by specifying the maximum step distance
in joint space.
)pbdoc") )pbdoc")
.def(py::init<>()); .def(py::init<>());
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"pbdoc( properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"pbdoc(
Use MoveIt's computeCartesianPath() to CartesianPath(self)
The planner uses MoveIt's ``computeCartesianPath()`` to
generate a straigh-line path between two scenes. generate a straigh-line path between two scenes.
::
from moveit.task_constructor import core
# Instantiate cartesian path planner
cartesianPlanner = core.CartesianPath()
cartesianPlanner.step_size = 0.01
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
)pbdoc") )pbdoc")
.property<double>("step_size", R"pbdoc( .property<double>("step_size", R"pbdoc(
double: Step size between consecutive waypoints. double: Specify the path interpolation resolution by adjusting the
step size between consecutive waypoints with this parameter.
)pbdoc") )pbdoc")
.property<double>("jump_threshold", R"pbdoc( .property<double>("jump_threshold", R"pbdoc(
double: Acceptable fraction of mean joint motion per step. double: Prevent jumps in inverse kinematic solutions by adjusting the
acceptable fraction of mean joint motion per step.
)pbdoc") )pbdoc")
.property<double>("min_fraction", R"pbdoc( .property<double>("min_fraction", R"pbdoc(
double: Fraction of motion required for success. double: Lower threshold of minimum motion per step required for
success in planning.
)pbdoc") )pbdoc")
.def(py::init<>()); .def(py::init<>());
} }