mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
core python docs
- add python docs to core classes
- rework whats included in the docs and what not
This commit is contained in:
parent
31577c10c0
commit
1b1dadb94a
@ -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"
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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<>());
|
||||
|
||||
@ -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.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user