From 5207a8b2b5bf1f0b04f0564ef06c8bee08b8a29c Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Tue, 28 Sep 2021 23:19:15 +0200 Subject: [PATCH] comply to google format + add docstrings --- core/doc/conf.py | 7 +- core/python/bindings/src/module.cpp | 6 +- core/python/bindings/src/stages.cpp | 302 +++++++++++++++++++++++----- 3 files changed, 264 insertions(+), 51 deletions(-) diff --git a/core/doc/conf.py b/core/doc/conf.py index c65acd4e..3bf4c01f 100644 --- a/core/doc/conf.py +++ b/core/doc/conf.py @@ -46,10 +46,9 @@ 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 +# Interpret docstring formatting as to Numpy code style conventions. +# napoleon_google_docstring = True +# napoleon_numpy_docstring = True # Add any paths that contain templates here, relative to this directory. diff --git a/core/python/bindings/src/module.cpp b/core/python/bindings/src/module.cpp index e300e4e7..7ecab9a7 100644 --- a/core/python/bindings/src/module.cpp +++ b/core/python/bindings/src/module.cpp @@ -46,8 +46,10 @@ void export_stages(pybind11::module& m); } // namespace moveit PYBIND11_MODULE(pymoveit_mtc, m) { - // pybind11::options options; - // options.show_user_defined_docstrings(); + // disable function signatures in generated documentation + pybind11::options options; + options.disable_function_signatures(); + auto msub = m.def_submodule("core", "MoveIt Task Contructor Core"); msub.doc() = R"pbdoc( This python package contains diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index d6e809cc..9fd5c338 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -80,50 +80,73 @@ std::vector elementOrList(const py::object& arg) { void export_stages(pybind11::module& m) { // clang-format off properties::class_(m, "ModifyPlanningScene", R"pbdoc( + ModifyPlanningScene(self, name) + 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 + Args: + name (str): Name of the stage. )pbdoc") + .def(py::init(), py::arg("name") = std::string("modify planning scene")) .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. - + 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. + + Args: + name (str): Object name that should be detached. + link (str): Link name from which the object should be detached. + Returns: + None )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, R"pbdoc( 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") .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( 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") .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( 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"); // clang-format on @@ -132,11 +155,16 @@ void export_stages(pybind11::module& m) { Fetch the current PlanningScene state via get_planning_scene service. + Args: + name (str): Name of the stage. + :: + task = core.Task() + # create a stage instance currentState = CurrentState('current state') - + task.add(currentState) )pbdoc") .def(py::init(), py::arg("name") = std::string("current state")); @@ -145,15 +173,25 @@ void export_stages(pybind11::module& m) { Spawn a pre-defined PlanningScene state. + Args: + name (str): Name of the stage. + :: + task = core.Task() + # create a stage instance fixedState = FixedState('fixed state') - + task.add(fixedState) )pbdoc") .def("setState", &FixedState::setState, R"pbdoc( + setState(self, scene) + Use a planning scene pointer to specify which state the Fixed State stage should have. + + Args: + scene (PlanningScenePtr): The desired planning scene state. )pbdoc") .def(py::init(), py::arg("name") = std::string("fixed state")); @@ -185,6 +223,23 @@ 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. + + :: + + # create a task + task = core.Task() + + # get the current robot state + currentState = stages.CurrentState("current state") + + # calculate the inverse kinematics for the current robot state + computeIK = stages.ComputeIK("compute IK", currentState) + task.add(computeIK) + )pbdoc") .property("eef", R"pbdoc( str: Specify which end effector of the active planning group @@ -224,45 +279,166 @@ void export_stages(pybind11::module& m) { // methods of base class py::class_ need to be called last! .def(py::init()); - properties::class_>(m, "MoveTo") - .property("group") - .property("ik_frame") - .property("path_constraints") + properties::class_>(m, "MoveTo", R"pbdoc( + MoveTo(self, name, planner) + + 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. + + :: + + # create a planner instance + jointspace = core.JointInterpolationPlanner() + + # specify planning group + group = "foo_robot" + + # create a task + task = core.Task() + + # get the current robot state + currentState = stages.CurrentState("current state") + task.add(currentState) + + # moveTo named posture, using joint-space interplation + move = stages.MoveTo("moveTo ready", jointspace) + move.group = group + move.setGoal("ready") + task.add(move) + )pbdoc") + .property("group", R"pbdoc( + 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. + + .. _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. + + .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html + )pbdoc") .def(py::init()) - .def("setGoal", py::overload_cast(&MoveTo::setGoal)) - .def("setGoal", py::overload_cast(&MoveTo::setGoal)) - .def("setGoal", py::overload_cast(&MoveTo::setGoal)) - .def("setGoal", py::overload_cast&>(&MoveTo::setGoal)) - .def("setGoal", py::overload_cast(&MoveTo::setGoal)); + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( + setGoal(self, goal) + + Args: + goal (PoseStamped_): Desired configuration. + + Returns: + None + + Move link to a given pose. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )pbdoc") + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( + Args: + goal (PointStamped_): Desired configuration. + + Returns: + None + + Move link to given point, 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( + Args: + goal (RobotState_): Desired configuration. + + Returns: + None + + Move joints specified in msg 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( + Args: + goal (dict): Desired configuration given in joint - value mappings. + + Returns: + None + + Move joints by name to their mapped target value. + )pbdoc") + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( + Args: + goal (str): Desired configuration as a name of a known pose. + + Returns: + None + + Move joint model group to given named pose. + )pbdoc"); properties::class_>(m, "MoveRelative", R"pbdoc( + MoveRelative(self, name, planner) + 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. + )pbdoc") + .property("group", R"pbdoc( + 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. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )pbdoc") + .property("min_distance", R"pbdoc( + + )pbdoc") + .property("max_distance", R"pbdoc( + )pbdoc") - .property("group") - .property("ik_frame") - .property("min_distance") - .property("max_distance") .property("path_constraints", R"pbdoc( - These are the path constraints. + 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( - Perform twist motion on specified link. + setDirection(self, twist) + + Perform twist motion on specified link. + + Args: + twist (Twist_): Use a Twist message as movement direction description. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html )pbdoc") .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"pbdoc( - Translate link along given direction. + Translate link along given direction. )pbdoc") .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), R"pbdoc( - Move specified joint variables by given amount. + Move specified joint variables by given amount. )pbdoc"); py::enum_(m, "MergeMode", R"pbdoc( + Define the merge strategy to use when performing planning operations + with e.g. the connect stage. )pbdoc") - .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL) - .value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS); + .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"); PropertyConverter(); properties::class_(m, "Connect", R"pbdoc( @@ -270,7 +446,19 @@ void export_stages(pybind11::module& m) { Connect arbitrary InterfaceStates by motion planning. You can specify the planning groups and the planners you - want to utilize: + want to utilize. + + 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. + + Args: + Name (str): Name of the stage. + Planners (list): List of the planner - group associations. :: @@ -283,22 +471,27 @@ void export_stages(pybind11::module& m) { ] # create a stage instance connect = Connect('connect', planners) - - 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", R"pbdoc( + FixCollisionObjects(self, name) + 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. + + :: + + task = core.Task() + + # check for collisions and find corrections + fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") + task.add(fixCollisionObjects) + )pbdoc") .property("max_penetration", R"pbdoc( Cutoff length up to which collision objects get fixed. @@ -311,6 +504,10 @@ void export_stages(pybind11::module& m) { 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. + )pbdoc") .property("object", R"pbdoc( str: Name of the Object in the planning scene, which should be grasped. @@ -329,8 +526,23 @@ void export_stages(pybind11::module& m) { )pbdoc") .def(py::init(), py::arg("name") = std::string("Generate Grasp Pose")); - properties::class_(m, "GeneratePose") - .property("pose") + properties::class_(m, "GeneratePose", R"pbdoc( + GeneratePose(self, name) + + 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. + + :: + + task = core.Task() + + )pbdoc") + .property("pose", R"pbdoc( + + )pbdoc") .def(py::init()); properties::class_(m, "Pick", R"pbdoc(