add python docstrings

This commit is contained in:
cpetersmeier 2021-08-31 13:33:29 +02:00 committed by Robert Haschke
parent 136c5c425e
commit d1c947c973
6 changed files with 257 additions and 68 deletions

View File

@ -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"]

View File

@ -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

View File

@ -172,8 +172,8 @@ void export_core(pybind11::module& m) {
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay")
.def(py::init<const std::string&>(), 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_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator")
.def(py::init<const std::string&>(), 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<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives")
.def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives"));
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"pbdoc(
Plan for different alternatives in parallel.
Solution of all children are reported - sorted by cost.
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives"), R"pbdoc(
Args:
Name (str): Name of the object
)pbdoc");
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fallbacks"));
@ -237,15 +245,31 @@ void export_core(pybind11::module& m) {
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase");
py::classh<Task>(m, "Task")
py::classh<Task>(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<const std::string&, bool>(), py::arg("ns") = std::string(), py::arg("introspection") = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(),
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)

View File

@ -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);
}

View File

@ -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_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner")
.property<std::string>("planner")
.property<uint>("num_planning_attempts")
.property<moveit_msgs::WorkspaceParameters>("workspace_parameters")
.property<double>("goal_joint_tolerance")
.property<double>("goal_position_tolerance")
.property<double>("goal_orientation_tolerance")
.property<bool>("display_motion_plans")
.property<bool>("publish_planning_requests")
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner", R"pbdoc(
Use MoveIt's PlanningPipeline to plan a trajectory between to scenes.
)pbdoc")
.property<std::string>("planner", R"pbdoc(
str: Planner ID.
)pbdoc")
.property<uint>("num_planning_attempts", R"pbdoc(
uint: Number of planning attempts.
)pbdoc")
.property<moveit_msgs::WorkspaceParameters>("workspace_parameters", R"pbdoc(
moveit_msgs::WorkspaceParameters: Workspace_parameters.
)pbdoc")
.property<double>("goal_joint_tolerance", R"pbdoc(
double: Tolerance for reaching joint goals.
)pbdoc")
.property<double>("goal_position_tolerance", R"pbdoc(
double: Tolerance for reaching position goals.
)pbdoc")
.property<double>("goal_orientation_tolerance", R"pbdoc(
double: Tolerance for reaching orientation goals.
)pbdoc")
.property<bool>("display_motion_plans", R"pbdoc(
bool: Publish generated solutions via a topic.
)pbdoc")
.property<bool>("publish_planning_requests", R"pbdoc(
bool: Publish motion planning requests via a topic.
)pbdoc")
.def(py::init<>());
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner")
.property<double>("max_step")
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"pbdoc(
Interpolate a trajectory between states in joint space.
Fails if direct joint space interpolation fails.
)pbdoc")
.property<double>("max_step", R"pbdoc(
double: Maximum joint step.
)pbdoc")
.def(py::init<>());
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath")
.property<double>("step_size")
.property<double>("jump_threshold")
.property<double>("min_fraction")
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"pbdoc(
Use MoveIt's computeCartesianPath() to
generate a straigh-line path between two scenes.
)pbdoc")
.property<double>("step_size", R"pbdoc(
double: Step size between consecutive waypoints.
)pbdoc")
.property<double>("jump_threshold", R"pbdoc(
double: Acceptable fraction of mean joint motion per step.
)pbdoc")
.property<double>("min_fraction", R"pbdoc(
double: Fraction of motion required for success.
)pbdoc")
.def(py::init<>());
}
} // namespace python

View File

@ -78,29 +78,65 @@ std::vector<T> elementOrList(const py::object& arg) {
void export_stages(pybind11::module& m) {
// clang-format off
properties::class_<ModifyPlanningScene, Stage>(m, "ModifyPlanningScene")
.def(py::init<const std::string&>(), py::arg("name") = std::string("modify planning scene"))
.def("attachObject", &ModifyPlanningScene::attachObject)
.def("detachObject", &ModifyPlanningScene::detachObject)
properties::class_<ModifyPlanningScene, Stage>(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<const std::string&>(), 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<std::string>(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<std::string>(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<std::string>(first), elementOrList<std::string>(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_<CurrentState, Stage>(m, "CurrentState")
.def(py::init<const std::string&>(), py::arg("name") = std::string("current state"));
properties::class_<CurrentState, Stage>(m, "CurrentState", R"pbdoc(
Fetch the current PlanningScene state via get_planning_scene service.
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("current state"),
"class CurrentState(self, name)");
properties::class_<FixedState, Stage>(m, "FixedState", R"pbdoc(
Spawn a pre - defined PlanningScene state.
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fixed state"));
properties::class_<FixedState, Stage>(m, "FixedState")
.def(py::init<const std::string&>(), 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?
@ -111,14 +147,53 @@ void export_stages(pybind11::module& m) {
#endif
;
properties::class_<ComputeIK, Stage>(m, "ComputeIK")
.property<std::string>("eef")
.property<std::string>("group")
.property<std::string>("default_pose")
.property<uint32_t>("max_ik_solutions")
.property<bool>("ignore_collisions")
.property<geometry_msgs::PoseStamped>("ik_frame")
.property<geometry_msgs::PoseStamped>("target_pose")
properties::class_<ComputeIK, Stage>(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<std::string>("eef", R"pbdoc(
str: Specify which end effector should
be used.
)pbdoc")
.property<std::string>("group", R"pbdoc(
str: Specify which planning group
should be used.
)pbdoc")
.property<std::string>("default_pose", R"pbdoc(
str: Default joint pose of the active group
(defines cost of IK).
)pbdoc")
.property<uint32_t>("max_ik_solutions", R"pbdoc(
int: Set the maximum number of inverse
kinematic solutions thats should be generated.
)pbdoc")
.property<bool>("ignore_collisions", R"pbdoc(
bool: Specify if collisions with other members of
the planning scene are allowed.
)pbdoc")
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
geometry_msgs/PoseStamped: Specify the frame with respect
to which the inverse kinematics should be calculated.
)pbdoc")
.property<geometry_msgs::PoseStamped>("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<const std::string&, Stage::pointer&&>());
@ -133,28 +208,56 @@ void export_stages(pybind11::module& m) {
.def("setGoal", py::overload_cast<const std::map<std::string, double>&>(&MoveTo::setGoal))
.def("setGoal", py::overload_cast<const std::string&>(&MoveTo::setGoal));
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative")
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative", R"pbdoc(
Perform a Cartesian motion relative to some link .
)pbdoc")
.property<std::string>("group")
.property<geometry_msgs::PoseStamped>("ik_frame")
.property<double>("min_distance")
.property<double>("max_distance")
.property<moveit_msgs::Constraints>("path_constraints")
.property<moveit_msgs::Constraints>("path_constraints", R"pbdoc(
These are the path constraints.
)pbdoc")
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>())
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection))
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection))
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection));
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection),
R"pbdoc(
Perform twist motion on specified link.
)pbdoc")
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection),
R"pbdoc(
Translate link along given direction.
)pbdoc")
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection),
R"pbdoc(
Move specified joint variables by given amount.
)pbdoc");
py::enum_<stages::Connect::MergeMode>(m, "MergeMode")
.value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL)
.value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS);
PropertyConverter<stages::Connect::MergeMode>();
properties::class_<Connect, Stage>(m, "Connect")
properties::class_<Connect, Stage>(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<const std::string&, const Connect::GroupPlannerVector&>(),
py::arg("name") = std::string("connect"), py::arg("planners"));
properties::class_<FixCollisionObjects, Stage>(m, "FixCollisionObjects")
.property<double>("max_penetration")
properties::class_<FixCollisionObjects, Stage>(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<double>("max_penetration", R"pbdoc(
Cutoff length up to which collision objects get fixed.
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fix collisions"));
properties::class_<GenerateGraspPose, MonitoringGenerator>(m, "GenerateGraspPose")
@ -169,7 +272,9 @@ void export_stages(pybind11::module& m) {
.property<geometry_msgs::PoseStamped>("pose")
.def(py::init<const std::string&>());
properties::class_<Pick, Stage>(m, "Pick")
properties::class_<Pick, Stage>(m, "Pick", R"pbdoc(
Specialization of PickPlaceBase to realize picking
)pbdoc")
.property<std::string>("object")
.property<std::string>("eef")
.property<std::string>("eef_frame")
@ -177,13 +282,14 @@ void export_stages(pybind11::module& m) {
.property<std::string>("eef_parent_group")
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("grasp_generator"),
py::arg("name") = std::string("pick"))
.def("setApproachMotion", &Pick::setApproachMotion)
.def("setLiftMotion",
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion))
.def("setLiftMotion", py::overload_cast<const std::map<std::string, double>&>(&Pick::setLiftMotion));
properties::class_<Place, Stage>(m, "Place")
properties::class_<Place, Stage>(m, "Place", R"pbdoc(
Specialization of PickPlaceBase to realize placing
)pbdoc")
.property<std::string>("object")
.property<std::string>("eef")
.property<std::string>("eef_frame")
@ -192,7 +298,9 @@ void export_stages(pybind11::module& m) {
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("place_generator"),
py::arg("name") = std::string("place"));
properties::class_<SimpleGrasp, Stage>(m, "SimpleGrasp")
properties::class_<SimpleGrasp, Stage>(m, "SimpleGrasp", R"pbdoc(
Specialization of SimpleGraspBase to realize grasping
)pbdoc")
.property<std::string>("eef")
.property<std::string>("object")
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("pose_generator"),
@ -201,7 +309,9 @@ void export_stages(pybind11::module& m) {
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame)
.def<void (SimpleGrasp::*)(const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame);
properties::class_<SimpleUnGrasp, Stage>(m, "SimpleUnGrasp")
properties::class_<SimpleUnGrasp, Stage>(m, "SimpleUnGrasp", R"pbdoc(
Specialization of SimpleGraspBase to realize ungrasping
)pbdoc")
.property<std::string>("eef")
.property<std::string>("object")
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("pose_generator"),