From 4f536637560aa6e163ca1cd46511cbb333414caa Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Thu, 30 Sep 2021 16:51:53 +0200 Subject: [PATCH] add docstrings and mwe's --- core/python/bindings/src/stages.cpp | 191 +++++++++++++++++++++++----- 1 file changed, 159 insertions(+), 32 deletions(-) diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 9fd5c338..4eb8a57a 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -80,19 +80,38 @@ 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) + 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. + 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. + + :: + + task = core.Task() + + # Specify a grasp object by the known name + graspObject = "sampleObject" + + # allow collisions + allowCollision = stages.ModifyPlanningScene("allow object - hand collision") + allowCollision.allowCollisions( + graspObject, + "right_fingers", + True + ) + task.add(allowCollision) - 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( + attachObject(self, name, link) + Args: name (str): Name of the object link (str): Name of the link, to which @@ -101,6 +120,8 @@ void export_stages(pybind11::module& m) { None )pbdoc") .def("detachObject", &ModifyPlanningScene::detachObject, R"pbdoc( + detachObject(self, name, link) + Detach an object from a robot link. Args: @@ -113,6 +134,8 @@ void export_stages(pybind11::module& m) { 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(self, attach_link, attach) + Attach multiple objects to a robot link. Args: @@ -126,6 +149,8 @@ void export_stages(pybind11::module& m) { const std::string& attach_link) { self.attachObjects(elementOrList(names), attach_link, false); }, py::arg("names"), py::arg("attach_link"), R"pbdoc( + detachObjects(self, attach_link) + Detach multiple objects from a robot link. Args: @@ -138,6 +163,8 @@ void export_stages(pybind11::module& m) { 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(self, first, second, enable_collision) + Allow or disable collisions between links and objects. Args: @@ -388,6 +415,27 @@ void export_stages(pybind11::module& m) { Args: name (str): Name of the stage. planner (PlannerInterface): Planner that is used to compute the path of motion. + + :: + + # Planning group + group = "panda_arm" + + # Cartesian planner + cartesian = core.CartesianPath() + + task = core.Task() + + # start from current robot state + task.add(stages.CurrentState("current state")) + + # move along x + move = stages.MoveRelative("x +0.2", cartesian) + move.group = group + header = Header(frame_id="world") + move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0))) + task.add(move) + )pbdoc") .property("group", R"pbdoc( str: Planning group which should be utilized for planning and execution. @@ -398,10 +446,10 @@ void export_stages(pybind11::module& m) { .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .property("min_distance", R"pbdoc( - + double: Set the minimum distance to move. )pbdoc") .property("max_distance", R"pbdoc( - + double: Set the maximum distance to move. )pbdoc") .property("path_constraints", R"pbdoc( Constraints_ : These are the path constraints. @@ -422,10 +470,14 @@ void export_stages(pybind11::module& m) { )pbdoc") .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"pbdoc( + setDirection(self, direction) + Translate link along given direction. )pbdoc") .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), R"pbdoc( + setDirection(self, joint_deltas) + Move specified joint variables by given amount. )pbdoc"); @@ -546,47 +598,122 @@ void export_stages(pybind11::module& m) { .def(py::init()); properties::class_(m, "Pick", R"pbdoc( - Specialization of PickPlaceBase to realize picking + Pick(self, grasp_generator, name) + + Args: + grasp_generator (): + name (str): Name of the stage. + + 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. + + Picking consist of the following sub stages: + + - Linearly approaching the object along an approach direction/twist "grasp" end effector posture + - Attach the object + - Lift along a given direction/twist + + The end effector postures corresponding to pre-grasp and grasp as well + as the end effector's cartesian pose needs to be provided by an external + grasp stage. + )pbdoc") + .property("object", R"pbdoc( + Name of object to pick. + )pbdoc") + .property("eef", R"pbdoc( + End effector name. + )pbdoc") + .property("eef_frame", R"pbdoc( + Name of the end effector frame. + )pbdoc") + .property("eef_group", R"pbdoc( + Joint model group of the end effector. + )pbdoc") + .property("eef_parent_group", R"pbdoc( + Joint model group of the eef's parent. )pbdoc") - .property("object") - .property("eef") - .property("eef_frame") - .property("eef_group") - .property("eef_parent_group") .def(py::init(), py::arg("grasp_generator"), py::arg("name") = std::string("pick")) - .def("setApproachMotion", &Pick::setApproachMotion) + .def("setApproachMotion", &Pick::setApproachMotion, R"pbdoc( + + )pbdoc") .def("setLiftMotion", - py::overload_cast(&Pick::setLiftMotion)) - .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion)); + py::overload_cast(&Pick::setLiftMotion), R"pbdoc( + + )pbdoc") + .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion), R"pbdoc( + + )pbdoc"); properties::class_(m, "Place", R"pbdoc( - Specialization of PickPlaceBase to realize placing + Place(self, place_generator, name) + + Args: + place_generator (): + name (str): Name of the stage. + + 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. + + Placing consist of the inverse order of stages: + + - Place down along a given direction + - Detach the object + - Linearly retract end effector + + The end effector postures corresponding to pre-grasp and grasp as well + as the end effector's Cartesian pose needs to be provided by an external + grasp stage. + )pbdoc") + .property("object", R"pbdoc( + Name of object to pick. + )pbdoc") + .property("eef", R"pbdoc( + End effector name. + )pbdoc") + .property("eef_frame", R"pbdoc( + Name of the end effector frame. + )pbdoc") + .property("eef_group", R"pbdoc( + Joint model group of the end effector. + )pbdoc") + .property("eef_parent_group", R"pbdoc( + Joint model group of the eef's parent. )pbdoc") - .property("object") - .property("eef") - .property("eef_frame") - .property("eef_group") - .property("eef_parent_group") .def(py::init(), py::arg("place_generator"), py::arg("name") = std::string("place")); properties::class_(m, "SimpleGrasp", R"pbdoc( Specialization of SimpleGraspBase to realize grasping )pbdoc") - .property("eef") - .property("object") + .property("eef", R"pbdoc( + + )pbdoc") + .property("object", R"pbdoc( + + )pbdoc") .def(py::init(), py::arg("pose_generator"), py::arg("name") = std::string("grasp generator")) - .def("setIKFrame", &SimpleGrasp::setIKFrame) - .def("setIKFrame", &SimpleGrasp::setIKFrame) - .def("setIKFrame", &SimpleGrasp::setIKFrame); + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( + + )pbdoc") + .def("setIKFrame", &SimpleGrasp::setIKFrame, + R"pbdoc( + + )pbdoc") + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( + + )pbdoc"); properties::class_(m, "SimpleUnGrasp", R"pbdoc( Specialization of SimpleGraspBase to realize ungrasping )pbdoc") - .property("eef") - .property("object") + .property("eef", R"pbdoc( + + )pbdoc") + .property("object", R"pbdoc( + + )pbdoc") .def(py::init(), py::arg("pose_generator"), py::arg("name") = std::string("place generator")); }