core python docs

- add python docs to core classes
    - rework whats included in the docs and what not
This commit is contained in:
cpetersmeier 2021-12-28 15:32:05 +01:00 committed by Robert Haschke
parent 31577c10c0
commit 1b1dadb94a
5 changed files with 559 additions and 310 deletions

View File

@ -226,5 +226,5 @@ intersphinx_mapping = {"https://docs.python.org/3": None}
# Default options for generating documentation.
autodoc_default_options = {
"exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward, MergeMode"
"exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward, PropagatingEitherWay, MergeMode, SerialContainer, Solutions, PropertyInitializerSource"
}

View File

@ -110,24 +110,69 @@ void export_core(pybind11::module& m) {
});
// clang-format off
py::classh<SolutionBase>(m, "Solution")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost)
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment)
py::classh<SolutionBase>(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 perticular solution.
)pbdoc")
.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")
;
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory")
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory", R"pbdoc(
SubTrajectory()
A SubTrajectory connects interface states of compute stages.
Args:
None
)pbdoc")
.def(py::init<>())
.def_property_readonly("start", &SolutionBase::start)
.def_property_readonly("end", &SolutionBase::end)
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost)
.def("markAsFailure", &SolutionBase::markAsFailure)
.def_property_readonly("isFailure", &SolutionBase::isFailure)
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment)
.def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers))
.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(
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<SolutionBaseConstPtr>;
@ -139,7 +184,7 @@ void export_core(pybind11::module& m) {
;
py::classh<InterfaceState>(m, "InterfaceState", R"pbdoc(
InterfaceState(self, scene)
InterfaceState(scene)
Args:
scene (obj): Desired Planning Scene at the interface.
@ -166,22 +211,69 @@ void export_core(pybind11::module& m) {
return pybind11::cast(static_cast<bool>(err));
});
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage")
.property<double>("timeout")
.property<std::string>("marker_ns")
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties)
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"pbdoc(
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<double>("timeout", R"pbdoc(
float: Timeout of stage per computation.
)pbdoc")
.property<std::string>("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")
// expose name as writeable property
.def_property("name", &Stage::name, &Stage::setName)
.def_property("name", &Stage::name, &Stage::setName, R"pbdoc(
str: Get / set the name of the stage.
)pbdoc")
// read-only access to properties + solutions
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties))
.def_property_readonly("solutions", &Stage::solutions)
.def_property_readonly("failures", &Stage::failures)
.def("reset", &Stage::reset)
.def("init", &Stage::init);
.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()
py::enum_<Stage::PropertyInitializerSource>(stage, "PropertyInitializerSource")
.value("PARENT", Stage::PARENT)
.value("INTERFACE", Stage::INTERFACE)
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>(stage, "PropertyInitializerSource", R"pbdoc(
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")
;
@ -207,7 +299,7 @@ void export_core(pybind11::module& m) {
;
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"pbdoc(
Generator(self, name)
Generator(name)
Args:
name (str): Name of the stage.
@ -245,32 +337,41 @@ void export_core(pybind11::module& m) {
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("generator"))
.def("canCompute", &Generator::canCompute, R"pbdoc(
canCompute(self)
canCompute()
Args:
None
Returns:
bool: Should ``compute()`` be called?
Guard for the ``compute()`` function.
)pbdoc")
.def("compute", &Generator::compute, R"pbdoc(
compute(self)
Implement the stage's logic here. To interface with other stages,
spwan an ``InterfaceState``.
)pbdoc")
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); }, R"pbdoc(
spawn(self, state, cost)
compute()
Args:
state (obj): The Interface State that should be used.
cost (double): The cost associated with that solution.
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")
;
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"pbdoc(
MonitoringGenerator(self, name)
MonitoringGenerator(name)
Args:
name (str): Name of the stage.
@ -330,10 +431,12 @@ void export_core(pybind11::module& m) {
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("generator"))
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, R"pbdoc(
setMonitoredStage(self, stage)
setMonitoredStage(stage)
Args:
stage (obj): Monitor solutions of this stage.
stage (Stage): Monitor solutions of this stage.
Returns:
None
Set the reference to the Monitored Stage.
)pbdoc")
@ -366,7 +469,7 @@ void export_core(pybind11::module& m) {
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"pbdoc(
Alternatives(self, name)
Alternatives(name)
Args:
name (str): Name of the stage.
@ -374,69 +477,14 @@ void export_core(pybind11::module& m) {
Plan for different alternatives in parallel.
Solution of all children are reported - sorted by cost.
::
.. literalinclude:: ./../../../demo/scripts/alternatives.py
:language: python
from moveit.task_constructor import core, stages
import rospy
import time
import numpy as np
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
jointPlanner = core.JointInterpolationPlanner()
task = core.Task()
# start from current robot state
currentState = stages.CurrentState("current state")
task.add(currentState)
alternatives = core.Alternatives('Alternatives')
goalConfig1 = {
'panda_joint1': 1.0,
'panda_joint2': -1.0,
'panda_joint3': 0.0,
'panda_joint4': -2.5,
'panda_joint5': 1.0,
'panda_joint6': 1.0,
'panda_joint7': 1.0,
}
goalConfig2 = {
'panda_joint1': -3.0,
'panda_joint2': -1.0,
'panda_joint3': 0.0,
'panda_joint4': -2.0,
'panda_joint5': 1.0,
'panda_joint6': 2.0,
'panda_joint7': 0.5,
}
# first motion plan to compare
moveTo1 = stages.MoveTo('Move To Goal Configuration 1', jointPlanner)
moveTo1.group = 'panda_arm'
moveTo1.setGoal(goalConfig1)
alternatives.insert(moveTo1)
# second motion plan to compare
moveTo2 = stages.MoveTo('Move To Goal Configuration 2', jointPlanner)
moveTo2.group = 'panda_arm'
moveTo2.setGoal(goalConfig2)
alternatives.insert(moveTo2)
task.add(alternatives)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives"));
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"pbdoc(
Fallbacks(self, name)
Fallbacks(name)
Args:
name (str): Name of the stage.
@ -447,53 +495,14 @@ void export_core(pybind11::module& m) {
an alternative planning strategy.
All solutions of the last active child are reported.
::
.. literalinclude:: ./../../../demo/scripts/fallbacks.py
:language: python
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import rospy
import time
roscpp_init("mtc_tutorial")
# use cartesian and joint interpolation planners
cartesianPlanner = core.CartesianPath()
jointPlanner = core.JointInterpolationPlanner()
# initialize the mtc task
task = core.Task()
# add the current planning scene state to the task hierarchy
currentState = stages.CurrentState("Current State")
task.add(currentState)
# create a fallback container to fall back to a different planner
# if motion generation fails with the primary one
fallbacks = core.Fallbacks('Fallbacks')
# primary motion plan
moveTo1 = stages.MoveTo('Move To Goal Configuration 1', cartesianPlanner)
moveTo1.group = 'panda_arm'
moveTo1.setGoal('extended')
fallbacks.insert(moveTo1)
# fallback motion plan
moveTo2 = stages.MoveTo('Move To Goal Configuration 2', jointPlanner)
moveTo2.group = 'panda_arm'
moveTo2.setGoal('extended')
fallbacks.insert(moveTo2)
# add the fallback container to the task hierarchy
task.add(fallbacks)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fallbacks"));
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"pbdoc(
Merger(self, name)
Merger(name)
Args:
name (str): Name of the stage.
@ -501,56 +510,18 @@ void export_core(pybind11::module& m) {
Plan for different sub tasks in parallel and finally merge
all sub solutions into a single trajectory
::
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import rospy
import time
roscpp_init("mtc_tutorial")
# use the joint interpolation planner
planner = core.JointInterpolationPlanner()
# the task will contain our stages
task = core.Task()
# start from current robot state
currentState = stages.CurrentState("current state")
task.add(currentState)
# the merger plans for two parallel execution paths
merger = core.Merger('Merger')
# first simultaneous execution
moveTo1 = stages.MoveTo('Move To Home', planner)
moveTo1.group = 'hand'
moveTo1.setGoal('close')
merger.insert(moveTo1)
# second simultaneous execution
moveTo2 = stages.MoveTo('Move To Ready', planner)
moveTo2.group = 'panda_arm'
moveTo2.setGoal('extended')
merger.insert(moveTo2)
# add the merger stage to the task hierarchy
task.add(merger)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
.. literalinclude:: ./../../../demo/scripts/merger.py
:language: python
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("merger"));
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"pbdoc(
WrapperBase(self, name, child)
WrapperBase(name, child)
Args:
name (str): Name of the stage.
child (obj): Wrapped stage.
child (Stage): Wrapped stage.
A wrapper wraps a single child stage, which can be
accessed via ``wrapped()``. Implementations of this
@ -562,7 +533,7 @@ void export_core(pybind11::module& m) {
)pbdoc");
py::classh<Task>(m, "Task", R"pbdoc(
Task(self, ns, introspection)
Task(ns, introspection)
Args:
ns (str): Namespace of the task.
@ -572,20 +543,12 @@ void export_core(pybind11::module& m) {
wrapping a single container
(by default a SerialContainer),
which serves as the root of all stages.
See below an exampel which creates a task with a
single stage in its hierarchy, fetching the current state
of the planning scene.
::
# Create a task
task = core.Task()
# add some stages e.g. currentState
currentState = stages.CurrentState("current state")
task.add(currentState)
# compute the stages that the task contains.
if task.plan():
# publish to the ros network
task.publish(task.solutions[0])
.. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python
)pbdoc")
.def(py::init<const std::string&, bool>(), py::arg("ns") = std::string(), py::arg("introspection") = true)
@ -593,22 +556,58 @@ void export_core(pybind11::module& m) {
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), R"pbdoc(
Access the property map of the task.
PropertyMap: 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
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(
Inspect failures that occurred during the planning phase.
Solutions: 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.
str: Set the name property of the task.
)pbdoc")
.def("loadRobotModel", &Task::loadRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, py::arg("enabled") = true)
.def("clear", &Task::clear)
.def("add", &Task::add)
.def("loadRobotModel", &Task::loadRobotModel, R"pbdoc(
loadRobotModel(robot_description)
Args:
robot_description (str): Which robot model to load.
Returns:
None
Load robot model from given parameter.
)pbdoc")
.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("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* {
Stage* child = t.stages()->findChild(name);
@ -620,13 +619,59 @@ 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)
.def("init", py::overload_cast<>(&Task::init))
.def("plan", &Task::plan, py::arg("max_solutions") = 0)
.def("preempt", &Task::preempt)
.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?
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")
.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")
.def("execute", [](const Task& self, const SolutionBasePtr& solution) {
moveit::planning_interface::PlanningSceneInterface psi;
moveit::planning_interface::MoveGroupInterface
@ -647,7 +692,16 @@ 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");
// clang-format on
}
} // namespace python

View File

@ -169,60 +169,224 @@ bool PropertyConverterBase::insert(const std::type_index& type_index, const std:
void export_properties(py::module& m) {
// clang-format off
py::classh<Property>(m, "Property")
.def("setValue", [](Property& self, const py::object& value)
{ self.setValue(PropertyConverterRegistry::fromPython(value)); })
.def("setCurrentValue", [](Property& self, const py::object& value)
{ self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); })
.def("value", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.value()); })
.def("value", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.defaultValue()); })
.def("reset", &Property::reset)
.def("defined", &Property::defined)
;
py::classh<Property>(m, "Property", R"pbdoc(
Property()
py::classh<PropertyMap>(m, "PropertyMap")
Construct a property that can hold any value.
)pbdoc")
.def(py::init<>())
.def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end(); },
"Check whether the map is nonempty")
.def("setValue", [](Property& self, const py::object& value)
{ self.setValue(PropertyConverterRegistry::fromPython(value)); }, R"pbdoc(
setValue(value)
Args:
value: Any value
Returns:
None
Set current value and default value.
)pbdoc")
.def("setCurrentValue", [](Property& self, const py::object& value)
{ self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); }, R"pbdoc(
setCurrentValue(value)
Args:
value: Any value
Returns:
None
Set the current value of the property.
)pbdoc")
.def("value", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.value()); }, R"pbdoc(
value()
1. Get the current value for the property. If not defined, return the
default value instead.
Args:
None
Returns:
Current value of the property.
)pbdoc")
.def("value", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.defaultValue()); }, R"pbdoc(
2. Get the current value for the property. If not defined, return the
default value instead.
Args:
None
Returns:
Current value of the property.
)pbdoc")
.def("reset", &Property::reset, R"pbdoc(
reset()
Args:
None
Returns:
None
Reset the property to the default value.
The default value can be empty.
)pbdoc")
.def("defined", &Property::defined, R"pbdoc(
defined()
Args:
None
Returns:
Returns true if the property is defined,
Is the current value defined or will the fallback be used?
)pbdoc")
.def("description", &Property::description, R"pbdoc(
description()
Args:
None
Returns:
Returns the description text.
Get the description text.
)pbdoc")
.def("setDescription", &Property::setDescription, R"pbdoc(
setDescription(desc)
Args:
desc (str): The desired description of the property
Returns:
None
Set the description of the property.
)pbdoc");
py::classh<PropertyMap>(m, "PropertyMap", R"pbdoc(
PropertyMap()
Conveniency methods are provided to setup property
initialization for several properties at once - always
inheriting from the identically named external property.
)pbdoc")
.def(py::init<>())
.def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();}, R"pbdoc(
Check whether the map is nonempty
)pbdoc")
.def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>()) // Essential: keep list alive while iterator exists
.def("items", [](const PropertyMap& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>())
py::keep_alive<0, 1>(), R"pbdoc(
items()
Iterator over the items in the property map.
)pbdoc")
.def("__contains__", [](const PropertyMap& self, const std::string &key) { return self.hasProperty(key); })
.def("property", [](PropertyMap& self, const std::string& key)
{ return self.property(key); }, py::return_value_policy::reference_internal)
{ return self.property(key); }, py::return_value_policy::reference_internal, R"pbdoc(
property(name)
Args:
name (str): Name of the property
Returns:
Property object that matches the given name.
Get the property with the given name, throws property - undeclared
for unkown name.
)pbdoc")
.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)
{ self.set(key, PropertyConverterRegistry::fromPython(value)); })
.def("reset", &PropertyMap::reset, "reset all properties to their defaults")
.def("reset", &PropertyMap::reset, R"pbdoc(
reset()
Args:
None
Returns:
None
Reset all properties to their default values
)pbdoc")
.def("update", [](PropertyMap& self, const py::dict& values) {
for (auto it = values.begin(), end = values.end(); it != end; ++it) {
self.set(it->first.cast<std::string>(),
PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second)));
}
})
}, R"pbdoc(
update(values)
Args:
values (dict): Name value pairs of properties
that should be updated.
Returns:
None
Update multiple properties at once.
)pbdoc")
.def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) {
std::set<std::string> s;
for (auto& item : names)
s.insert(item.cast<std::string>());
self.configureInitFrom(sources, s);
}, "configure initialization of listed/all properties from given source",
}, R"pbdoc(
configureInitFrom(sources, names)
Args:
sources (SourceFlags): Where should the property
values be obtained from?
names (list): List of str of the property names
that should be configured. Optional, empty by default
(which means all properties are obtained).
Returns:
None
Configure initialization of listed/all properties from given source"
)pbdoc",
py::arg("sources"), py::arg("names") = py::list())
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) {
self.exposeTo(other, name, name);
})
}, R"pbdoc(
exposeTo(other, name)
1. Declare given property that is also present in other PropertyMap.
Args:
other (PropertyMap): PropertyMap as the source for the
new property values.
names (str): Name of the property that should be obtained.
Returns:
None
)pbdoc")
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) {
self.exposeTo(other, name, other_name);
})
}, R"pbdoc(
2. Declare given property name as `other_namer` in other PropertyMap.
Args:
other (PropertyMap): PropertyMap as the source for the
new property values.
names (str): Name of the property that should be obtained.
other_name (str): New name of the property.
Returns:
None
)pbdoc")
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) {
std::set<std::string> s;
for (auto& item : names)
s.insert(item.cast<std::string>());
self.exposeTo(other, s);
})
}, R"pbdoc(
3. Declare all given properties also in other PropertyMap.
Args:
other (PropertyMap): PropertyMap as the source for the
new property values.
names (list): List of str which contains the names of the
properties that should be obtained.
Returns:
None
)pbdoc")
;
// clang-format on
}

View File

@ -58,7 +58,7 @@ void export_solvers(py::module& m) {
py::return_value_policy::reference_internal);
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner", R"pbdoc(
PipelinePlanner(self)
PipelinePlanner()
Use MoveIt's PlanningPipeline to plan a trajectory between to scenes.
@ -93,16 +93,18 @@ void export_solvers(py::module& m) {
uint: Number of planning attempts.
)pbdoc")
.property<moveit_msgs::WorkspaceParameters>("workspace_parameters", R"pbdoc(
moveit_msgs::WorkspaceParameters: Workspace_parameters.
WorkspaceParameters_: Workspace_parameters for the planning algorithm
.. _WorkspaceParameters: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/WorkspaceParameters.html
)pbdoc")
.property<double>("goal_joint_tolerance", R"pbdoc(
double: Tolerance for reaching joint goals.
float: Tolerance for reaching joint goals.
)pbdoc")
.property<double>("goal_position_tolerance", R"pbdoc(
double: Tolerance for reaching position goals.
float: Tolerance for reaching position goals.
)pbdoc")
.property<double>("goal_orientation_tolerance", R"pbdoc(
double: Tolerance for reaching orientation goals.
float: Tolerance for reaching orientation goals.
)pbdoc")
.property<bool>("display_motion_plans", R"pbdoc(
bool: Publish generated solutions via a topic.
@ -113,7 +115,7 @@ void export_solvers(py::module& m) {
.def(py::init<>());
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"pbdoc(
JointInterpolationPlanner(self)
JointInterpolationPlanner()
Interpolate a trajectory between states in joint space.
Fails if direct joint space interpolation fails.
@ -127,13 +129,13 @@ void export_solvers(py::module& m) {
jointPlanner.max_step = 0.1
)pbdoc")
.property<double>("max_step", R"pbdoc(
double: Prevent large movements by specifying the maximum step distance
float: Prevent large movements by specifying the maximum step distance
in joint space.
)pbdoc")
.def(py::init<>());
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"pbdoc(
CartesianPath(self)
CartesianPath()
The planner uses MoveIt's ``computeCartesianPath()`` to
generate a straigh-line path between two scenes.
@ -148,15 +150,15 @@ void export_solvers(py::module& m) {
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
)pbdoc")
.property<double>("step_size", R"pbdoc(
double: Specify the path interpolation resolution by adjusting the
float: Specify the path interpolation resolution by adjusting the
step size between consecutive waypoints with this parameter.
)pbdoc")
.property<double>("jump_threshold", R"pbdoc(
double: Prevent jumps in inverse kinematic solutions by adjusting the
float: Prevent jumps in inverse kinematic solutions by adjusting the
acceptable fraction of mean joint motion per step.
)pbdoc")
.property<double>("min_fraction", R"pbdoc(
double: Lower threshold of minimum motion per step required for
float: Lower threshold of minimum motion per step required for
success in planning.
)pbdoc")
.def(py::init<>());

View File

@ -171,6 +171,8 @@ void export_stages(pybind11::module& m) {
Args:
collision_object (CollisionObject_): Object to be added. Must be
in the appropriate message format.
Returns:
None
.. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html
@ -211,6 +213,8 @@ void export_stages(pybind11::module& m) {
Args:
scene (PlanningScenePtr): The desired planning scene state.
Returns:
None
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fixed state"));
@ -325,57 +329,50 @@ void export_stages(pybind11::module& m) {
.def("setGoal", py::overload_cast<const geometry_msgs::PoseStamped&>(&MoveTo::setGoal), R"pbdoc(
setGoal(goal)
1. Move link to a given pose.
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<const geometry_msgs::PointStamped&>(&MoveTo::setGoal), R"pbdoc(
setGoal(goal)
2. Move link to given point, keeping current orientation.
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<const moveit_msgs::RobotState&>(&MoveTo::setGoal), R"pbdoc(
3. Move joints specified in msg to their target values.
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<const std::map<std::string, double>&>(&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
Move joints by name to their mapped target value.
)pbdoc")
.def("setGoal", py::overload_cast<const std::string&>(&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
Move joint model group to given named pose.
)pbdoc");
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative", R"pbdoc(
@ -391,6 +388,14 @@ void export_stages(pybind11::module& m) {
:language: python
:lines: 26-31
To implement your own propagtor logic on top of the `moveRelative` class' functionality,
you may derive from the stage like so:
.. literalinclude:: ./../../python/test/rostest_trampoline.py
:language: python
:lines: 72-87
)pbdoc")
.property<std::string>("group", R"pbdoc(
str: Planning group which should be utilized for planning and execution.
@ -416,24 +421,35 @@ void export_stages(pybind11::module& m) {
R"pbdoc(
setDirection(twist)
Perform twist motion on specified link.
1. Perform twist motion on specified link.
Args:
twist (Twist_): Use a Twist message as movement direction description.
Returns:
None
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)pbdoc")
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection),
R"pbdoc(
setDirection(direction)
2. Translate link along given direction.
Translate link along given direction.
Args:
direction (Vector3Stamped_): Desired direction.
Returns:
None
.. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html
)pbdoc")
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection),
R"pbdoc(
setDirection(joint_deltas)
3. Move specified joint variables by given amount.
Move specified joint variables by given amount.
Args:
joint_deltas (dict): Desired direction,
given as a (joint_name: str, joint_value: float), mapping.
Returns:
None
)pbdoc");
py::enum_<stages::Connect::MergeMode>(m, "MergeMode", R"pbdoc(
@ -491,7 +507,7 @@ void export_stages(pybind11::module& m) {
)pbdoc")
.property<double>("max_penetration", R"pbdoc(
Cutoff length up to which collision objects get fixed.
float: Cutoff length up to which collision objects get fixed.
)pbdoc")
.def(py::init<const std::string&>(), py::arg("name") = std::string("fix collisions"));
@ -569,11 +585,9 @@ void export_stages(pybind11::module& m) {
.. literalinclude:: ./../../../demo/scripts/generate_pose.py
:language: python
:lines: 35-48
)pbdoc")
.property<geometry_msgs::PoseStamped>("pose", R"pbdoc(
PoseStamped_: Set the pose, which should be spawned
on each new solution of the monitored stage.
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")
@ -635,6 +649,8 @@ void export_stages(pybind11::module& m) {
approach motion.
min_distance (float): Minimum allowed distance.
max_distance (float): Maximum allowed distance.
Returns:
None
The approaching motion towards the grasping state is represented
by a twist message.
@ -645,26 +661,26 @@ void export_stages(pybind11::module& m) {
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&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.
The lifting motion away from the grasping state is represented
by a twist message.
Returns:
None
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)pbdoc")
.def("setLiftMotion", py::overload_cast<const std::map<std::string, double>&>(&Pick::setLiftMotion), R"pbdoc(
setLiftMotion(place)
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.
The lifting motion away from the grasping state is represented
by its destination as joint-value pairs.
Returns:
None
)pbdoc");
properties::class_<Place, Stage>(m, "Place", R"pbdoc(
@ -716,6 +732,8 @@ void export_stages(pybind11::module& m) {
retract motion.
min_distance (float): Minimum allowed distance.
max_distance (float): Maximum allowed distance.
Returns:
None
The retract motion towards the final state is represented
by a twist message.
@ -726,27 +744,27 @@ void export_stages(pybind11::module& m) {
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&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.
The object-placing motion towards the final state is represented
by a twist message.
Returns:
None
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)pbdoc")
.def("setPlaceMotion", py::overload_cast<const std::map<std::string, double>&>(&Place::setPlaceMotion), R"pbdoc(
setPlaceMotion(joints)
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.
The placing motion to the final state is represented
by its destination as joint-value pairs.
Returns:
None
)pbdoc")
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("place_generator"),
py::arg("name") = std::string("place"));
@ -758,6 +776,8 @@ void export_stages(pybind11::module& m) {
pose_generator (GenerateGraspPose): Generator stage to
sample possible grasp poses.
name (str): Name of the stage.
Returns:
None
Specialization of SimpleGraspBase to realize grasping.
Refer to the pick_ stage for a minimum code example:
@ -785,39 +805,43 @@ void export_stages(pybind11::module& m) {
.def<void (SimpleGrasp::*)(const geometry_msgs::PoseStamped&)>("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.
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
Returns:
None
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)pbdoc")
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame,
R"pbdoc(
setIKFrame(pose, link)
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.
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
Returns:
None
)pbdoc")
.def<void (SimpleGrasp::*)(const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc(
setIKFrame(link)
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.
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
Returns:
None
)pbdoc")
.def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"pbdoc(
setMaxIKSolutions(max_ik_solutions)
Args:
max_ik_solutions (int):
max_ik_solutions (int): Maximum number of ik solutions.
Returns:
None
Set the maximum number of inverse kinematics solutions that
should be computed.
@ -832,6 +856,8 @@ void export_stages(pybind11::module& m) {
sample possible Place
poses.
name (str): Name of the stage.
Returns:
None
Specialization of SimpleGraspBase to realize ungrasping
Refer to the place_ stage for a minimum code example.
@ -863,44 +889,47 @@ void export_stages(pybind11::module& m) {
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)pbdoc")
.def<void (SimpleUnGrasp::*)(const geometry_msgs::PoseStamped&)>("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.
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
Returns:
None
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)pbdoc")
.def<void (SimpleUnGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame",
&SimpleUnGrasp::setIKFrame, R"pbdoc(
setIKFrame(pose, link)
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.
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
Returns:
None
)pbdoc")
.def<void (SimpleUnGrasp::*)(const std::string&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc(
setIKFrame(link)
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.
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
Returns:
None
)pbdoc")
.def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"pbdoc(
setMaxIKSolutions(max_ik_solutions)
Args:
max_ik_solutions (int):
max_ik_solutions (int): Maximum number of ik solutions.
Returns:
None
Set the maximum number of inverse kinematics solutions that
should be computed.