Rework python documentation

This commit is contained in:
Christian Petersmeier 2022-01-22 09:26:17 -08:00 committed by Robert Haschke
parent 95f24747b0
commit 9103af2704
4 changed files with 379 additions and 996 deletions

View File

@ -42,6 +42,7 @@
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
namespace py = pybind11; namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
namespace moveit { namespace moveit {
@ -110,178 +111,90 @@ void export_core(pybind11::module& m) {
}); });
// clang-format off // clang-format off
py::classh<SolutionBase>(m, "Solution", R"pbdoc( py::classh<SolutionBase>(m, "Solution", "Solution class encapsulates a particular solution")
Solution class encapsulates a particular solution. .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost,"float: Cost that is associated with a particular solution")
)pbdoc") .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment that is associated with a particular solution")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, R"pbdoc(
float: Cost that is associated with a particular solution.
)pbdoc")
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, R"pbdoc(
str: Comment that is associated with a particular solution.
)pbdoc")
.def("toMsg", [](const SolutionBasePtr& s) { .def("toMsg", [](const SolutionBasePtr& s) {
moveit_task_constructor_msgs::Solution msg; moveit_task_constructor_msgs::Solution msg;
s->fillMessage(msg); s->fillMessage(msg);
return msg; return msg;
}, R"pbdoc( }, "Convert a solution object into a ros message type")
toMsg()
Args:
None
Returns:
ROS message type of the solution.
Convert a solution object into a ros message type.
)pbdoc")
; ;
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory", R"pbdoc( py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory", "A SubTrajectory connects interface states of compute stages")
SubTrajectory()
A SubTrajectory connects interface states of compute stages.
Args:
None
)pbdoc")
.def(py::init<>()) .def(py::init<>())
.def_property_readonly("start", &SolutionBase::start, R"pbdoc( .def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory. Readonly property")
InterfaceState: Start of the trajectory. Readonly property. .def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory. Readonly property")
)pbdoc") .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost of the solution")
.def_property_readonly("end", &SolutionBase::end, R"pbdoc( .def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "msg"_a)
InterfaceState: End of the trajectory. Readonly property. .def_property_readonly("isFailure", &SolutionBase::isFailure, "bool: True if the trajectory is marked as a failure. Readonly property")
)pbdoc") .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment, which can be assigned to the trajectory")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, R"pbdoc( .def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers), R"(
float: Cost of the solution.
)pbdoc")
.def("markAsFailure", &SolutionBase::markAsFailure, R"pbdoc(
markAsFailure(msg)
Args:
msg (str): Failure message.
Returns:
None
Mark the SubTrajectory as a failure.
)pbdoc")
.def_property_readonly("isFailure", &SolutionBase::isFailure, R"pbdoc(
bool: True if the trajectory is marked as a failure. Readonly property.
)pbdoc")
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, R"pbdoc(
str: Comment, which can be assigned to the trajectory.
)pbdoc")
.def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers), R"pbdoc(
Marker_: Markers that visualize the trajectory. Readonly property. Marker_: Markers that visualize the trajectory. Readonly property.
.. _Marker: https://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html .. _Marker: https://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html
)pbdoc") )")
; ;
using Solutions = ordered<SolutionBaseConstPtr>; using Solutions = ordered<SolutionBaseConstPtr>;
py::classh<Solutions>(m, "Solutions") py::classh<Solutions>(m, "Solutions", "Encapsulates multiple 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("__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>()) py::keep_alive<0, 1>())
; ;
py::classh<InterfaceState>(m, "InterfaceState", R"pbdoc( py::classh<InterfaceState>(m, "InterfaceState", R"(
InterfaceState(scene) InterfaceState describes a potential start or goal state
for a planning stage. A start or goal state for planning
Args: is essentially defined by the state of a planning scene.)")
scene (obj): Desired Planning Scene at the interface. .def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a)
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties),
InterfaceState describes a potential start or goal state "PropertyMap: Get access to the PropertyMap of the stage. Notice that this is a read-only property")
for a planning stage. A start or goal state for planning .def_property_readonly("scene", &InterfaceState::scene,
is essentially defined by the state of a planning scene. "PlanningScene: Get access to the planning scene of the interface stage. Notice that this is a read-only property")
)pbdoc")
.def(py::init<const planning_scene::PlanningScenePtr&>(), py::arg("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") py::classh<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode", "Encapsulates moveit error code message")
.def_readonly("val", &moveit::core::MoveItErrorCode::val) .def_readonly("val", &moveit::core::MoveItErrorCode::val, "int: Value of the error code")
.def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) { .def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) {
return pybind11::cast(static_cast<bool>(err)); return pybind11::cast(static_cast<bool>(err));
}); });
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"pbdoc( auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"(
Stage base type. All other stages derive from this type. Stage base type. All other stages derive from this type.
Object is not instantiable and should not be added to the task hierarchy Object is not instantiable and should not be added to the task hierarchy
in a standalone fashion. Rather, derive from generator or propagator stages in a standalone fashion. Rather, derive from generator or propagator stages
to implement custom logic. to implement custom logic.
)pbdoc") )")
.property<double>("timeout", R"pbdoc( .property<double>("timeout","float: Timeout of stage per computation")
float: Timeout of stage per computation. .property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage")
)pbdoc") .def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, "list: Get / set a list of forwarded properties")
.property<std::string>("marker_ns", R"pbdoc(
str: Namespace for any markers that are associated to the stage.
)pbdoc")
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, R"pbdoc(
list: Get / set a list of forwarded properties.
)pbdoc")
// expose name as writeable property // expose name as writeable property
.def_property("name", &Stage::name, &Stage::setName, R"pbdoc( .def_property("name", &Stage::name, &Stage::setName, "str: Get / set the name of the stage")
str: Get / set the name of the stage.
)pbdoc")
// read-only access to properties + solutions // read-only access to properties + solutions
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties), R"pbdoc( .def_property_readonly("properties", py::overload_cast<>(&Stage::properties), "PropertyMap: Return the property map of the stage. Readonly property")
PropertyMap: Return the property map of the stage. Readonly property. .def_property_readonly("solutions", &Stage::solutions, "Solutions: Get the solutions of a stage")
)pbdoc") .def_property_readonly("failures", &Stage::failures, "Solutions: Get the failed compuations of a stage")
.def_property_readonly("solutions", &Stage::solutions, R"pbdoc( .def("reset", &Stage::reset, "Reset the stage, clearing all solutions, interfaces and inherited properties")
Solutions: Get the solutions of a stage. .def("init", &Stage::init, R"(
)pbdoc") Initialize the stage once before planning. When called, properties configured for
.def_property_readonly("failures", &Stage::failures, R"pbdoc( initialization from parent are already defined. Push interfaces are not yet defined!
Solutions: Get the failed compuations of a stage. )", "robot_model"_a);
)pbdoc")
.def("reset", &Stage::reset, R"pbdoc(
reset()
Args: py::enum_<Stage::PropertyInitializerSource>(stage, "PropertyInitializerSource", R"(
None
Returns:
None
Reset the stage, clearing all solutions, interfaces and inherited properties.
)pbdoc")
.def("init", &Stage::init, R"pbdoc(
init(robot_model)
Args:
robot_model (RobotModel): Initialize the stage with a particular robot model.
Returns:
None
Initialize the stage once before planning.
When called, properties configured for initialization from parent are already defined.
Push interfaces are not yet defined!
)pbdoc");
py::enum_<Stage::PropertyInitializerSource>(stage, "PropertyInitializerSource", R"pbdoc(
Define, from where properties should be initialized when using the `configureInitFrom()` Define, from where properties should be initialized when using the `configureInitFrom()`
functions from the PropertyMap class. functions from the PropertyMap class.
)pbdoc") )")
.value("PARENT", Stage::PARENT, R"pbdoc( .value("PARENT", Stage::PARENT, "Inherit properties from parent stage in task hierarchy")
Inherit properties from parent stage in task hierarchy. .value("INTERFACE", Stage::INTERFACE, "Inherit properties from interface, i.e. preceeding stage in the task hierarchy")
)pbdoc")
.value("INTERFACE", Stage::INTERFACE, R"pbdoc(
Inherit properties from interface, i.e. preceeding stage
in the task hierarchy.
)pbdoc")
; ;
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay") auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay", "Base class for solution forwarding in both directions")
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingEitherWay")) .def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay"))
.def("restrictDirection", &PropagatingEitherWay::restrictDirection) .def("restrictDirection", &PropagatingEitherWay::restrictDirection, "Explicitly specify computation direction")
.def("computeForward", &PropagatingEitherWay::computeForward, "compute forward") .def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward")
.def("computeBackward", &PropagatingEitherWay::computeBackward, "compute backward") .def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
//.def("sendForward", &PropagatingEitherWay::sendForward) //.def("sendForward", &PropagatingEitherWay::sendForward)
//.def("sendBackward", &PropagatingEitherWay::sendBackward) //.def("sendBackward", &PropagatingEitherWay::sendBackward)
; ;
@ -291,19 +204,14 @@ void export_core(pybind11::module& m) {
.value("FORWARD", PropagatingEitherWay::FORWARD) .value("FORWARD", PropagatingEitherWay::FORWARD)
.value("BACKWARD", PropagatingEitherWay::BACKWARD); .value("BACKWARD", PropagatingEitherWay::BACKWARD);
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward") py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward", "Base class for forward solution propagation")
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingForward")) .def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"))
; ;
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(m, "PropagatingBackward") py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(m, "PropagatingBackward", "Base class for backward solution propagation")
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingBackward")) .def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"))
; ;
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"pbdoc( properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"(
Generator(name)
Args:
name (str): Name of the stage.
Derive from this stage to implement a custom stage Derive from this stage to implement a custom stage
that spawns a solution. When traversing through the whole that spawns a solution. When traversing through the whole
task hierarchy, the ``compute()`` function of this generator stage task hierarchy, the ``compute()`` function of this generator stage
@ -334,48 +242,16 @@ void export_core(pybind11::module& m) {
self.num = self.num - 1 self.num = self.num - 1
self.spawn(core.InterfaceState(self.ps), self.num) 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&>(), "name"_a = std::string("generator"))
.def("canCompute", &Generator::canCompute, R"pbdoc( .def("canCompute", &Generator::canCompute, "Guard for the ``compute()`` function")
canCompute() .def("compute", &Generator::compute, "Implement the stage's logic here. To interface with other stages, spawn an ``InterfaceState``")
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); },
Args: "Spawn an interface state that gets forwarded to the next stage",
None "state"_a, "cost"_a)
Returns:
bool: Should ``compute()`` be called?
Guard for the ``compute()`` function.
)pbdoc")
.def("compute", &Generator::compute, R"pbdoc(
compute()
Args:
None
Returns:
None
Implement the stage's logic here. To interface with other stages,
spawn an ``InterfaceState``.
)pbdoc")
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); }, R"pbdoc(
spawn(state, cost)
Args:
state (InterfaceState): The Interface State that should be used.
cost (float): The cost associated with that solution.
Returns:
None
Spawn an interface state that gets forwarded to the next stage.
)pbdoc")
; ;
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"pbdoc( properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"(
MonitoringGenerator(name)
Args:
name (str): Name of the stage.
Generator that monitors solutions of another stage to make reuse of them Generator that monitors solutions of another stage to make reuse of them
Sometimes its necessary to reuse a previously planned solution, e.g. to 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. traverse it in reverse order or to access the state of another generator.
@ -428,28 +304,17 @@ void export_core(pybind11::module& m) {
task["generator"].setMonitoredStage(task["current"]) task["generator"].setMonitoredStage(task["current"])
)pbdoc") )")
.def(py::init<const std::string&>(), py::arg("name") = std::string("generator")) .def(py::init<const std::string&>(), "name"_a = std::string("generator"))
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, R"pbdoc( .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the reference to the Monitored Stage", "stage"_a)
setMonitoredStage(stage)
Args:
stage (Stage): Monitor solutions of this stage.
Returns:
None
Set the reference to the Monitored Stage.
)pbdoc")
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution) .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution)
; ;
py::classh<ContainerBase, Stage>(m, "ContainerBase") py::classh<ContainerBase, Stage>(m, "ContainerBase", "Base class for containers that implements utility functionality")
.def("add", &ContainerBase::add) .def("add", &ContainerBase::add, "Add a stage to the container")
.def("insert", &ContainerBase::insert, py::arg("stage"), py::arg("before") = -1) .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), R"pbdoc( .def("remove", py::overload_cast<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
Remove child stage by index. .def("clear", &ContainerBase::clear, "Clear the stages of the container")
)pbdoc")
.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* {
Stage* child = c.findChild(name); Stage* child = c.findChild(name);
@ -463,32 +328,23 @@ void export_core(pybind11::module& m) {
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives }, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
; ;
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer") py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Base class for container containing a serial set of stages")
.def(py::init<const std::string&>(), py::arg("name") = std::string("serial container")); .def(py::init<const std::string&>(), "name"_a = std::string("serial container"));
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase"); py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase", "Base class for parallel containers");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"pbdoc(
Alternatives(name)
Args:
name (str): Name of the stage.
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 reported - sorted by cost.
.. literalinclude:: ./../../../demo/scripts/alternatives.py .. literalinclude:: ./../../../demo/scripts/alternatives.py
:language: python :language: python
)pbdoc") )")
.def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives")); .def(py::init<const std::string&>(), "name"_a = std::string("alternatives"))
;
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"pbdoc(
Fallbacks(name)
Args:
name (str): Name of the stage.
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 first child.
Only if this fails, proceed to the next child trying Only if this fails, proceed to the next child trying
@ -498,31 +354,22 @@ void export_core(pybind11::module& m) {
.. literalinclude:: ./../../../demo/scripts/fallbacks.py .. literalinclude:: ./../../../demo/scripts/fallbacks.py
:language: python :language: python
)pbdoc") )")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fallbacks")); .def(py::init<const std::string&>(), "name"_a = std::string("fallbacks"))
;
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"pbdoc(
Merger(name)
Args:
name (str): Name of the stage.
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 finally merge
all sub solutions into a single trajectory all sub solutions into a single trajectory
.. literalinclude:: ./../../../demo/scripts/merger.py .. literalinclude:: ./../../../demo/scripts/merger.py
:language: python :language: python
)pbdoc") )")
.def(py::init<const std::string&>(), py::arg("name") = std::string("merger")); .def(py::init<const std::string&>(), "name"_a = std::string("merger"))
;
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"pbdoc(
WrapperBase(name, child)
Args:
name (str): Name of the stage.
child (Stage): Wrapped stage.
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"(
A wrapper wraps a single child stage, which can be A wrapper wraps a single child stage, which can be
accessed via ``wrapped()``. Implementations of this accessed via ``wrapped()``. Implementations of this
interface need to implement ``onNewSolution()``, which interface need to implement ``onNewSolution()``, which
@ -530,15 +377,10 @@ void export_core(pybind11::module& m) {
The wrapper may reject the solution or create one or The wrapper may reject the solution or create one or
multiple derived solutions, potentially adapting the cost, multiple derived solutions, potentially adapting the cost,
as well as its start and end states. as well as its start and end states.
)pbdoc"); )")
;
py::classh<Task>(m, "Task", R"pbdoc(
Task(ns, introspection)
Args:
ns (str): Namespace of the task.
introspection (bool): Enable introspection.
py::classh<Task>(m, "Task", R"(
A task is the root of a tree of stages by A task is the root of a tree of stages by
wrapping a single container wrapping a single container
(by default a SerialContainer), (by default a SerialContainer),
@ -550,66 +392,23 @@ void export_core(pybind11::module& m) {
.. literalinclude:: ./../../../demo/scripts/current_state.py .. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python :language: python
)pbdoc") )")
.def(py::init<const std::string&, bool>(), py::arg("ns") = std::string(), py::arg("introspection") = true) .def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), .def(py::init<const std::string&, bool, ContainerBase::pointer&&>(),
py::arg("ns") = std::string(), py::arg("introspection") = true, py::arg("container")) "ns"_a = std::string(), "introspection"_a = true, "container"_a)
// read-only access to properties + solutions // read-only access to properties + solutions
.def_property_readonly("properties", py::overload_cast<>(&Task::properties), R"pbdoc( .def_property_readonly("properties", py::overload_cast<>(&Task::properties),
PropertyMap: Access the property map of the task. "PropertyMap: Access the property map of the task")
)pbdoc") .def_property_readonly("solutions", &Task::solutions,
.def_property_readonly("solutions", &Task::solutions, R"pbdoc( "Solutions: Access the solutions of the task, once the sub stage hierarchy has been traversed and planned")
Solutions: Access the solutions of the task, once the sub stage hierarchy has been .def_property_readonly("failures", &Task::failures,
traversed and planned. "Solutions: Inspect failures that occurred during the planning phase")
)pbdoc") .def_property("name", &Task::name, &Task::setName, "str: Set the name property of the task")
.def_property_readonly("failures", &Task::failures, R"pbdoc( .def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", "Load robot model from given parameter")
Solutions: Inspect failures that occurred during the planning phase.
)pbdoc")
.def_property("name", &Task::name, &Task::setName, R"pbdoc(
str: Set the name property of the task.
)pbdoc")
.def("loadRobotModel", &Task::loadRobotModel, py::arg("robot_description") = "robot_description",
R"pbdoc(
loadRobotModel(robot_description)
Args:
robot_description (str): Which robot model to load.
Returns:
None
Load robot model from given parameter.
)pbdoc")
.def("getRobotModel", &Task::getRobotModel) .def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, py::arg("enabled") = true, R"pbdoc( .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, "Enable introspection publish for use with `rviz`")
enableIntrospection(enable) .def("clear", &Task::clear, "Reset the stage hierarchy")
.def("add", &Task::add, "Add a stage to the task hierarchy", "stage"_a)
Args:
enable (bool): Defaults to true.
Returns:
None
Enable introspection publish for use with `rviz`.
)pbdoc")
.def("clear", &Task::clear, R"pbdoc(
clear()
Args:
None
Returns:
None
Reset the stage hierarchy.
)pbdoc")
.def("add", &Task::add, R"pbdoc(
add(stage)
Args:
stage (Stage): Stage to be added to the task hierarchy.
Returns:
None
Add a stage to the task hierarchy.
)pbdoc")
.def("__len__", [](const Task& t) { t.stages()->numChildren(); }) .def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* { .def("__getitem__", [](const Task& t, const std::string &name) -> Stage* {
Stage* child = t.stages()->findChild(name); Stage* child = t.stages()->findChild(name);
@ -621,59 +420,17 @@ void export_core(pybind11::module& m) {
const auto& children = t.stages()->pimpl()->children(); const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end()); return py::make_iterator(children.begin(), children.end());
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives }, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def("reset", &Task::reset, R"pbdoc( .def("reset", &Task::reset, "Reset all stages")
reset() .def("init", py::overload_cast<>(&Task::init), "Initialize all stages with a given scene")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(
Args:
None
Returns:
None
Reset all stages.
)pbdoc")
.def("init", py::overload_cast<>(&Task::init), R"pbdoc(
init()
Args:
None
Returns:
None
Initialize all stages with a given scene.
)pbdoc")
.def("plan", &Task::plan, py::arg("max_solutions") = 0, R"pbdoc(
plan(max_solutions)
Args:
max_solutions (int): Maximum allowed solutions of the planning process.
Returns:
Was the planning successful?
Reset, initialize scene (if not yet done), and initialize all Reset, initialize scene (if not yet done), and initialize all
stages, then start planning. stages, then start planning with ``max_allowed_solutions``. Returns if planning
)pbdoc") was successful.
.def("preempt", &Task::preempt, R"pbdoc( )")
preempt() .def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
Args:
None
Returns:
None
Interrupt current planning (or execution).
)pbdoc")
.def("publish", [](Task& self, const SolutionBasePtr& solution) { .def("publish", [](Task& self, const SolutionBasePtr& solution) {
self.introspection().publishSolution(*solution); self.introspection().publishSolution(*solution);
}, R"pbdoc( }, "solution"_a, "Publish a given solution to the solution ros topic")
publish(solution)
Args:
solution (Solution): Publish solution
Returns:
None
Publish a given solution to the solution ros topic.
)pbdoc")
.def("execute", [](const Task& self, const SolutionBasePtr& solution) { .def("execute", [](const Task& self, const SolutionBasePtr& solution) {
moveit::planning_interface::PlanningSceneInterface psi; moveit::planning_interface::PlanningSceneInterface psi;
moveit::planning_interface::MoveGroupInterface moveit::planning_interface::MoveGroupInterface
@ -693,17 +450,8 @@ void export_core(pybind11::module& m) {
} }
psi.applyPlanningScene(traj.scene_diff); psi.applyPlanningScene(traj.scene_diff);
} }
ROS_INFO("Executed successfully."); ROS_INFO("Executed successfully");
}, R"pbdoc( }, "solution"_a, "Execute Solution");
execute(solution)
Args:
solution (Solution): Solution, which should be executed.
Returns:
None
Execute Solution.
)pbdoc");
// clang-format on // clang-format on
} }
} // namespace python } // namespace python

View File

@ -36,6 +36,7 @@
#include <boost/core/demangle.hpp> #include <boost/core/demangle.hpp>
namespace py = pybind11; namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
@ -173,10 +174,10 @@ void export_properties(py::module& m) {
.def(py::init<>()) .def(py::init<>())
.def("setValue", [](Property& self, const py::object& value) .def("setValue", [](Property& self, const py::object& value)
{ self.setValue(PropertyConverterRegistry::fromPython(value)); }, { self.setValue(PropertyConverterRegistry::fromPython(value)); },
"Set current and default value.", py::arg("value")) "Set current and default value.", "value"_a)
.def("setCurrentValue", [](Property& self, const py::object& value) .def("setCurrentValue", [](Property& self, const py::object& value)
{ self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); }, { self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); },
"Set the current value only, w/o touching the default.", py::arg("value")) "Set the current value only, w/o touching the default.", "value"_a)
.def("value", [](const Property& self) .def("value", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.") { return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.")
.def("defaultValue", [](const Property& self) .def("defaultValue", [](const Property& self)
@ -186,9 +187,9 @@ void export_properties(py::module& m) {
.def("defined", &Property::defined, "Was a (non-default) value stored?") .def("defined", &Property::defined, "Was a (non-default) value stored?")
.def("description", &Property::description, "Retrive the property description string") .def("description", &Property::description, "Retrive the property description string")
.def("setDescription", &Property::setDescription, .def("setDescription", &Property::setDescription,
"Set the property's description", py::arg("desc")); "Set the property's description", "desc"_a);
py::classh<PropertyMap>(m, "PropertyMap", "dictionary of named :doc:`properties <pymoveit_mtc.core.Property>`") py::classh<PropertyMap>(m, "PropertyMap", "Dictionary of named :doc:`properties <pymoveit_mtc.core.Property>`")
.def(py::init<>()) .def(py::init<>())
.def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();}) .def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();})
.def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); }, .def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); },
@ -200,7 +201,7 @@ void export_properties(py::module& m) {
{ return self.property(key); }, py::return_value_policy::reference_internal, R"( { return self.property(key); }, py::return_value_policy::reference_internal, R"(
Retrieve the property instance for the given key. Retrieve the property instance for the given key.
This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)", This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)",
py::arg("key")) "key"_a)
.def("__getitem__", [](const PropertyMap& self, const std::string& key) .def("__getitem__", [](const PropertyMap& self, const std::string& key)
{ return PropertyConverterRegistry::toPython(self.get(key)); }) { return PropertyConverterRegistry::toPython(self.get(key)); })
.def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value) .def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value)
@ -211,29 +212,29 @@ void export_properties(py::module& m) {
self.set(it->first.cast<std::string>(), self.set(it->first.cast<std::string>(),
PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second))); PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second)));
} }
}, "Update property values from another dictionary", py::arg("values")) }, "Update property values from another dictionary", "values"_a)
.def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) { .def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) {
std::set<std::string> s; std::set<std::string> s;
for (auto& item : names) for (auto& item : names)
s.insert(item.cast<std::string>()); s.insert(item.cast<std::string>());
self.configureInitFrom(sources, s); self.configureInitFrom(sources, s);
}, "Configure initialization of listed (or all) properties from given source(s).", }, "Configure initialization of listed (or all) properties from given source(s).",
py::arg("sources"), py::arg("names") = py::list()) "sources"_a, "names"_a = py::list())
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) { .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) {
self.exposeTo(other, name, name); self.exposeTo(other, name, name);
}, "Declare ``named`` property in ``other`` PropertyMap - using same name.", }, "Declare ``named`` property in ``other`` PropertyMap - using same name.",
py::arg("other"), py::arg("name")) "other"_a, "name"_a)
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) { .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) {
self.exposeTo(other, name, other_name); self.exposeTo(other, name, other_name);
}, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.", }, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.",
py::arg("other"), py::arg("name"), py::arg("other_name")) "other"_a, "name"_a, "other_name"_a)
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) { .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) {
std::set<std::string> s; std::set<std::string> s;
for (auto& item : names) for (auto& item : names)
s.insert(item.cast<std::string>()); s.insert(item.cast<std::string>());
self.exposeTo(other, s); self.exposeTo(other, s);
}, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.", }, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.",
py::arg("other"), py::arg("names")) "other"_a, "names"_a)
; ;
// clang-format on // clang-format on
} }

View File

@ -51,15 +51,14 @@ namespace moveit {
namespace python { namespace python {
void export_solvers(py::module& m) { void export_solvers(py::module& m) {
properties::class_<PlannerInterface>(m, "PlannerInterface") properties::class_<PlannerInterface>(m, "PlannerInterface", "Base class for planning algorithms")
.property<double>("max_velocity_scaling_factor") .property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
.property<double>("max_acceleration_scaling_factor") .property<double>("max_acceleration_scaling_factor",
"float: Reduce the maximum acceleration by scaling between (0,1]")
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
py::return_value_policy::reference_internal); py::return_value_policy::reference_internal, "Properties of the planner");
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner", R"pbdoc(
PipelinePlanner()
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner", R"(
Use MoveIt's PlanningPipeline to plan a trajectory between to scenes. Use MoveIt's PlanningPipeline to plan a trajectory between to scenes.
:: ::
@ -85,38 +84,22 @@ void export_solvers(py::module& m) {
move.setGoal("extended") move.setGoal("extended")
task.add(move) task.add(move)
)pbdoc") )")
.property<std::string>("planner", R"pbdoc( .property<std::string>("planner", "str: Planner ID")
str: Planner ID. .property<uint>("num_planning_attempts", "int: Number of planning attempts")
)pbdoc") .property<moveit_msgs::WorkspaceParameters>("workspace_parameters", R"(
.property<uint>("num_planning_attempts", R"pbdoc(
uint: Number of planning attempts.
)pbdoc")
.property<moveit_msgs::WorkspaceParameters>("workspace_parameters", R"pbdoc(
WorkspaceParameters_: Workspace_parameters for the planning algorithm WorkspaceParameters_: Workspace_parameters for the planning algorithm
.. _WorkspaceParameters: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/WorkspaceParameters.html .. _WorkspaceParameters: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/WorkspaceParameters.html
)pbdoc") )")
.property<double>("goal_joint_tolerance", R"pbdoc( .property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
float: Tolerance for reaching joint goals. .property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
)pbdoc") .property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
.property<double>("goal_position_tolerance", R"pbdoc( .property<bool>("display_motion_plans", "bool: Publish generated solutions via a topic")
float: Tolerance for reaching position goals. .property<bool>("publish_planning_requests", "bool: Publish motion planning requests via a topic")
)pbdoc")
.property<double>("goal_orientation_tolerance", R"pbdoc(
float: Tolerance for reaching orientation goals.
)pbdoc")
.property<bool>("display_motion_plans", R"pbdoc(
bool: Publish generated solutions via a topic.
)pbdoc")
.property<bool>("publish_planning_requests", R"pbdoc(
bool: Publish motion planning requests via a topic.
)pbdoc")
.def(py::init<>()); .def(py::init<>());
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"pbdoc( properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"(
JointInterpolationPlanner()
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.
@ -127,16 +110,12 @@ void export_solvers(py::module& m) {
# Instantiate joint interpolation planner # Instantiate joint interpolation planner
jointPlanner = core.JointInterpolationPlanner() jointPlanner = core.JointInterpolationPlanner()
jointPlanner.max_step = 0.1 jointPlanner.max_step = 0.1
)pbdoc") )")
.property<double>("max_step", R"pbdoc( .property<double>("max_step",
float: Prevent large movements by specifying the maximum step distance " float: Prevent large movements by specifying the maximum step distance in joint space")
in joint space.
)pbdoc")
.def(py::init<>()); .def(py::init<>());
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"pbdoc( properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
CartesianPath()
The planner uses MoveIt's ``computeCartesianPath()`` to The planner uses MoveIt's ``computeCartesianPath()`` to
generate a straigh-line path between two scenes. generate a straigh-line path between two scenes.
@ -148,19 +127,17 @@ void export_solvers(py::module& m) {
cartesianPlanner = core.CartesianPath() cartesianPlanner = core.CartesianPath()
cartesianPlanner.step_size = 0.01 cartesianPlanner.step_size = 0.01
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
)pbdoc") )")
.property<double>("step_size", R"pbdoc( .property<double>("step_size", R"(
float: Specify the path interpolation resolution by adjusting the float: Specify the path interpolation resolution by adjusting the
step size between consecutive waypoints with this parameter. step size between consecutive waypoints with this parameter.
)pbdoc") )")
.property<double>("jump_threshold", R"pbdoc( .property<double>("jump_threshold", R"(
float: Prevent jumps in inverse kinematic solutions by adjusting the float: Prevent jumps in inverse kinematic solutions by adjusting the
acceptable fraction of mean joint motion per step. acceptable fraction of mean joint motion per step.
)pbdoc") )")
.property<double>("min_fraction", R"pbdoc( .property<double>("min_fraction",
float: Lower threshold of minimum motion per step required for "float: Lower threshold of minimum motion per step required for success in planning")
success in planning.
)pbdoc")
.def(py::init<>()); .def(py::init<>());
} }
} // namespace python } // namespace python

File diff suppressed because it is too large Load Diff