diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 86169132..bfe04bf9 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -42,6 +42,7 @@ #include namespace py = pybind11; +using namespace py::literals; using namespace moveit::task_constructor; namespace moveit { @@ -110,178 +111,90 @@ void export_core(pybind11::module& m) { }); // clang-format off - py::classh(m, "Solution", R"pbdoc( - Solution class encapsulates a particular solution. - )pbdoc") - .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") + py::classh(m, "Solution", "Solution class encapsulates a particular solution") + .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost,"float: Cost that is associated with a particular solution") + .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment that is associated with a particular solution") .def("toMsg", [](const SolutionBasePtr& s) { moveit_task_constructor_msgs::Solution msg; s->fillMessage(msg); return msg; - }, R"pbdoc( - toMsg() - - Args: - None - Returns: - ROS message type of the solution. - - Convert a solution object into a ros message type. - )pbdoc") + }, "Convert a solution object into a ros message type") ; - py::classh(m, "SubTrajectory", R"pbdoc( - SubTrajectory() - - A SubTrajectory connects interface states of compute stages. - - Args: - None - )pbdoc") + py::classh(m, "SubTrajectory", "A SubTrajectory connects interface states of compute stages") .def(py::init<>()) - .def_property_readonly("start", &SolutionBase::start, R"pbdoc( - InterfaceState: Start of the trajectory. Readonly property. - )pbdoc") - .def_property_readonly("end", &SolutionBase::end, R"pbdoc( - InterfaceState: End of the trajectory. Readonly property. - )pbdoc") - .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, R"pbdoc( - 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( + .def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory. Readonly property") + .def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory. Readonly property") + .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost of the solution") + .def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "msg"_a) + .def_property_readonly("isFailure", &SolutionBase::isFailure, "bool: True if the trajectory is marked as a failure. Readonly property") + .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment, which can be assigned to the trajectory") + .def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers), R"( Marker_: Markers that visualize the trajectory. Readonly property. .. _Marker: https://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html - )pbdoc") + )") ; using Solutions = ordered; - py::classh(m, "Solutions") + py::classh(m, "Solutions", "Encapsulates multiple solutions") .def("__len__", &Solutions::size) .def("__getitem__", &get_item) .def("__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); }, py::keep_alive<0, 1>()) ; - py::classh(m, "InterfaceState", R"pbdoc( - InterfaceState(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), 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, "InterfaceState", R"( + 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.)") + .def(py::init(), "scene"_a) + .def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), + "PropertyMap: Get access to the PropertyMap of the stage. Notice that this is a read-only property") + .def_property_readonly("scene", &InterfaceState::scene, + "PlanningScene: Get access to the planning scene of the interface stage. Notice that this is a read-only property") ; - py::classh(m, "MoveItErrorCode") - .def_readonly("val", &moveit::core::MoveItErrorCode::val) + py::classh(m, "MoveItErrorCode", "Encapsulates moveit error code message") + .def_readonly("val", &moveit::core::MoveItErrorCode::val, "int: Value of the error code") .def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast(err)); }); - auto stage = properties::class_>(m, "Stage", R"pbdoc( + auto stage = properties::class_>(m, "Stage", R"( Stage base type. All other stages derive from this type. Object is not instantiable and should not be added to the task hierarchy in a standalone fashion. Rather, derive from generator or propagator stages to implement custom logic. - )pbdoc") - .property("timeout", R"pbdoc( - float: Timeout of stage per computation. - )pbdoc") - .property("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") + )") + .property("timeout","float: Timeout of stage per computation") + .property("marker_ns", "str: Namespace for any markers that are associated to the stage") + .def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, "list: Get / set a list of forwarded properties") // expose name as writeable property - .def_property("name", &Stage::name, &Stage::setName, R"pbdoc( - str: Get / set the name of the stage. - )pbdoc") + .def_property("name", &Stage::name, &Stage::setName, "str: Get / set the name of the stage") // read-only access to properties + solutions - .def_property_readonly("properties", py::overload_cast<>(&Stage::properties), R"pbdoc( - PropertyMap: Return the property map of the stage. Readonly property. - )pbdoc") - .def_property_readonly("solutions", &Stage::solutions, R"pbdoc( - Solutions: Get the solutions of a stage. - )pbdoc") - .def_property_readonly("failures", &Stage::failures, R"pbdoc( - Solutions: Get the failed compuations of a stage. - )pbdoc") - .def("reset", &Stage::reset, R"pbdoc( - reset() + .def_property_readonly("properties", py::overload_cast<>(&Stage::properties), "PropertyMap: Return the property map of the stage. Readonly property") + .def_property_readonly("solutions", &Stage::solutions, "Solutions: Get the solutions of a stage") + .def_property_readonly("failures", &Stage::failures, "Solutions: Get the failed compuations of a stage") + .def("reset", &Stage::reset, "Reset the stage, clearing all solutions, interfaces and inherited properties") + .def("init", &Stage::init, R"( + Initialize the stage once before planning. When called, properties configured for + initialization from parent are already defined. Push interfaces are not yet defined! + )", "robot_model"_a); - Args: - 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", R"pbdoc( + py::enum_(stage, "PropertyInitializerSource", R"( Define, from where properties should be initialized when using the `configureInitFrom()` functions from the PropertyMap class. - )pbdoc") - .value("PARENT", Stage::PARENT, R"pbdoc( - Inherit properties from parent stage in task hierarchy. - )pbdoc") - .value("INTERFACE", Stage::INTERFACE, R"pbdoc( - Inherit properties from interface, i.e. preceeding stage - in the task hierarchy. - )pbdoc") + )") + .value("PARENT", Stage::PARENT, "Inherit properties from parent stage in task hierarchy") + .value("INTERFACE", Stage::INTERFACE, "Inherit properties from interface, i.e. preceeding stage in the task hierarchy") ; - auto either_way = py::classh>(m, "PropagatingEitherWay") - .def(py::init(), py::arg("name") = std::string("PropagatingEitherWay")) - .def("restrictDirection", &PropagatingEitherWay::restrictDirection) - .def("computeForward", &PropagatingEitherWay::computeForward, "compute forward") - .def("computeBackward", &PropagatingEitherWay::computeBackward, "compute backward") + auto either_way = py::classh>(m, "PropagatingEitherWay", "Base class for solution forwarding in both directions") + .def(py::init(), "name"_a = std::string("PropagatingEitherWay")) + .def("restrictDirection", &PropagatingEitherWay::restrictDirection, "Explicitly specify computation direction") + .def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward") + .def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward") //.def("sendForward", &PropagatingEitherWay::sendForward) //.def("sendBackward", &PropagatingEitherWay::sendBackward) ; @@ -291,19 +204,14 @@ void export_core(pybind11::module& m) { .value("FORWARD", PropagatingEitherWay::FORWARD) .value("BACKWARD", PropagatingEitherWay::BACKWARD); - py::classh>(m, "PropagatingForward") - .def(py::init(), py::arg("name") = std::string("PropagatingForward")) + py::classh>(m, "PropagatingForward", "Base class for forward solution propagation") + .def(py::init(), "name"_a = std::string("PropagatingForward")) ; - py::classh>(m, "PropagatingBackward") - .def(py::init(), py::arg("name") = std::string("PropagatingBackward")) + py::classh>(m, "PropagatingBackward", "Base class for backward solution propagation") + .def(py::init(), "name"_a = std::string("PropagatingBackward")) ; - properties::class_>(m, "Generator", R"pbdoc( - Generator(name) - - Args: - name (str): Name of the stage. - + properties::class_>(m, "Generator", R"( 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 @@ -334,48 +242,16 @@ void export_core(pybind11::module& m) { self.num = self.num - 1 self.spawn(core.InterfaceState(self.ps), self.num) - )pbdoc") - .def(py::init(), py::arg("name") = std::string("generator")) - .def("canCompute", &Generator::canCompute, R"pbdoc( - canCompute() - - Args: - None - 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") + )") + .def(py::init(), "name"_a = std::string("generator")) + .def("canCompute", &Generator::canCompute, "Guard for the ``compute()`` function") + .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); }, + "Spawn an interface state that gets forwarded to the next stage", + "state"_a, "cost"_a) ; - properties::class_>(m, "MonitoringGenerator", R"pbdoc( - MonitoringGenerator(name) - - Args: - name (str): Name of the stage. - + properties::class_>(m, "MonitoringGenerator", R"( 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. @@ -428,28 +304,17 @@ void export_core(pybind11::module& m) { task["generator"].setMonitoredStage(task["current"]) - )pbdoc") - .def(py::init(), py::arg("name") = std::string("generator")) - .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, R"pbdoc( - setMonitoredStage(stage) - - Args: - stage (Stage): Monitor solutions of this stage. - Returns: - None - - Set the reference to the Monitored Stage. - )pbdoc") + )") + .def(py::init(), "name"_a = std::string("generator")) + .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the reference to the Monitored Stage", "stage"_a) .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution) ; - py::classh(m, "ContainerBase") - .def("add", &ContainerBase::add) - .def("insert", &ContainerBase::insert, py::arg("stage"), py::arg("before") = -1) - .def("remove", py::overload_cast(&ContainerBase::remove), R"pbdoc( - Remove child stage by index. - )pbdoc") - .def("clear", &ContainerBase::clear) + py::classh(m, "ContainerBase", "Base class for containers that implements utility functionality") + .def("add", &ContainerBase::add, "Add a stage to the container") + .def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1, "Insert a stage before given index into container") + .def("remove", py::overload_cast(&ContainerBase::remove), "Remove child stage by index", "pos"_a) + .def("clear", &ContainerBase::clear, "Clear the stages of the container") .def("__len__", &ContainerBase::numChildren) .def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* { 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::classh(m, "SerialContainer") - .def(py::init(), py::arg("name") = std::string("serial container")); + py::classh(m, "SerialContainer", "Base class for container containing a serial set of stages") + .def(py::init(), "name"_a = std::string("serial container")); - py::classh(m, "ParallelContainerBase"); - - py::classh(m, "Alternatives", R"pbdoc( - Alternatives(name) - - Args: - name (str): Name of the stage. + py::classh(m, "ParallelContainerBase", "Base class for parallel containers"); + py::classh(m, "Alternatives", R"( Plan for different alternatives in parallel. Solution of all children are reported - sorted by cost. .. literalinclude:: ./../../../demo/scripts/alternatives.py :language: python - )pbdoc") - .def(py::init(), py::arg("name") = std::string("alternatives")); - - py::classh(m, "Fallbacks", R"pbdoc( - Fallbacks(name) - - Args: - name (str): Name of the stage. + )") + .def(py::init(), "name"_a = std::string("alternatives")) + ; + py::classh(m, "Fallbacks", R"( Plan for different alternatives in sequence. Try to find feasible solutions using first child. 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 :language: python - )pbdoc") - .def(py::init(), py::arg("name") = std::string("fallbacks")); - - py::classh(m, "Merger", R"pbdoc( - Merger(name) - - Args: - name (str): Name of the stage. + )") + .def(py::init(), "name"_a = std::string("fallbacks")) + ; + py::classh(m, "Merger", R"( Plan for different sub tasks in parallel and finally merge all sub solutions into a single trajectory .. literalinclude:: ./../../../demo/scripts/merger.py :language: python - )pbdoc") - .def(py::init(), py::arg("name") = std::string("merger")); - - py::classh(m, "WrapperBase", R"pbdoc( - WrapperBase(name, child) - - Args: - name (str): Name of the stage. - child (Stage): Wrapped stage. + )") + .def(py::init(), "name"_a = std::string("merger")) + ; + py::classh(m, "WrapperBase", R"( A wrapper wraps a single child stage, which can be accessed via ``wrapped()``. Implementations of this 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 multiple derived solutions, potentially adapting the cost, as well as its start and end states. - )pbdoc"); - - py::classh(m, "Task", R"pbdoc( - Task(ns, introspection) - - Args: - ns (str): Namespace of the task. - introspection (bool): Enable introspection. + )") + ; + py::classh(m, "Task", R"( A task is the root of a tree of stages by wrapping a single container (by default a SerialContainer), @@ -550,66 +392,23 @@ void export_core(pybind11::module& m) { .. literalinclude:: ./../../../demo/scripts/current_state.py :language: python - )pbdoc") - .def(py::init(), py::arg("ns") = std::string(), py::arg("introspection") = true) + )") + .def(py::init(), "ns"_a = std::string(), "introspection"_a = true) .def(py::init(), - 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 - .def_property_readonly("properties", py::overload_cast<>(&Task::properties), R"pbdoc( - PropertyMap: Access the property map of the task. - )pbdoc") - .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. - )pbdoc") - .def_property_readonly("failures", &Task::failures, R"pbdoc( - 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_property_readonly("properties", py::overload_cast<>(&Task::properties), + "PropertyMap: Access the property map of the task") + .def_property_readonly("solutions", &Task::solutions, + "Solutions: Access the solutions of the task, once the sub stage hierarchy has been traversed and planned") + .def_property_readonly("failures", &Task::failures, + "Solutions: Inspect failures that occurred during the planning phase") + .def_property("name", &Task::name, &Task::setName, "str: Set the name property of the task") + .def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", "Load robot model from given parameter") .def("getRobotModel", &Task::getRobotModel) - .def("enableIntrospection", &Task::enableIntrospection, py::arg("enabled") = true, R"pbdoc( - enableIntrospection(enable) - - 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("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, "Enable introspection publish for use with `rviz`") + .def("clear", &Task::clear, "Reset the stage hierarchy") + .def("add", &Task::add, "Add a stage to the task hierarchy", "stage"_a) .def("__len__", [](const Task& t) { t.stages()->numChildren(); }) .def("__getitem__", [](const Task& t, const std::string &name) -> Stage* { Stage* child = t.stages()->findChild(name); @@ -621,59 +420,17 @@ void export_core(pybind11::module& m) { const auto& children = t.stages()->pimpl()->children(); return py::make_iterator(children.begin(), children.end()); }, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives - .def("reset", &Task::reset, R"pbdoc( - reset() - - 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? - + .def("reset", &Task::reset, "Reset all stages") + .def("init", py::overload_cast<>(&Task::init), "Initialize all stages with a given scene") + .def("plan", &Task::plan, "max_solutions"_a = 0, R"( Reset, initialize scene (if not yet done), and initialize all - stages, then start planning. - )pbdoc") - .def("preempt", &Task::preempt, R"pbdoc( - preempt() - - Args: - None - Returns: - None - - Interrupt current planning (or execution). - )pbdoc") + stages, then start planning with ``max_allowed_solutions``. Returns if planning + was successful. + )") + .def("preempt", &Task::preempt, "Interrupt current planning (or execution)") .def("publish", [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); - }, R"pbdoc( - publish(solution) - - Args: - solution (Solution): Publish solution - Returns: - None - - Publish a given solution to the solution ros topic. - )pbdoc") + }, "solution"_a, "Publish a given solution to the solution ros topic") .def("execute", [](const Task& self, const SolutionBasePtr& solution) { moveit::planning_interface::PlanningSceneInterface psi; moveit::planning_interface::MoveGroupInterface @@ -693,17 +450,8 @@ void export_core(pybind11::module& m) { } psi.applyPlanningScene(traj.scene_diff); } - ROS_INFO("Executed successfully."); - }, R"pbdoc( - execute(solution) - - Args: - solution (Solution): Solution, which should be executed. - Returns: - None - - Execute Solution. - )pbdoc"); + ROS_INFO("Executed successfully"); + }, "solution"_a, "Execute Solution"); // clang-format on } } // namespace python diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp index 1c4e3464..60d25547 100644 --- a/core/python/bindings/src/properties.cpp +++ b/core/python/bindings/src/properties.cpp @@ -36,6 +36,7 @@ #include namespace py = pybind11; +using namespace py::literals; using namespace moveit::task_constructor; PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) @@ -173,10 +174,10 @@ void export_properties(py::module& m) { .def(py::init<>()) .def("setValue", [](Property& self, const py::object& 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) { 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) { return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.") .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("description", &Property::description, "Retrive the property description string") .def("setDescription", &Property::setDescription, - "Set the property's description", py::arg("desc")); + "Set the property's description", "desc"_a); - py::classh(m, "PropertyMap", "dictionary of named :doc:`properties `") + py::classh(m, "PropertyMap", "Dictionary of named :doc:`properties `") .def(py::init<>()) .def("__bool__", [](const PropertyMap& self) { return 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"( Retrieve the property instance for the given key. 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) { return PropertyConverterRegistry::toPython(self.get(key)); }) .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(), PropertyConverterRegistry::fromPython(py::reinterpret_borrow(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) { std::set s; for (auto& item : names) s.insert(item.cast()); self.configureInitFrom(sources, 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) { self.exposeTo(other, name, 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) { self.exposeTo(other, name, 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) { std::set s; for (auto& item : names) s.insert(item.cast()); self.exposeTo(other, s); }, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.", - py::arg("other"), py::arg("names")) + "other"_a, "names"_a) ; // clang-format on } diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 9102b44b..94be99d1 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -51,15 +51,14 @@ namespace moveit { namespace python { void export_solvers(py::module& m) { - properties::class_(m, "PlannerInterface") - .property("max_velocity_scaling_factor") - .property("max_acceleration_scaling_factor") + properties::class_(m, "PlannerInterface", "Base class for planning algorithms") + .property("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]") + .property("max_acceleration_scaling_factor", + "float: Reduce the maximum acceleration by scaling between (0,1]") .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), - py::return_value_policy::reference_internal); - - properties::class_(m, "PipelinePlanner", R"pbdoc( - PipelinePlanner() + py::return_value_policy::reference_internal, "Properties of the planner"); + properties::class_(m, "PipelinePlanner", R"( Use MoveIt's PlanningPipeline to plan a trajectory between to scenes. :: @@ -85,38 +84,22 @@ void export_solvers(py::module& m) { move.setGoal("extended") task.add(move) - )pbdoc") - .property("planner", R"pbdoc( - str: Planner ID. - )pbdoc") - .property("num_planning_attempts", R"pbdoc( - uint: Number of planning attempts. - )pbdoc") - .property("workspace_parameters", R"pbdoc( + )") + .property("planner", "str: Planner ID") + .property("num_planning_attempts", "int: Number of planning attempts") + .property("workspace_parameters", R"( WorkspaceParameters_: Workspace_parameters for the planning algorithm .. _WorkspaceParameters: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/WorkspaceParameters.html - )pbdoc") - .property("goal_joint_tolerance", R"pbdoc( - float: Tolerance for reaching joint goals. - )pbdoc") - .property("goal_position_tolerance", R"pbdoc( - float: Tolerance for reaching position goals. - )pbdoc") - .property("goal_orientation_tolerance", R"pbdoc( - float: Tolerance for reaching orientation goals. - )pbdoc") - .property("display_motion_plans", R"pbdoc( - bool: Publish generated solutions via a topic. - )pbdoc") - .property("publish_planning_requests", R"pbdoc( - bool: Publish motion planning requests via a topic. - )pbdoc") + )") + .property("goal_joint_tolerance", "float: Tolerance for reaching joint goals") + .property("goal_position_tolerance", "float: Tolerance for reaching position goals") + .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") + .property("display_motion_plans", "bool: Publish generated solutions via a topic") + .property("publish_planning_requests", "bool: Publish motion planning requests via a topic") .def(py::init<>()); - properties::class_(m, "JointInterpolationPlanner", R"pbdoc( - JointInterpolationPlanner() - + properties::class_(m, "JointInterpolationPlanner", R"( Interpolate a trajectory between states in joint space. Fails if direct joint space interpolation fails. @@ -127,16 +110,12 @@ void export_solvers(py::module& m) { # Instantiate joint interpolation planner jointPlanner = core.JointInterpolationPlanner() jointPlanner.max_step = 0.1 - )pbdoc") - .property("max_step", R"pbdoc( - float: Prevent large movements by specifying the maximum step distance - in joint space. - )pbdoc") + )") + .property("max_step", + " float: Prevent large movements by specifying the maximum step distance in joint space") .def(py::init<>()); - properties::class_(m, "CartesianPath", R"pbdoc( - CartesianPath() - + properties::class_(m, "CartesianPath", R"( The planner uses MoveIt's ``computeCartesianPath()`` to generate a straigh-line path between two scenes. @@ -148,19 +127,17 @@ void export_solvers(py::module& m) { cartesianPlanner = core.CartesianPath() cartesianPlanner.step_size = 0.01 cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. - )pbdoc") - .property("step_size", R"pbdoc( + )") + .property("step_size", R"( float: Specify the path interpolation resolution by adjusting the step size between consecutive waypoints with this parameter. - )pbdoc") - .property("jump_threshold", R"pbdoc( + )") + .property("jump_threshold", R"( float: Prevent jumps in inverse kinematic solutions by adjusting the acceptable fraction of mean joint motion per step. - )pbdoc") - .property("min_fraction", R"pbdoc( - float: Lower threshold of minimum motion per step required for - success in planning. - )pbdoc") + )") + .property("min_fraction", + "float: Lower threshold of minimum motion per step required for success in planning") .def(py::init<>()); } } // namespace python diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index a1e28904..0a15d0de 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -42,6 +42,7 @@ #include namespace py = pybind11; +using namespace py::literals; using namespace moveit::task_constructor; using namespace moveit::task_constructor::stages; @@ -80,142 +81,61 @@ std::vector elementOrList(const py::object& arg) { void export_stages(pybind11::module& m) { // clang-format off - properties::class_(m, "ModifyPlanningScene", R"pbdoc( - ModifyPlanningScene(name) - + properties::class_(m, "ModifyPlanningScene", R"( Allows modification of the planning scene. + This stage takes the incoming planning scene and applies previously scheduled changes to it, for example: - Modify allowed collision matrix, enabling or disabling collision pairs. - Attach or detach objects to robot links. - Spawn or remove objects. - Args: - name (str): Name of the stage. - .. literalinclude:: ./../../../demo/scripts/modify_planning_scene.py :language: python - )pbdoc") - .def(py::init(), py::arg("name") = std::string("modify planning scene")) - .def("attachObject", &ModifyPlanningScene::attachObject, R"pbdoc( - attachObject(name, link) - - Args: - name (str): Name of the object - link (str): Name of the link, to which - the object should be attached. - Returns: - None - )pbdoc") - .def("detachObject", &ModifyPlanningScene::detachObject, R"pbdoc( - detachObject(name, link) - - Detach an object from a robot link. - - Args: - name (str): Object name that should be detached. - link (str): Link name from which the object should be detached. - Returns: - None - )pbdoc") + )") + .def(py::init(), "name"_a = std::string("modify planning scene")) + .def("attachObject", &ModifyPlanningScene::attachObject, "Attach an object to a robot link", "name"_a, "link"_a) + .def("detachObject", &ModifyPlanningScene::detachObject, "Detach an object from a robot link", "name"_a, "link"_a) .def("attachObjects", [](ModifyPlanningScene& self, const py::object& names, const std::string& attach_link, bool attach) { self.attachObjects(elementOrList(names), attach_link, attach); - }, py::arg("names"), py::arg("attach_link"), py::arg("attach") = true, R"pbdoc( - attachObjects(attach_link, attach) - - Attach multiple objects to a robot link. - - Args: - names (list): Objects that should be attached. - attach_link (str): Link to which the objects should be attached. - attach (bool): Set to true to attach the objects. - Returns: - None - )pbdoc") + }, "Attach multiple objects to a robot link", "names"_a, "attach_link"_a, "attach"_a = true) .def("detachObjects", [](ModifyPlanningScene& self, const py::object& names, const std::string& attach_link) { self.attachObjects(elementOrList(names), attach_link, false); - }, py::arg("names"), py::arg("attach_link"), R"pbdoc( - detachObjects(attach_link) - - Detach multiple objects from a robot link. - - Args: - names (list): Objects that should be attached. - attach_link (str): Link from which the objects should be detached. - Returns: - None - )pbdoc") + }, "Detach multiple objects from a robot link", "names"_a, "attach_link"_a) .def("allowCollisions", [](ModifyPlanningScene& self, const py::object& first, const py::object& second, bool enable_collision) { self.allowCollisions(elementOrList(first), elementOrList(second), enable_collision); - }, py::arg("first"), py::arg("second"), py::arg("enable_collision") = true, R"pbdoc( - allowCollisions(first, second, enable_collision) - - Allow or disable collisions between links and objects. - - Args: - first (str): Name of the first object or link. - second (str): Name of the second object or link. - enable_collision (bool): Set to true to enable collisions checks; - set to false to disable collision checks. - Returns: - None - )pbdoc") - .def("addObject", &ModifyPlanningScene::addObject, R"pbdoc( - addObject(collision_object) - - Add an object to the planning scene - - Args: - collision_object (CollisionObject_): Object to be added. Must be - in the appropriate message format. - Returns: - None + }, "Allow or disable collisions between links and objects", "first"_a, "second"_a, "enable_collision"_a = true) + .def("addObject", &ModifyPlanningScene::addObject, R"( + Add a CollisionObject_ to the planning scene .. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html - )pbdoc"); - - properties::class_(m, "CurrentState", R"pbdoc( - CurrentState(name) + )", "collision_object"_a); + properties::class_(m, "CurrentState", R"( Fetch the current PlanningScene state via the ``get_planning_scene`` service. - Args: - name (str): Name of the stage. - .. literalinclude:: ./../../../demo/scripts/current_state.py :language: python - )pbdoc") - .def(py::init(), py::arg("name") = std::string("current state")); - - properties::class_(m, "FixedState", R"pbdoc( - FixedState(name) + )") + .def(py::init(), "name"_a = std::string("current state")); + properties::class_(m, "FixedState", R"( Spawn a pre-defined PlanningScene state. - Args: - name (str): Name of the stage. - .. literalinclude:: ./../../../demo/scripts/fixed_state.py :language: python - )pbdoc") - .def("setState", &FixedState::setState, R"pbdoc( - setState(scene) - + )") + .def("setState", &FixedState::setState, R"( Use a planning scene pointer to specify which state the Fixed State stage should have. - - Args: - scene (PlanningScenePtr): The desired planning scene state. - Returns: - None - )pbdoc") - .def(py::init(), py::arg("name") = std::string("fixed state")); + )", "scene"_a) + .def(py::init(), "name"_a = std::string("fixed state")); #if 0 .def("setState", [](FixedState& stage, const moveit_msg::PlanningScene& scene_msg) { @@ -227,9 +147,7 @@ void export_stages(pybind11::module& m) { #endif ; - properties::class_(m, "ComputeIK", R"pbdoc( - ComputeIK(name, stage) - + properties::class_(m, "ComputeIK", R"( Wrapper for any pose generator stage to compute the inverse kinematics for a pose in Cartesian space. @@ -246,42 +164,38 @@ void export_stages(pybind11::module& m) { Properties of the internally received ``InterfaceState`` can be forwarded to the newly generated, externally exposed ``InterfaceState``. - Args: - name (str): Name of the stage. - stage: Stage that contains the robot state for IK calculation. - .. literalinclude:: ./../../../demo/scripts/compute_ik.py :language: python - )pbdoc") - .property("eef", R"pbdoc( + )") + .property("eef", R"( str: Specify which end effector of the active planning group should be used. - )pbdoc") - .property("group", R"pbdoc( + )") + .property("group", R"( str: Specify which planning group should be used. - )pbdoc") - .property("default_pose", R"pbdoc( + )") + .property("default_pose", R"( str: Default joint pose of the active group (defines cost of the inverse kinematics). - )pbdoc") - .property("max_ik_solutions", R"pbdoc( + )") + .property("max_ik_solutions", R"( int: Set the maximum number of inverse kinematic solutions thats should be generated. - )pbdoc") - .property("ignore_collisions", R"pbdoc( + )") + .property("ignore_collisions", R"( bool: Specify if collisions with other members of the planning scene are allowed. - )pbdoc") - .property("ik_frame", R"pbdoc( + )") + .property("ik_frame", R"( PoseStamped_: Specify the frame with respect to which the inverse kinematics should be calculated. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .property("target_pose", R"pbdoc( + )") + .property("target_pose", R"( PoseStamped_: Specify the pose on which the inverse kinematics should be calculated on. Since this property should @@ -290,99 +204,60 @@ void export_stages(pybind11::module& m) { if possible, avoid setting it manually. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") + )") // methods of base class py::class_ need to be called last! - .def(py::init()); - - properties::class_>(m, "MoveTo", R"pbdoc( - MoveTo(name, planner) + .def(py::init(), "name"_a, "stage"_a); + properties::class_>(m, "MoveTo", R"( Compute a trajectory between the robot state from the interface state of the preceeding stage and a specified - goal. - - Args: - name (str): Name of the stage. - planner (PlannerInterface): Planner that is used to compute the path of motion. + goal .. literalinclude:: ./../../../demo/scripts/cartesian.py :language: python :lines: 51-55 - )pbdoc") - .property("group", R"pbdoc( + )") + .property("group", R"( str: Planning group which should be utilized for planning and execution. - )pbdoc") - .property("ik_frame", R"pbdoc( - PoseStamped_: IK reference frame for the goal pose. + )") + .property("ik_frame", R"( + PoseStamped_: IK reference frame for the goal pose .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .property("path_constraints", R"pbdoc( - Constraints_: Set path constraints via the corresponding moveit message type. + )") + .property("path_constraints", R"( + Constraints_: Set path constraints via the corresponding moveit message type .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html - )pbdoc") - .def(py::init()) - .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( - setGoal(goal) - - 1. Move link to a given pose. - - Args: - goal (PoseStamped_): Desired configuration. - Returns: - None + )") + .def(py::init(), "name"_a, "planner"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move link to a given PoseStamped_ .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( - 2. Move link to given point, keeping current orientation. - - Args: - goal (PointStamped_): Desired configuration. - Returns: - None + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move link to given PointStamped_, keeping current orientation .. _PointStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html - )pbdoc") - .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( - 3. Move joints specified in msg to their target values. - - Args: - goal (RobotState_): Desired configuration. - Returns: - None + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move joints specified in RobotState_ to their target values .. _RobotState: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotState.html - )pbdoc") - .def("setGoal", py::overload_cast&>(&MoveTo::setGoal), R"pbdoc( - 4. Move joints by name to their mapped target value. - - Args: - goal (dict): Desired configuration given in joint - value mappings. - Returns: - None - )pbdoc") - .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( - 5. Move joint model group to given named pose. - - Args: - goal (str): Desired configuration as a name of a known pose. - Returns: - None - )pbdoc"); - - properties::class_>(m, "MoveRelative", R"pbdoc( - MoveRelative(name, planner) + )", "goal"_a) + .def("setGoal", py::overload_cast&>(&MoveTo::setGoal), R"( + Move joints by name to their mapped target value provided by dict goal argument + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move joint model group to given named pose provided as a str argument + )", "goal"_a); + properties::class_>(m, "MoveRelative", R"( Perform a Cartesian motion relative to some link. - Args: - name (str): Name of the stage. - planner (PlannerInterface): Planner that is used to compute the path of motion. - .. literalinclude:: ./../../../demo/scripts/cartesian.py :language: python :lines: 26-31 @@ -394,78 +269,46 @@ void export_stages(pybind11::module& m) { :language: python :lines: 72-87 - - )pbdoc") - .property("group", R"pbdoc( + )") + .property("group", R"( str: Planning group which should be utilized for planning and execution. - )pbdoc") - .property("ik_frame", R"pbdoc( + )") + .property("ik_frame", R"( PoseStamped_: IK reference frame for the goal pose. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .property("min_distance", R"pbdoc( - float: Set the minimum distance to move. - )pbdoc") - .property("max_distance", R"pbdoc( - float: Set the maximum distance to move. - )pbdoc") - .property("path_constraints", R"pbdoc( + )") + .property("min_distance", "float: Set the minimum distance to move") + .property("max_distance", "float: Set the maximum distance to move") + .property("path_constraints", R"( Constraints_: These are the path constraints. .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html - )pbdoc") - .def(py::init()) - .def("setDirection", py::overload_cast(&MoveRelative::setDirection), - R"pbdoc( - setDirection(twist) - - 1. Perform twist motion on specified link. - - Args: - twist (Twist_): Use a Twist message as movement direction description. - Returns: - None + )") + .def(py::init(), "name"_a, "planner"_a) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"( + Perform twist motion on specified link. .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html - )pbdoc") - .def("setDirection", py::overload_cast(&MoveRelative::setDirection), - R"pbdoc( - 2. Translate link along given direction. - - Args: - direction (Vector3Stamped_): Desired direction. - Returns: - None + )", "twist"_a) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"( + Translate link along given direction. .. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html - )pbdoc") - .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), - R"pbdoc( - 3. Move specified joint variables by given amount. + )", "direction"_a) + .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), R"( + Move specified joint variables by given amount. + )", "joint_deltas"_a); - Args: - joint_deltas (dict): Desired direction, - given as a (joint_name: str, joint_value: float), mapping. - Returns: - None - )pbdoc"); - - py::enum_(m, "MergeMode", R"pbdoc( + py::enum_(m, "MergeMode", R"( Define the merge strategy to use when performing planning operations with e.g. the connect stage. - )pbdoc") - .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL, R"pbdoc( - Store sequential trajectories. - )pbdoc") - .value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS, R"pbdoc( - Join trajectories by their waypoints. - )pbdoc"); + )") + .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL, "Store sequential trajectories") + .value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS, "Join trajectories by their waypoints"); PropertyConverter(); - properties::class_(m, "Connect", R"pbdoc( - Connect(name, planners) - + properties::class_(m, "Connect", R"( Connect arbitrary InterfaceStates by motion planning. You can specify the planning groups and the planners you want to utilize. @@ -478,131 +321,95 @@ void export_stages(pybind11::module& m) { sub trajectories of individual planning results. If this fails, the sequential planning result is returned. - Args: - Name (str): Name of the stage. - Planners (list): List of the planner - group associations. - The example below contains a snippet from the :ref:`pick pipeline example`. .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python :lines: 48-60 - )pbdoc") + )") .def(py::init(), - py::arg("name") = std::string("connect"), py::arg("planners")); - - properties::class_(m, "FixCollisionObjects", R"pbdoc( - FixCollisionObjects(name) + "name"_a = std::string("connect"), "planners"_a); + properties::class_(m, "FixCollisionObjects", R"( Test for collisions and find a correction for applicable objects. Move the objects out of the way along the correction direction. - Args: - name (str): Name of the stage. - .. literalinclude:: ./../../../demo/scripts/fix_collision_objects.py :language: python - )pbdoc") - .property("max_penetration", R"pbdoc( + )") + .property("max_penetration", R"( float: Cutoff length up to which collision objects get fixed. - )pbdoc") - .def(py::init(), py::arg("name") = std::string("fix collisions")); - - properties::class_(m, "GeneratePlacePose", R"pbdoc( - GeneratePlacePose(name) + )") + .def(py::init(), "name"_a = std::string("fix collisions")); + properties::class_(m, "GeneratePlacePose", R"( GeneratePlacePose stage derives from monitoring generator and generates poses for the place pipeline. Notice that whilst GenerateGraspPose spawns poses with an ``angle_delta`` intervall, GeneratePlacePose samples a fixed amount, which is dependent on the objects shape. - Args: - name (str): Name of the stage. - The example below contains a snippet from the :ref:`pick pipeline example`. .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python :lines: 115-122 - )pbdoc") - .property("object", R"pbdoc( - str: Name of the object in the planning scene, - attached to the robot which should be placed. - )pbdoc") - .property("eef", R"pbdoc( - str: Name of the end effector that should be used for grasping. - )pbdoc") - .property("pose") - .def(py::init(), py::arg("name") = std::string("Generate Place Pose")); + )") + .property("object", R"( + str: Name of the object in the planning scene, attached to the robot which should be placed + )") + .property("eef", "str: Name of the end effector that should be used for grasping") + .property("pose", R"( + PoseStamped_: The pose where the object should be placed, i.e. states should be sampled + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def(py::init(), "name"_a = std::string("Generate Place Pose")); - properties::class_(m, "GenerateGraspPose", R"pbdoc( - GenerateGraspPose(name) - + properties::class_(m, "GenerateGraspPose", R"( GenerateGraspPose stage derives from monitoring generator and can be used to generate poses for grasping. Set the desired attributes of the grasp using the stages properties. - Args: - name (str): Name of the stage. - The example below contains a snippet from the :ref:`pick pipeline example`. .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python :lines: 62-68 - )pbdoc") - .property("object", R"pbdoc( - str: Name of the Object in the planning scene, which should be grasped. - )pbdoc") - .property("eef", R"pbdoc( - str: Name of the end effector that should be used for grasping. - )pbdoc") - .property("pregrasp", R"pbdoc( - str: Name of the pre-grasp pose. - )pbdoc") - .property("grasp", R"pbdoc( - str: Name of the grasp pose. - )pbdoc") - .property("angle_delta", R"pbdoc( + )") + .property("object", R"( + str: Name of the Object in the planning scene, which should be grasped + )") + .property("eef", R"( + str: Name of the end effector that should be used for grasping + )") + .property("pregrasp", "str: Name of the pre-grasp pose") + .property("grasp", "str: Name of the grasp pose") + .property("angle_delta", R"( float: Angular step distance in rad with which positions around the object are sampled. - )pbdoc") - .def(py::init(), py::arg("name") = std::string("Generate Grasp Pose")); - - properties::class_(m, "GeneratePose", R"pbdoc( - GeneratePose(name) + )") + .def(py::init(), "name"_a = std::string("Generate Grasp Pose")); + properties::class_(m, "GeneratePose", R"( Monitoring generator stage which can be used to generate a pose, based on solutions provided by the monitored stage. - Args: - name (str): Name of the stage. - .. literalinclude:: ./../../../demo/scripts/generate_pose.py :language: python :lines: 35-48 - )pbdoc") - .property("pose", R"pbdoc( + )") + .property("pose", R"( PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .def(py::init()); - - properties::class_(m, "Pick", R"pbdoc( - Pick(grasp_generator, name) - - Args: - grasp_generator (SimpleGrasp): Simple Grasp stage to provide - poses and inverse kinematics solutions. - name (str): Name of the stage. - - .. _pick: + )") + .def(py::init(), "name"_a); + properties::class_(m, "Pick", R"( The Pick stage is a specialization of the PickPlaceBase class, which wraps the pipeline to pick or place an object with a given end effector. @@ -623,72 +430,32 @@ void export_stages(pybind11::module& m) { .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python - )pbdoc") - .property("object", R"pbdoc( - str: Name of object to pick. - )pbdoc") - .property("eef", R"pbdoc( - str: The End effector name. - )pbdoc") - .property("eef_frame", R"pbdoc( - str: Name of the end effector frame. - )pbdoc") - .property("eef_group", R"pbdoc( - str: Joint model group of the end effector. - )pbdoc") - .property("eef_parent_group", R"pbdoc( - str: Joint model group of the eef's parent. - )pbdoc") - .def(py::init(), py::arg("grasp_generator"), - py::arg("name") = std::string("pick")) - .def("setApproachMotion", &Pick::setApproachMotion, R"pbdoc( - setApproachMotion(motion, min_distance, max_distance) - - Args: - motion (Twist_): The twist, which represents the - approach motion. - min_distance (float): Minimum allowed distance. - max_distance (float): Maximum allowed distance. - Returns: - None - + )") + .property("object", "str: Name of object to pick") + .property("eef", "str: The End effector name") + .property("eef_frame", "str: Name of the end effector frame") + .property("eef_group", "str: Joint model group of the end effector") + .property("eef_parent_group", "str: Joint model group of the eef's parent") + .def(py::init(), "grasp_generator"_a, + "name"_a = std::string("pick")) + .def("setApproachMotion", &Pick::setApproachMotion, R"( The approaching motion towards the grasping state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html - )pbdoc") - .def("setLiftMotion", py::overload_cast(&Pick::setLiftMotion), R"pbdoc( - setLiftMotion(motion, min_distance, max_distance) - - 1. The lifting motion away from the grasping state is represented by a twist message. - - Args: - motion (Twist_): The twist, which represents the - lift motion. - min_distance (float): Minimum allowed distance. - max_distance (float): Maximum allowed distance. - Returns: - None + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setLiftMotion", py::overload_cast(&Pick::setLiftMotion), R"( + The lifting motion away from the grasping state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html - )pbdoc") - .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion), R"pbdoc( - 2. The lifting motion away from the grasping state is represented by its destination as joint-value pairs. - - Args: - place (dict): The place where the object should be lifted to, - given as joint-value pairs. - Returns: - None - )pbdoc"); - - properties::class_(m, "Place", R"pbdoc( - Place(place_generator, name) - - Args: - place_generator (SimpleUnGrasp): SimpleUnGrasp Wrapper for pose generation - name (str): Name of the stage. + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion), R"( + The lifting motion away from the grasping state is represented by its destination as joint-value pairs + )", "place"_a); + properties::class_(m, "Place", R"( The Place stage is a specialization of the PickPlaceBase class, which wraps the pipeline to pick or place an object with a given end effector. @@ -702,236 +469,126 @@ void export_stages(pybind11::module& m) { as the end effector's Cartesian pose needs to be provided by an external grasp stage. - For a working example, please consider the Pick_ Stage. + For a working example, please consider the :doc:`Pick ` stage. .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python - :lines: 131-135 - )pbdoc") - .property("object", R"pbdoc( - str: Name of object to pick. - )pbdoc") - .property("eef", R"pbdoc( - str: The End effector name. - )pbdoc") - .property("eef_frame", R"pbdoc( - str: Name of the end effector frame. - )pbdoc") - .property("eef_group", R"pbdoc( - str: Joint model group of the end effector. - )pbdoc") - .property("eef_parent_group", R"pbdoc( - str: Joint model group of the eef's parent. - )pbdoc") - .def("setRetractMotion", &Place::setRetractMotion, R"pbdoc( - setRetractMotion(motion, min_distance, max_distance) - - Args: - motion (Twist_): The twist, which represents the - retract motion. - min_distance (float): Minimum allowed distance. - max_distance (float): Maximum allowed distance. - Returns: - None - + :lines: 102-118 + )") + .property("object", "str: Name of object to pick") + .property("eef", "str: The End effector name") + .property("eef_frame", "str: Name of the end effector frame") + .property("eef_group", "str: Joint model group of the end effector") + .property("eef_parent_group", "str: Joint model group of the eef's parent") + .def("setRetractMotion", &Place::setRetractMotion, R"( The retract motion towards the final state is represented - by a twist message. + by a Twist_ message. Additionally specify the minimum and + maximum allowed distances to travel. .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html - )pbdoc") - .def("setPlaceMotion", py::overload_cast(&Place::setPlaceMotion), R"pbdoc( - setPlaceMotion(motion, min_distance, max_distance) - - 1. The object-placing motion towards the final state is represented by a twist message. - - Args: - motion (Twist_): The twist, which represents the - place motion. - min_distance (float): Minimum allowed distance. - max_distance (float): Maximum allowed distance. - Returns: - None + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setPlaceMotion", py::overload_cast(&Place::setPlaceMotion), R"( + The object-placing motion towards the final state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a ) + .def("setPlaceMotion", py::overload_cast&>(&Place::setPlaceMotion), R"( + The placing motion to the final state is represented by its destination as joint-value pairs + )", "joints"_a ) + .def(py::init(), "place_generator"_a, + "name"_a = std::string("place")); - )pbdoc") - .def("setPlaceMotion", py::overload_cast&>(&Place::setPlaceMotion), R"pbdoc( - 2. The placing motion to the final state is represented by its destination as joint-value pairs. - - Args: - joints (dict): The place where the object should be placed at, - given as joint-value pairs. - Returns: - None - )pbdoc") - .def(py::init(), py::arg("place_generator"), - py::arg("name") = std::string("place")); - - properties::class_(m, "SimpleGrasp", R"pbdoc( - SimpleGrasp(pose_generator, name) - - Args: - pose_generator (GenerateGraspPose): Generator stage to - sample possible grasp poses. - name (str): Name of the stage. - Returns: - None - + properties::class_(m, "SimpleGrasp", R"( Specialization of SimpleGraspBase to realize grasping. - Refer to the pick_ stage for a minimum code example: + Refer to the :doc:`Pick ` + stage for a minimum code example: .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python - :lines: 70-79 - )pbdoc") - .property("eef", R"pbdoc( - str: The end effector of the robot. - )pbdoc") - .property("object", R"pbdoc( - str: The object to grasp (Must be present in the planning scene). - )pbdoc") - .property("ik_frame", R"pbdoc( + :lines: 58-64 + )") + .property("eef", "str: The end effector of the robot") + .property("object", "str: The object to grasp (Must be present in the planning scene)") + .property("ik_frame", R"( PoseStamped_: Set the frame for which the inverse kinematics are calculated with respect to each pose generated by the pose_generator. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .def(py::init(), py::arg("pose_generator"), - py::arg("name") = std::string("grasp generator")) - .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( - setIKFrame(transform) - - 1. Set the frame for which the inverse kinematics are calculated with respect to - each pose generated by the pose_generator. - - Args: - transform (PoseStamped_): Transform to the IK Frame. - Returns: - None + )") + .def(py::init(), "pose_generator"_a, + "name"_a = std::string("grasp generator")) + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"( + Set the frame as a PoseStamped_ for which the inverse kinematics are calculated with respect to + each pose generated by the pose_generator. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( - 2. Set the frame for which the inverse kinematics are calculated - with respect to each pose generated by the pose_generator. - - Args: - pose (PoseStamped_): Transform from the given link to the IK frame. - link (str): Base link for pose transform to IK frame. - Returns: - None - )pbdoc") - .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( - 3. Set the frame for which the inverse kinematics are calculated - with respect to each pose generated by the pose_generator. - - Args: - link (str): IK Frame will be placed at the base frame of this link. - Returns: - None - )pbdoc") - .def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"pbdoc( - setMaxIKSolutions(max_ik_solutions) - - Args: - max_ik_solutions (int): Maximum number of ik solutions. - Returns: - None + )", "transform"_a) + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"( + Set the frame as a PoseStamped_ for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "pose"_a, "link"_a) + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"( + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + )", "link"_a) + .def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"( Set the maximum number of inverse kinematics solutions that should be computed. + )", "max_ik_solutions"_a); - )pbdoc"); - - properties::class_(m, "SimpleUnGrasp", R"pbdoc( - SimpleUnGrasp(pose_generator, name) - - Args: - pose_generator (GeneratePlacePose): Generator stage to - sample possible Place - poses. - name (str): Name of the stage. - Returns: - None - + properties::class_(m, "SimpleUnGrasp", R"( Specialization of SimpleGraspBase to realize ungrasping - Refer to the place_ stage for a minimum code example. - Make shure to set the ``grasp`` and ``pregrasp`` properties - here again since they are not forwarded through the stage - hierarchy. + Refer to the :doc:`Place ` + stage for a minimum code example. + Be sure to forward the ``grasp`` and ``pregrasp`` properties + through the stage hierarchy so that they are available here again. .. literalinclude:: ./../../../demo/scripts/pickplace.py :language: python - :lines: 124-129 + :lines: 99-100 - )pbdoc") - .property("eef", R"pbdoc( - str: The end effector of the robot. - )pbdoc") - .property("object", R"pbdoc( - str: The object to grasp (Must be present in the planning scene). - )pbdoc") - .property("pregrasp", R"pbdoc( - str: Name of the pre-grasp pose. - )pbdoc") - .property("grasp", R"pbdoc( - str: Name of the grasp pose. - )pbdoc") - .property("ik_frame", R"pbdoc( + )") + .property("eef", "str: The end effector of the robot") + .property("object", "str: The object to grasp (Must be present in the planning scene)") + .property("pregrasp", "str: Name of the pre-grasp pose") + .property("grasp", "str: Name of the grasp pose") + .property("ik_frame", R"( PoseStamped_: Specify the frame with respect to which the inverse kinematics should be calculated. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc( - setIKFrame(transform) - - 1. Set the frame for which the inverse kinematics are calculated - with respect to each pose generated by the pose_generator. - - Args: - transform (PoseStamped_): Transform to the IK Frame. - Returns: - None + )") + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"( + Set the frame transform as a PoseStamped_ for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html - )pbdoc") - .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc( - 2. Set the frame for which the inverse kinematics are calculated - with respect to each pose generated by the pose_generator. - - Args: - pose (PoseStamped_): Transform from the given link to the IK frame. - link (str): Base link for pose transform to IK frame. - Returns: - None - )pbdoc") - .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc( - 3. Set the frame for which the inverse kinematics are calculated - with respect to each pose generated by the pose_generator. - - Args: - link (str): IK Frame will be placed at the base frame of this link. - Returns: - None - )pbdoc") - .def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"pbdoc( - setMaxIKSolutions(max_ik_solutions) - - Args: - max_ik_solutions (int): Maximum number of ik solutions. - Returns: - None + )", "transform"_a) + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"( + Set the frame transform as a PoseStamped_ in reference to a given link + for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "pose"_a, "link"_a) + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"( + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + The IK Frame will be placed at the base frame of this link given + as an argument. + )", "link"_a) + .def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"( Set the maximum number of inverse kinematics solutions that should be computed. - - )pbdoc") - .def(py::init(), py::arg("pose_generator"), - py::arg("name") = std::string("place generator")); + )", "max_ik_solutions"_a) + .def(py::init(), "pose_generator"_a, + "name"_a = std::string("place generator")); } } // namespace python } // namespace moveit