From d1c947c9732b40de1696d0a118a2e827db640f77 Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Tue, 31 Aug 2021 13:33:29 +0200 Subject: [PATCH] add python docstrings --- core/doc/conf.py | 14 ++- core/doc/index.rst | 10 +- core/python/bindings/src/core.cpp | 44 +++++-- core/python/bindings/src/module.cpp | 19 ++- core/python/bindings/src/solvers.cpp | 62 +++++++--- core/python/bindings/src/stages.cpp | 176 ++++++++++++++++++++++----- 6 files changed, 257 insertions(+), 68 deletions(-) diff --git a/core/doc/conf.py b/core/doc/conf.py index f581f7b8..3ac17bdb 100644 --- a/core/doc/conf.py +++ b/core/doc/conf.py @@ -34,11 +34,21 @@ extensions = [ autosummary_generate = True autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries -html_show_sourcelink = True # Remove 'view source code' from top of page (for html, not python) +# Remove 'view source code' from top of page (for html, not python) +html_show_sourcelink = True autodoc_inherit_docstrings = True # If no docstring, inherit from base class -set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints +# Enable 'expensive' imports for sphinx_autodoc_typehints +set_type_checking_flag = True add_module_names = False +# Look for the function signature in the first line of the docstring. +autodoc_docstring_signature = True + +# Interpret docstring formatting as to Google code style conventions. +# https://google.github.io/styleguide/pyguide.html#Comments +napoleon_google_docstring = True +napoleon_numpy_docstring = False + # Add any paths that contain templates here, relative to this directory. templates_path = ["_templates"] diff --git a/core/doc/index.rst b/core/doc/index.rst index 0592eb4f..8aa228d4 100644 --- a/core/doc/index.rst +++ b/core/doc/index.rst @@ -1,11 +1,15 @@ +MoveIt Task Constructor Documentation +===================================== + +The MoveIt Task Constructor planning framework +constructs a task hierarchy of interrelated planning stages. + .. toctree:: - :maxdepth: 2 + :maxdepth: 4 .. autosummary:: :toctree: _autosummary :template: custom-module-template.rst - moveit.task_constructor.core - moveit.task_constructor.stages pymoveit_mtc.core pymoveit_mtc.stages diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 15dd876c..f155faa0 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -172,8 +172,8 @@ void export_core(pybind11::module& m) { auto either_way = py::classh>(m, "PropagatingEitherWay") .def(py::init(), py::arg("name") = std::string("PropagatingEitherWay")) .def("restrictDirection", &PropagatingEitherWay::restrictDirection) - .def("computeForward", &PropagatingEitherWay::computeForward) - .def("computeBackward", &PropagatingEitherWay::computeBackward) + .def("computeForward", &PropagatingEitherWay::computeForward, "compute forward") + .def("computeBackward", &PropagatingEitherWay::computeBackward, "compute backward") //.def("sendForward", &PropagatingEitherWay::sendForward) //.def("sendBackward", &PropagatingEitherWay::sendBackward) ; @@ -199,7 +199,7 @@ void export_core(pybind11::module& m) { properties::class_>(m, "MonitoringGenerator") .def(py::init(), py::arg("name") = std::string("generator")) - .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage) + .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the reference to the Monitored Stage.") .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution) ; @@ -226,8 +226,16 @@ void export_core(pybind11::module& m) { py::classh(m, "ParallelContainerBase"); - py::classh(m, "Alternatives") - .def(py::init(), py::arg("name") = std::string("alternatives")); + py::classh(m, "Alternatives", R"pbdoc( + Plan for different alternatives in parallel. + Solution of all children are reported - sorted by cost. + + )pbdoc") + .def(py::init(), py::arg("name") = std::string("alternatives"), R"pbdoc( + + Args: + Name (str): Name of the object + )pbdoc"); py::classh(m, "Fallbacks") .def(py::init(), py::arg("name") = std::string("fallbacks")); @@ -237,15 +245,31 @@ void export_core(pybind11::module& m) { py::classh(m, "WrapperBase"); - py::classh(m, "Task") + py::classh(m, "Task", R"pbdoc( + A Task is the root of a tree of stages. + + Implementation detail: + A tasks wraps a single container + (by default a SerialContainer), + which serves as the root of all stages. + )pbdoc") .def(py::init(), py::arg("ns") = std::string(), py::arg("introspection") = true) .def(py::init(), py::arg("ns") = std::string(), py::arg("introspection") = true, py::arg("container")) // read-only access to properties + solutions - .def_property_readonly("properties", py::overload_cast<>(&Task::properties)) - .def_property_readonly("solutions", &Task::solutions) - .def_property_readonly("failures", &Task::failures) - .def_property("name", &Task::name, &Task::setName) + .def_property_readonly("properties", py::overload_cast<>(&Task::properties), R"pbdoc( + Access the property map of the task. + )pbdoc") + .def_property_readonly("solutions", &Task::solutions, R"pbdoc( + 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( + Inspect failures that occurred during the planning phase. + )pbdoc") + .def_property("name", &Task::name, &Task::setName, R"pbdoc( + Set the name property of the task. + )pbdoc") .def("loadRobotModel", &Task::loadRobotModel) .def("enableIntrospection", &Task::enableIntrospection, py::arg("enabled") = true) .def("clear", &Task::clear) diff --git a/core/python/bindings/src/module.cpp b/core/python/bindings/src/module.cpp index 57eacc75..c3e4eb51 100644 --- a/core/python/bindings/src/module.cpp +++ b/core/python/bindings/src/module.cpp @@ -46,15 +46,24 @@ void export_stages(pybind11::module& m); } // namespace moveit PYBIND11_MODULE(pymoveit_mtc, m) { - m.doc() = "MoveIt Task Constructor"; + // pybind11::options options; + // options.show_user_defined_docstrings(); + auto msub = m.def_submodule("core", "MoveIt Task Contructor Core"); + msub.doc() = R"pbdoc( + Core components such as base types of stage- + and planner classes. + )pbdoc"; - auto msub = m.def_submodule("core"); - msub.doc() = "MTC core components"; moveit::python::export_properties(msub); moveit::python::export_solvers(msub); moveit::python::export_core(msub); - msub = m.def_submodule("stages"); - msub.doc() = "MTC stages"; + msub = m.def_submodule("stages", "MoveIt Task Constructor Stages"); + msub.doc() = R"pbdoc( + Contains all stages that + are available to the user. + The arrangement of stages + define the task to be carried out. + )pbdoc"; moveit::python::export_stages(msub); } diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 26880cae..518e69a6 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -57,25 +57,57 @@ void export_solvers(py::module& m) { .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), py::return_value_policy::reference_internal); - properties::class_(m, "PipelinePlanner") - .property("planner") - .property("num_planning_attempts") - .property("workspace_parameters") - .property("goal_joint_tolerance") - .property("goal_position_tolerance") - .property("goal_orientation_tolerance") - .property("display_motion_plans") - .property("publish_planning_requests") + properties::class_(m, "PipelinePlanner", R"pbdoc( + Use MoveIt's PlanningPipeline to plan a trajectory between to scenes. + )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( + moveit_msgs::WorkspaceParameters: Workspace_parameters. + )pbdoc") + .property("goal_joint_tolerance", R"pbdoc( + double: Tolerance for reaching joint goals. + )pbdoc") + .property("goal_position_tolerance", R"pbdoc( + double: Tolerance for reaching position goals. + )pbdoc") + .property("goal_orientation_tolerance", R"pbdoc( + double: 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") .def(py::init<>()); - properties::class_(m, "JointInterpolationPlanner") - .property("max_step") + properties::class_(m, "JointInterpolationPlanner", R"pbdoc( + Interpolate a trajectory between states in joint space. + Fails if direct joint space interpolation fails. + )pbdoc") + .property("max_step", R"pbdoc( + double: Maximum joint step. + )pbdoc") .def(py::init<>()); - properties::class_(m, "CartesianPath") - .property("step_size") - .property("jump_threshold") - .property("min_fraction") + properties::class_(m, "CartesianPath", R"pbdoc( + Use MoveIt's computeCartesianPath() to + generate a straigh-line path between two scenes. + )pbdoc") + .property("step_size", R"pbdoc( + double: Step size between consecutive waypoints. + )pbdoc") + .property("jump_threshold", R"pbdoc( + double: Acceptable fraction of mean joint motion per step. + )pbdoc") + .property("min_fraction", R"pbdoc( + double: Fraction of motion required for success. + )pbdoc") .def(py::init<>()); } } // namespace python diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 31768eb6..143c8f02 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -78,29 +78,65 @@ std::vector elementOrList(const py::object& arg) { void export_stages(pybind11::module& m) { // clang-format off - properties::class_(m, "ModifyPlanningScene") - .def(py::init(), py::arg("name") = std::string("modify planning scene")) - .def("attachObject", &ModifyPlanningScene::attachObject) - .def("detachObject", &ModifyPlanningScene::detachObject) + properties::class_(m, "ModifyPlanningScene", R"pbdoc( + 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. + )pbdoc") + .def(py::init(), py::arg("name") = std::string("modify planning scene"), R"pbdoc( + Args: + Name (str): Name of the stage. + + Returns: + None + )pbdoc") + .def("attachObject", &ModifyPlanningScene::attachObject, R"pbdoc( + + 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( + Detach an object from a robot link. + )pbdoc") .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) + }, py::arg("names"), py::arg("attach_link"), py::arg("attach") = true, R"pbdoc( + Attach multiple objects to a robot link. + )pbdoc") .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")) + }, py::arg("names"), py::arg("attach_link"), R"pbdoc( + Detach multiple objects from a robot link. + )pbdoc") .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); + }, py::arg("first"), py::arg("second"), py::arg("enable_collision") = true, R"pbdoc( + Allow or disable collisions between links and objects. + )pbdoc"); // clang-format on - properties::class_(m, "CurrentState") - .def(py::init(), py::arg("name") = std::string("current state")); + properties::class_(m, "CurrentState", R"pbdoc( + Fetch the current PlanningScene state via get_planning_scene service. + )pbdoc") + .def(py::init(), py::arg("name") = std::string("current state"), + "class CurrentState(self, name)"); + + properties::class_(m, "FixedState", R"pbdoc( + Spawn a pre - defined PlanningScene state. + )pbdoc") + .def(py::init(), py::arg("name") = std::string("fixed state")); - properties::class_(m, "FixedState") - .def(py::init(), py::arg("name") = std::string("fixed state")) #if 0 .def("setState", [](FixedState& stage, const moveit_msg::PlanningScene& scene_msg) { // TODO: How to initialize the PlanningScene? @@ -109,16 +145,55 @@ void export_stages(pybind11::module& m) { stage.setState(scene); }) #endif - ; + ; - properties::class_(m, "ComputeIK") - .property("eef") - .property("group") - .property("default_pose") - .property("max_ik_solutions") - .property("ignore_collisions") - .property("ik_frame") - .property("target_pose") + properties::class_(m, "ComputeIK", R"pbdoc( + Wrapper for any pose generator stage to compute IK poses + for a cartesian pose. + + The wrapper reads a target_pose from the interface state of + solutions provided by the wrapped stage. + This Cartesian pose (PoseStamped msg) is used as a goal pose + for inverse kinematics. + + Usually, the end effector's parent link or the group's tip link + is used as the IK frame, which should be moved to the goal frame. + However, any other IK frame can be defined (which is linked to the tip of the group). + + Properties of the internally received InterfaceState can be + forwarded to the newly generated, externally exposed InterfaceState. + )pbdoc") + .property("eef", R"pbdoc( + str: Specify which end effector should + be used. + )pbdoc") + .property("group", R"pbdoc( + str: Specify which planning group + should be used. + )pbdoc") + .property("default_pose", R"pbdoc( + str: Default joint pose of the active group + (defines cost of IK). + )pbdoc") + .property("max_ik_solutions", R"pbdoc( + int: Set the maximum number of inverse + kinematic solutions thats should be generated. + )pbdoc") + .property("ignore_collisions", R"pbdoc( + bool: Specify if collisions with other members of + the planning scene are allowed. + )pbdoc") + .property("ik_frame", R"pbdoc( + geometry_msgs/PoseStamped: Specify the frame with respect + to which the inverse kinematics should be calculated. + )pbdoc") + .property("target_pose", R"pbdoc( + geometry_msgs/PoseStamped: Specify the pose on which + the inverse kinematics should be + calculated on. Since this property should almost always be set + in the Interface State which is sent by the child, + if possible, avoid setting it manually. + )pbdoc") // methods of base class py::class_ need to be called last! .def(py::init()); @@ -133,28 +208,56 @@ void export_stages(pybind11::module& m) { .def("setGoal", py::overload_cast&>(&MoveTo::setGoal)) .def("setGoal", py::overload_cast(&MoveTo::setGoal)); - properties::class_>(m, "MoveRelative") + properties::class_>(m, "MoveRelative", R"pbdoc( + Perform a Cartesian motion relative to some link . + )pbdoc") .property("group") .property("ik_frame") .property("min_distance") .property("max_distance") - .property("path_constraints") + .property("path_constraints", R"pbdoc( + These are the path constraints. + )pbdoc") .def(py::init()) - .def("setDirection", py::overload_cast(&MoveRelative::setDirection)) - .def("setDirection", py::overload_cast(&MoveRelative::setDirection)) - .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection)); + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), + R"pbdoc( + Perform twist motion on specified link. + )pbdoc") + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), + R"pbdoc( + Translate link along given direction. + )pbdoc") + .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), + R"pbdoc( + Move specified joint variables by given amount. + )pbdoc"); py::enum_(m, "MergeMode") .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL) .value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS); PropertyConverter(); - properties::class_(m, "Connect") + properties::class_(m, "Connect", R"pbdoc( + Connect arbitrary InterfaceStates by motion planning + + The states may differ in various planning groups. + To connect both states, the planners provided for + individual sub groups are applied in the specified order. + Each planner only plan for joints within the corresponding + planning group. Finally, an attempt is made to merge the + sub trajectories of individual planning results. + If this fails, the sequential planning result is returned. + )pbdoc") .def(py::init(), py::arg("name") = std::string("connect"), py::arg("planners")); - properties::class_(m, "FixCollisionObjects") - .property("max_penetration") + properties::class_(m, "FixCollisionObjects", R"pbdoc( + Test for collisions and find a correction for applicable objects. + Move the objects out of the way along the correction direction. + )pbdoc") + .property("max_penetration", R"pbdoc( + Cutoff length up to which collision objects get fixed. + )pbdoc") .def(py::init(), py::arg("name") = std::string("fix collisions")); properties::class_(m, "GenerateGraspPose") @@ -169,7 +272,9 @@ void export_stages(pybind11::module& m) { .property("pose") .def(py::init()); - properties::class_(m, "Pick") + properties::class_(m, "Pick", R"pbdoc( + Specialization of PickPlaceBase to realize picking + )pbdoc") .property("object") .property("eef") .property("eef_frame") @@ -177,13 +282,14 @@ void export_stages(pybind11::module& m) { .property("eef_parent_group") .def(py::init(), py::arg("grasp_generator"), py::arg("name") = std::string("pick")) - .def("setApproachMotion", &Pick::setApproachMotion) .def("setLiftMotion", py::overload_cast(&Pick::setLiftMotion)) .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion)); - properties::class_(m, "Place") + properties::class_(m, "Place", R"pbdoc( + Specialization of PickPlaceBase to realize placing + )pbdoc") .property("object") .property("eef") .property("eef_frame") @@ -192,7 +298,9 @@ void export_stages(pybind11::module& m) { .def(py::init(), py::arg("place_generator"), py::arg("name") = std::string("place")); - properties::class_(m, "SimpleGrasp") + properties::class_(m, "SimpleGrasp", R"pbdoc( + Specialization of SimpleGraspBase to realize grasping + )pbdoc") .property("eef") .property("object") .def(py::init(), py::arg("pose_generator"), @@ -201,7 +309,9 @@ void export_stages(pybind11::module& m) { .def("setIKFrame", &SimpleGrasp::setIKFrame) .def("setIKFrame", &SimpleGrasp::setIKFrame); - properties::class_(m, "SimpleUnGrasp") + properties::class_(m, "SimpleUnGrasp", R"pbdoc( + Specialization of SimpleGraspBase to realize ungrasping + )pbdoc") .property("eef") .property("object") .def(py::init(), py::arg("pose_generator"),