mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Rework python documentation
This commit is contained in:
parent
95f24747b0
commit
9103af2704
@ -42,6 +42,7 @@
|
|||||||
#include <moveit/move_group_interface/move_group_interface.h>
|
#include <moveit/move_group_interface/move_group_interface.h>
|
||||||
|
|
||||||
namespace py = pybind11;
|
namespace py = pybind11;
|
||||||
|
using namespace py::literals;
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
@ -110,178 +111,90 @@ void export_core(pybind11::module& m) {
|
|||||||
});
|
});
|
||||||
|
|
||||||
// clang-format off
|
// clang-format off
|
||||||
py::classh<SolutionBase>(m, "Solution", R"pbdoc(
|
py::classh<SolutionBase>(m, "Solution", "Solution class encapsulates a particular solution")
|
||||||
Solution class encapsulates a particular solution.
|
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost,"float: Cost that is associated with a particular solution")
|
||||||
)pbdoc")
|
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment that is associated with a particular solution")
|
||||||
.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 particular solution.
|
|
||||||
)pbdoc")
|
|
||||||
.def("toMsg", [](const SolutionBasePtr& s) {
|
.def("toMsg", [](const SolutionBasePtr& s) {
|
||||||
moveit_task_constructor_msgs::Solution msg;
|
moveit_task_constructor_msgs::Solution msg;
|
||||||
s->fillMessage(msg);
|
s->fillMessage(msg);
|
||||||
return msg;
|
return msg;
|
||||||
}, R"pbdoc(
|
}, "Convert a solution object into a ros message type")
|
||||||
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", R"pbdoc(
|
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory", "A SubTrajectory connects interface states of compute stages")
|
||||||
SubTrajectory()
|
|
||||||
|
|
||||||
A SubTrajectory connects interface states of compute stages.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
None
|
|
||||||
)pbdoc")
|
|
||||||
.def(py::init<>())
|
.def(py::init<>())
|
||||||
.def_property_readonly("start", &SolutionBase::start, R"pbdoc(
|
.def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory. Readonly property")
|
||||||
InterfaceState: Start of the trajectory. Readonly property.
|
.def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory. Readonly property")
|
||||||
)pbdoc")
|
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost of the solution")
|
||||||
.def_property_readonly("end", &SolutionBase::end, R"pbdoc(
|
.def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "msg"_a)
|
||||||
InterfaceState: End of the trajectory. Readonly property.
|
.def_property_readonly("isFailure", &SolutionBase::isFailure, "bool: True if the trajectory is marked as a failure. Readonly property")
|
||||||
)pbdoc")
|
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, "str: Comment, which can be assigned to the trajectory")
|
||||||
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, R"pbdoc(
|
.def_property_readonly("markers", py::overload_cast<>(&SolutionBase::markers), R"(
|
||||||
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_: Markers that visualize the trajectory. Readonly property.
|
||||||
|
|
||||||
.. _Marker: https://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html
|
.. _Marker: https://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html
|
||||||
)pbdoc")
|
)")
|
||||||
;
|
;
|
||||||
|
|
||||||
using Solutions = ordered<SolutionBaseConstPtr>;
|
using Solutions = ordered<SolutionBaseConstPtr>;
|
||||||
py::classh<Solutions>(m, "Solutions")
|
py::classh<Solutions>(m, "Solutions", "Encapsulates multiple solutions")
|
||||||
.def("__len__", &Solutions::size)
|
.def("__len__", &Solutions::size)
|
||||||
.def("__getitem__", &get_item<Solutions>)
|
.def("__getitem__", &get_item<Solutions>)
|
||||||
.def("__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
|
.def("__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
|
||||||
py::keep_alive<0, 1>())
|
py::keep_alive<0, 1>())
|
||||||
;
|
;
|
||||||
|
|
||||||
py::classh<InterfaceState>(m, "InterfaceState", R"pbdoc(
|
py::classh<InterfaceState>(m, "InterfaceState", R"(
|
||||||
InterfaceState(scene)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
scene (obj): Desired Planning Scene at the interface.
|
|
||||||
|
|
||||||
InterfaceState describes a potential start or goal state
|
InterfaceState describes a potential start or goal state
|
||||||
for a planning stage. A start or goal state for planning
|
for a planning stage. A start or goal state for planning
|
||||||
is essentially defined by the state of a planning scene.
|
is essentially defined by the state of a planning scene.)")
|
||||||
|
.def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a)
|
||||||
)pbdoc")
|
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties),
|
||||||
.def(py::init<const planning_scene::PlanningScenePtr&>(), py::arg("scene"))
|
"PropertyMap: Get access to the PropertyMap of the stage. Notice that this is a read-only property")
|
||||||
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), R"pbdoc(
|
.def_property_readonly("scene", &InterfaceState::scene,
|
||||||
PropertyMap: Get access to the PropertyMap of the stage.
|
"PlanningScene: Get access to the planning scene of the interface stage. Notice that this is a read-only property")
|
||||||
Notice that this is a read-only property.
|
|
||||||
)pbdoc")
|
|
||||||
.def_property_readonly("scene", &InterfaceState::scene, R"pbdoc(
|
|
||||||
PlanningScene: Get access to the planning scene of the interface stage.
|
|
||||||
Notice that this is a read-only property.
|
|
||||||
)pbdoc")
|
|
||||||
;
|
;
|
||||||
|
|
||||||
py::classh<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode")
|
py::classh<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode", "Encapsulates moveit error code message")
|
||||||
.def_readonly("val", &moveit::core::MoveItErrorCode::val)
|
.def_readonly("val", &moveit::core::MoveItErrorCode::val, "int: Value of the error code")
|
||||||
.def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) {
|
.def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) {
|
||||||
return pybind11::cast(static_cast<bool>(err));
|
return pybind11::cast(static_cast<bool>(err));
|
||||||
});
|
});
|
||||||
|
|
||||||
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"pbdoc(
|
auto stage = properties::class_<Stage, PyStage<>>(m, "Stage", R"(
|
||||||
Stage base type. All other stages derive from this type.
|
Stage base type. All other stages derive from this type.
|
||||||
Object is not instantiable and should not be added to the task hierarchy
|
Object is not instantiable and should not be added to the task hierarchy
|
||||||
in a standalone fashion. Rather, derive from generator or propagator stages
|
in a standalone fashion. Rather, derive from generator or propagator stages
|
||||||
to implement custom logic.
|
to implement custom logic.
|
||||||
)pbdoc")
|
)")
|
||||||
.property<double>("timeout", R"pbdoc(
|
.property<double>("timeout","float: Timeout of stage per computation")
|
||||||
float: Timeout of stage per computation.
|
.property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage")
|
||||||
)pbdoc")
|
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, "list: Get / set a list of forwarded properties")
|
||||||
.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
|
// expose name as writeable property
|
||||||
.def_property("name", &Stage::name, &Stage::setName, R"pbdoc(
|
.def_property("name", &Stage::name, &Stage::setName, "str: Get / set the name of the stage")
|
||||||
str: Get / set the name of the stage.
|
|
||||||
)pbdoc")
|
|
||||||
// read-only access to properties + solutions
|
// read-only access to properties + solutions
|
||||||
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties), R"pbdoc(
|
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties), "PropertyMap: Return the property map of the stage. Readonly property")
|
||||||
PropertyMap: Return the property map of the stage. Readonly property.
|
.def_property_readonly("solutions", &Stage::solutions, "Solutions: Get the solutions of a stage")
|
||||||
)pbdoc")
|
.def_property_readonly("failures", &Stage::failures, "Solutions: Get the failed compuations of a stage")
|
||||||
.def_property_readonly("solutions", &Stage::solutions, R"pbdoc(
|
.def("reset", &Stage::reset, "Reset the stage, clearing all solutions, interfaces and inherited properties")
|
||||||
Solutions: Get the solutions of a stage.
|
.def("init", &Stage::init, R"(
|
||||||
)pbdoc")
|
Initialize the stage once before planning. When called, properties configured for
|
||||||
.def_property_readonly("failures", &Stage::failures, R"pbdoc(
|
initialization from parent are already defined. Push interfaces are not yet defined!
|
||||||
Solutions: Get the failed compuations of a stage.
|
)", "robot_model"_a);
|
||||||
)pbdoc")
|
|
||||||
.def("reset", &Stage::reset, R"pbdoc(
|
|
||||||
reset()
|
|
||||||
|
|
||||||
Args:
|
py::enum_<Stage::PropertyInitializerSource>(stage, "PropertyInitializerSource", R"(
|
||||||
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()`
|
Define, from where properties should be initialized when using the `configureInitFrom()`
|
||||||
functions from the PropertyMap class.
|
functions from the PropertyMap class.
|
||||||
)pbdoc")
|
)")
|
||||||
.value("PARENT", Stage::PARENT, R"pbdoc(
|
.value("PARENT", Stage::PARENT, "Inherit properties from parent stage in task hierarchy")
|
||||||
Inherit properties from parent stage in task hierarchy.
|
.value("INTERFACE", Stage::INTERFACE, "Inherit properties from interface, i.e. preceeding stage in the task hierarchy")
|
||||||
)pbdoc")
|
|
||||||
.value("INTERFACE", Stage::INTERFACE, R"pbdoc(
|
|
||||||
Inherit properties from interface, i.e. preceeding stage
|
|
||||||
in the task hierarchy.
|
|
||||||
)pbdoc")
|
|
||||||
;
|
;
|
||||||
|
|
||||||
|
|
||||||
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay")
|
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(m, "PropagatingEitherWay", "Base class for solution forwarding in both directions")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingEitherWay"))
|
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay"))
|
||||||
.def("restrictDirection", &PropagatingEitherWay::restrictDirection)
|
.def("restrictDirection", &PropagatingEitherWay::restrictDirection, "Explicitly specify computation direction")
|
||||||
.def("computeForward", &PropagatingEitherWay::computeForward, "compute forward")
|
.def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward")
|
||||||
.def("computeBackward", &PropagatingEitherWay::computeBackward, "compute backward")
|
.def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
|
||||||
//.def("sendForward", &PropagatingEitherWay::sendForward)
|
//.def("sendForward", &PropagatingEitherWay::sendForward)
|
||||||
//.def("sendBackward", &PropagatingEitherWay::sendBackward)
|
//.def("sendBackward", &PropagatingEitherWay::sendBackward)
|
||||||
;
|
;
|
||||||
@ -291,19 +204,14 @@ void export_core(pybind11::module& m) {
|
|||||||
.value("FORWARD", PropagatingEitherWay::FORWARD)
|
.value("FORWARD", PropagatingEitherWay::FORWARD)
|
||||||
.value("BACKWARD", PropagatingEitherWay::BACKWARD);
|
.value("BACKWARD", PropagatingEitherWay::BACKWARD);
|
||||||
|
|
||||||
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward")
|
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(m, "PropagatingForward", "Base class for forward solution propagation")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingForward"))
|
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"))
|
||||||
;
|
;
|
||||||
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(m, "PropagatingBackward")
|
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(m, "PropagatingBackward", "Base class for backward solution propagation")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("PropagatingBackward"))
|
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"))
|
||||||
;
|
;
|
||||||
|
|
||||||
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"pbdoc(
|
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"(
|
||||||
Generator(name)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
name (str): Name of the stage.
|
|
||||||
|
|
||||||
Derive from this stage to implement a custom stage
|
Derive from this stage to implement a custom stage
|
||||||
that spawns a solution. When traversing through the whole
|
that spawns a solution. When traversing through the whole
|
||||||
task hierarchy, the ``compute()`` function of this generator stage
|
task hierarchy, the ``compute()`` function of this generator stage
|
||||||
@ -334,48 +242,16 @@ void export_core(pybind11::module& m) {
|
|||||||
self.num = self.num - 1
|
self.num = self.num - 1
|
||||||
self.spawn(core.InterfaceState(self.ps), self.num)
|
self.spawn(core.InterfaceState(self.ps), self.num)
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("generator"))
|
.def(py::init<const std::string&>(), "name"_a = std::string("generator"))
|
||||||
.def("canCompute", &Generator::canCompute, R"pbdoc(
|
.def("canCompute", &Generator::canCompute, "Guard for the ``compute()`` function")
|
||||||
canCompute()
|
.def("compute", &Generator::compute, "Implement the stage's logic here. To interface with other stages, spawn an ``InterfaceState``")
|
||||||
|
.def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); },
|
||||||
Args:
|
"Spawn an interface state that gets forwarded to the next stage",
|
||||||
None
|
"state"_a, "cost"_a)
|
||||||
Returns:
|
|
||||||
bool: Should ``compute()`` be called?
|
|
||||||
|
|
||||||
Guard for the ``compute()`` function.
|
|
||||||
)pbdoc")
|
|
||||||
.def("compute", &Generator::compute, R"pbdoc(
|
|
||||||
compute()
|
|
||||||
|
|
||||||
Args:
|
|
||||||
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(
|
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"(
|
||||||
MonitoringGenerator(name)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
name (str): Name of the stage.
|
|
||||||
|
|
||||||
Generator that monitors solutions of another stage to make reuse of them
|
Generator that monitors solutions of another stage to make reuse of them
|
||||||
Sometimes its necessary to reuse a previously planned solution, e.g. to
|
Sometimes its necessary to reuse a previously planned solution, e.g. to
|
||||||
traverse it in reverse order or to access the state of another generator.
|
traverse it in reverse order or to access the state of another generator.
|
||||||
@ -428,28 +304,17 @@ void export_core(pybind11::module& m) {
|
|||||||
|
|
||||||
task["generator"].setMonitoredStage(task["current"])
|
task["generator"].setMonitoredStage(task["current"])
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("generator"))
|
.def(py::init<const std::string&>(), "name"_a = std::string("generator"))
|
||||||
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, R"pbdoc(
|
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the reference to the Monitored Stage", "stage"_a)
|
||||||
setMonitoredStage(stage)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
stage (Stage): Monitor solutions of this stage.
|
|
||||||
Returns:
|
|
||||||
None
|
|
||||||
|
|
||||||
Set the reference to the Monitored Stage.
|
|
||||||
)pbdoc")
|
|
||||||
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution)
|
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution)
|
||||||
;
|
;
|
||||||
|
|
||||||
py::classh<ContainerBase, Stage>(m, "ContainerBase")
|
py::classh<ContainerBase, Stage>(m, "ContainerBase", "Base class for containers that implements utility functionality")
|
||||||
.def("add", &ContainerBase::add)
|
.def("add", &ContainerBase::add, "Add a stage to the container")
|
||||||
.def("insert", &ContainerBase::insert, py::arg("stage"), py::arg("before") = -1)
|
.def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1, "Insert a stage before given index into container")
|
||||||
.def("remove", py::overload_cast<int>(&ContainerBase::remove), R"pbdoc(
|
.def("remove", py::overload_cast<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
|
||||||
Remove child stage by index.
|
.def("clear", &ContainerBase::clear, "Clear the stages of the container")
|
||||||
)pbdoc")
|
|
||||||
.def("clear", &ContainerBase::clear)
|
|
||||||
.def("__len__", &ContainerBase::numChildren)
|
.def("__len__", &ContainerBase::numChildren)
|
||||||
.def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* {
|
.def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* {
|
||||||
Stage* child = c.findChild(name);
|
Stage* child = c.findChild(name);
|
||||||
@ -463,32 +328,23 @@ void export_core(pybind11::module& m) {
|
|||||||
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
|
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
|
||||||
;
|
;
|
||||||
|
|
||||||
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer")
|
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Base class for container containing a serial set of stages")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("serial container"));
|
.def(py::init<const std::string&>(), "name"_a = std::string("serial container"));
|
||||||
|
|
||||||
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase");
|
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase", "Base class for parallel containers");
|
||||||
|
|
||||||
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"pbdoc(
|
|
||||||
Alternatives(name)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
name (str): Name of the stage.
|
|
||||||
|
|
||||||
|
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"(
|
||||||
Plan for different alternatives in parallel.
|
Plan for different alternatives in parallel.
|
||||||
Solution of all children are reported - sorted by cost.
|
Solution of all children are reported - sorted by cost.
|
||||||
|
|
||||||
.. literalinclude:: ./../../../demo/scripts/alternatives.py
|
.. literalinclude:: ./../../../demo/scripts/alternatives.py
|
||||||
:language: python
|
:language: python
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("alternatives"));
|
.def(py::init<const std::string&>(), "name"_a = std::string("alternatives"))
|
||||||
|
;
|
||||||
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"pbdoc(
|
|
||||||
Fallbacks(name)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
name (str): Name of the stage.
|
|
||||||
|
|
||||||
|
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"(
|
||||||
Plan for different alternatives in sequence.
|
Plan for different alternatives in sequence.
|
||||||
Try to find feasible solutions using first child.
|
Try to find feasible solutions using first child.
|
||||||
Only if this fails, proceed to the next child trying
|
Only if this fails, proceed to the next child trying
|
||||||
@ -498,31 +354,22 @@ void export_core(pybind11::module& m) {
|
|||||||
.. literalinclude:: ./../../../demo/scripts/fallbacks.py
|
.. literalinclude:: ./../../../demo/scripts/fallbacks.py
|
||||||
:language: python
|
:language: python
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("fallbacks"));
|
.def(py::init<const std::string&>(), "name"_a = std::string("fallbacks"))
|
||||||
|
;
|
||||||
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"pbdoc(
|
|
||||||
Merger(name)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
name (str): Name of the stage.
|
|
||||||
|
|
||||||
|
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"(
|
||||||
Plan for different sub tasks in parallel and finally merge
|
Plan for different sub tasks in parallel and finally merge
|
||||||
all sub solutions into a single trajectory
|
all sub solutions into a single trajectory
|
||||||
|
|
||||||
.. literalinclude:: ./../../../demo/scripts/merger.py
|
.. literalinclude:: ./../../../demo/scripts/merger.py
|
||||||
:language: python
|
:language: python
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("merger"));
|
.def(py::init<const std::string&>(), "name"_a = std::string("merger"))
|
||||||
|
;
|
||||||
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"pbdoc(
|
|
||||||
WrapperBase(name, child)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
name (str): Name of the stage.
|
|
||||||
child (Stage): Wrapped stage.
|
|
||||||
|
|
||||||
|
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"(
|
||||||
A wrapper wraps a single child stage, which can be
|
A wrapper wraps a single child stage, which can be
|
||||||
accessed via ``wrapped()``. Implementations of this
|
accessed via ``wrapped()``. Implementations of this
|
||||||
interface need to implement ``onNewSolution()``, which
|
interface need to implement ``onNewSolution()``, which
|
||||||
@ -530,15 +377,10 @@ void export_core(pybind11::module& m) {
|
|||||||
The wrapper may reject the solution or create one or
|
The wrapper may reject the solution or create one or
|
||||||
multiple derived solutions, potentially adapting the cost,
|
multiple derived solutions, potentially adapting the cost,
|
||||||
as well as its start and end states.
|
as well as its start and end states.
|
||||||
)pbdoc");
|
)")
|
||||||
|
;
|
||||||
py::classh<Task>(m, "Task", R"pbdoc(
|
|
||||||
Task(ns, introspection)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
ns (str): Namespace of the task.
|
|
||||||
introspection (bool): Enable introspection.
|
|
||||||
|
|
||||||
|
py::classh<Task>(m, "Task", R"(
|
||||||
A task is the root of a tree of stages by
|
A task is the root of a tree of stages by
|
||||||
wrapping a single container
|
wrapping a single container
|
||||||
(by default a SerialContainer),
|
(by default a SerialContainer),
|
||||||
@ -550,66 +392,23 @@ void export_core(pybind11::module& m) {
|
|||||||
.. literalinclude:: ./../../../demo/scripts/current_state.py
|
.. literalinclude:: ./../../../demo/scripts/current_state.py
|
||||||
:language: python
|
:language: python
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.def(py::init<const std::string&, bool>(), py::arg("ns") = std::string(), py::arg("introspection") = true)
|
.def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
|
||||||
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(),
|
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(),
|
||||||
py::arg("ns") = std::string(), py::arg("introspection") = true, py::arg("container"))
|
"ns"_a = std::string(), "introspection"_a = true, "container"_a)
|
||||||
// read-only access to properties + solutions
|
// read-only access to properties + solutions
|
||||||
.def_property_readonly("properties", py::overload_cast<>(&Task::properties), R"pbdoc(
|
.def_property_readonly("properties", py::overload_cast<>(&Task::properties),
|
||||||
PropertyMap: Access the property map of the task.
|
"PropertyMap: Access the property map of the task")
|
||||||
)pbdoc")
|
.def_property_readonly("solutions", &Task::solutions,
|
||||||
.def_property_readonly("solutions", &Task::solutions, R"pbdoc(
|
"Solutions: Access the solutions of the task, once the sub stage hierarchy has been traversed and planned")
|
||||||
Solutions: Access the solutions of the task, once the sub stage hierarchy has been
|
.def_property_readonly("failures", &Task::failures,
|
||||||
traversed and planned.
|
"Solutions: Inspect failures that occurred during the planning phase")
|
||||||
)pbdoc")
|
.def_property("name", &Task::name, &Task::setName, "str: Set the name property of the task")
|
||||||
.def_property_readonly("failures", &Task::failures, R"pbdoc(
|
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", "Load robot model from given parameter")
|
||||||
Solutions: Inspect failures that occurred during the planning phase.
|
|
||||||
)pbdoc")
|
|
||||||
.def_property("name", &Task::name, &Task::setName, R"pbdoc(
|
|
||||||
str: Set the name property of the task.
|
|
||||||
)pbdoc")
|
|
||||||
.def("loadRobotModel", &Task::loadRobotModel, py::arg("robot_description") = "robot_description",
|
|
||||||
R"pbdoc(
|
|
||||||
loadRobotModel(robot_description)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
robot_description (str): Which robot model to load.
|
|
||||||
Returns:
|
|
||||||
None
|
|
||||||
|
|
||||||
Load robot model from given parameter.
|
|
||||||
)pbdoc")
|
|
||||||
.def("getRobotModel", &Task::getRobotModel)
|
.def("getRobotModel", &Task::getRobotModel)
|
||||||
.def("enableIntrospection", &Task::enableIntrospection, py::arg("enabled") = true, R"pbdoc(
|
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, "Enable introspection publish for use with `rviz`")
|
||||||
enableIntrospection(enable)
|
.def("clear", &Task::clear, "Reset the stage hierarchy")
|
||||||
|
.def("add", &Task::add, "Add a stage to the task hierarchy", "stage"_a)
|
||||||
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("__len__", [](const Task& t) { t.stages()->numChildren(); })
|
||||||
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* {
|
.def("__getitem__", [](const Task& t, const std::string &name) -> Stage* {
|
||||||
Stage* child = t.stages()->findChild(name);
|
Stage* child = t.stages()->findChild(name);
|
||||||
@ -621,59 +420,17 @@ void export_core(pybind11::module& m) {
|
|||||||
const auto& children = t.stages()->pimpl()->children();
|
const auto& children = t.stages()->pimpl()->children();
|
||||||
return py::make_iterator(children.begin(), children.end());
|
return py::make_iterator(children.begin(), children.end());
|
||||||
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
|
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
|
||||||
.def("reset", &Task::reset, R"pbdoc(
|
.def("reset", &Task::reset, "Reset all stages")
|
||||||
reset()
|
.def("init", py::overload_cast<>(&Task::init), "Initialize all stages with a given scene")
|
||||||
|
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(
|
||||||
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
|
Reset, initialize scene (if not yet done), and initialize all
|
||||||
stages, then start planning.
|
stages, then start planning with ``max_allowed_solutions``. Returns if planning
|
||||||
)pbdoc")
|
was successful.
|
||||||
.def("preempt", &Task::preempt, R"pbdoc(
|
)")
|
||||||
preempt()
|
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
|
||||||
|
|
||||||
Args:
|
|
||||||
None
|
|
||||||
Returns:
|
|
||||||
None
|
|
||||||
|
|
||||||
Interrupt current planning (or execution).
|
|
||||||
)pbdoc")
|
|
||||||
.def("publish", [](Task& self, const SolutionBasePtr& solution) {
|
.def("publish", [](Task& self, const SolutionBasePtr& solution) {
|
||||||
self.introspection().publishSolution(*solution);
|
self.introspection().publishSolution(*solution);
|
||||||
}, R"pbdoc(
|
}, "solution"_a, "Publish a given solution to the solution ros topic")
|
||||||
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) {
|
.def("execute", [](const Task& self, const SolutionBasePtr& solution) {
|
||||||
moveit::planning_interface::PlanningSceneInterface psi;
|
moveit::planning_interface::PlanningSceneInterface psi;
|
||||||
moveit::planning_interface::MoveGroupInterface
|
moveit::planning_interface::MoveGroupInterface
|
||||||
@ -693,17 +450,8 @@ void export_core(pybind11::module& m) {
|
|||||||
}
|
}
|
||||||
psi.applyPlanningScene(traj.scene_diff);
|
psi.applyPlanningScene(traj.scene_diff);
|
||||||
}
|
}
|
||||||
ROS_INFO("Executed successfully.");
|
ROS_INFO("Executed successfully");
|
||||||
}, R"pbdoc(
|
}, "solution"_a, "Execute Solution");
|
||||||
execute(solution)
|
|
||||||
|
|
||||||
Args:
|
|
||||||
solution (Solution): Solution, which should be executed.
|
|
||||||
Returns:
|
|
||||||
None
|
|
||||||
|
|
||||||
Execute Solution.
|
|
||||||
)pbdoc");
|
|
||||||
// clang-format on
|
// clang-format on
|
||||||
}
|
}
|
||||||
} // namespace python
|
} // namespace python
|
||||||
|
|||||||
@ -36,6 +36,7 @@
|
|||||||
#include <boost/core/demangle.hpp>
|
#include <boost/core/demangle.hpp>
|
||||||
|
|
||||||
namespace py = pybind11;
|
namespace py = pybind11;
|
||||||
|
using namespace py::literals;
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
|
||||||
@ -173,10 +174,10 @@ void export_properties(py::module& m) {
|
|||||||
.def(py::init<>())
|
.def(py::init<>())
|
||||||
.def("setValue", [](Property& self, const py::object& value)
|
.def("setValue", [](Property& self, const py::object& value)
|
||||||
{ self.setValue(PropertyConverterRegistry::fromPython(value)); },
|
{ self.setValue(PropertyConverterRegistry::fromPython(value)); },
|
||||||
"Set current and default value.", py::arg("value"))
|
"Set current and default value.", "value"_a)
|
||||||
.def("setCurrentValue", [](Property& self, const py::object& value)
|
.def("setCurrentValue", [](Property& self, const py::object& value)
|
||||||
{ self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); },
|
{ self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); },
|
||||||
"Set the current value only, w/o touching the default.", py::arg("value"))
|
"Set the current value only, w/o touching the default.", "value"_a)
|
||||||
.def("value", [](const Property& self)
|
.def("value", [](const Property& self)
|
||||||
{ return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.")
|
{ return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.")
|
||||||
.def("defaultValue", [](const Property& self)
|
.def("defaultValue", [](const Property& self)
|
||||||
@ -186,9 +187,9 @@ void export_properties(py::module& m) {
|
|||||||
.def("defined", &Property::defined, "Was a (non-default) value stored?")
|
.def("defined", &Property::defined, "Was a (non-default) value stored?")
|
||||||
.def("description", &Property::description, "Retrive the property description string")
|
.def("description", &Property::description, "Retrive the property description string")
|
||||||
.def("setDescription", &Property::setDescription,
|
.def("setDescription", &Property::setDescription,
|
||||||
"Set the property's description", py::arg("desc"));
|
"Set the property's description", "desc"_a);
|
||||||
|
|
||||||
py::classh<PropertyMap>(m, "PropertyMap", "dictionary of named :doc:`properties <pymoveit_mtc.core.Property>`")
|
py::classh<PropertyMap>(m, "PropertyMap", "Dictionary of named :doc:`properties <pymoveit_mtc.core.Property>`")
|
||||||
.def(py::init<>())
|
.def(py::init<>())
|
||||||
.def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();})
|
.def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();})
|
||||||
.def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); },
|
.def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); },
|
||||||
@ -200,7 +201,7 @@ void export_properties(py::module& m) {
|
|||||||
{ return self.property(key); }, py::return_value_policy::reference_internal, R"(
|
{ return self.property(key); }, py::return_value_policy::reference_internal, R"(
|
||||||
Retrieve the property instance for the given key.
|
Retrieve the property instance for the given key.
|
||||||
This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)",
|
This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)",
|
||||||
py::arg("key"))
|
"key"_a)
|
||||||
.def("__getitem__", [](const PropertyMap& self, const std::string& key)
|
.def("__getitem__", [](const PropertyMap& self, const std::string& key)
|
||||||
{ return PropertyConverterRegistry::toPython(self.get(key)); })
|
{ return PropertyConverterRegistry::toPython(self.get(key)); })
|
||||||
.def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value)
|
.def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value)
|
||||||
@ -211,29 +212,29 @@ void export_properties(py::module& m) {
|
|||||||
self.set(it->first.cast<std::string>(),
|
self.set(it->first.cast<std::string>(),
|
||||||
PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second)));
|
PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second)));
|
||||||
}
|
}
|
||||||
}, "Update property values from another dictionary", py::arg("values"))
|
}, "Update property values from another dictionary", "values"_a)
|
||||||
.def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) {
|
.def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) {
|
||||||
std::set<std::string> s;
|
std::set<std::string> s;
|
||||||
for (auto& item : names)
|
for (auto& item : names)
|
||||||
s.insert(item.cast<std::string>());
|
s.insert(item.cast<std::string>());
|
||||||
self.configureInitFrom(sources, s);
|
self.configureInitFrom(sources, s);
|
||||||
}, "Configure initialization of listed (or all) properties from given source(s).",
|
}, "Configure initialization of listed (or all) properties from given source(s).",
|
||||||
py::arg("sources"), py::arg("names") = py::list())
|
"sources"_a, "names"_a = py::list())
|
||||||
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) {
|
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) {
|
||||||
self.exposeTo(other, name, name);
|
self.exposeTo(other, name, name);
|
||||||
}, "Declare ``named`` property in ``other`` PropertyMap - using same name.",
|
}, "Declare ``named`` property in ``other`` PropertyMap - using same name.",
|
||||||
py::arg("other"), py::arg("name"))
|
"other"_a, "name"_a)
|
||||||
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) {
|
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) {
|
||||||
self.exposeTo(other, name, other_name);
|
self.exposeTo(other, name, other_name);
|
||||||
}, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.",
|
}, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.",
|
||||||
py::arg("other"), py::arg("name"), py::arg("other_name"))
|
"other"_a, "name"_a, "other_name"_a)
|
||||||
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) {
|
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) {
|
||||||
std::set<std::string> s;
|
std::set<std::string> s;
|
||||||
for (auto& item : names)
|
for (auto& item : names)
|
||||||
s.insert(item.cast<std::string>());
|
s.insert(item.cast<std::string>());
|
||||||
self.exposeTo(other, s);
|
self.exposeTo(other, s);
|
||||||
}, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.",
|
}, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.",
|
||||||
py::arg("other"), py::arg("names"))
|
"other"_a, "names"_a)
|
||||||
;
|
;
|
||||||
// clang-format on
|
// clang-format on
|
||||||
}
|
}
|
||||||
|
|||||||
@ -51,15 +51,14 @@ namespace moveit {
|
|||||||
namespace python {
|
namespace python {
|
||||||
|
|
||||||
void export_solvers(py::module& m) {
|
void export_solvers(py::module& m) {
|
||||||
properties::class_<PlannerInterface>(m, "PlannerInterface")
|
properties::class_<PlannerInterface>(m, "PlannerInterface", "Base class for planning algorithms")
|
||||||
.property<double>("max_velocity_scaling_factor")
|
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
|
||||||
.property<double>("max_acceleration_scaling_factor")
|
.property<double>("max_acceleration_scaling_factor",
|
||||||
|
"float: Reduce the maximum acceleration by scaling between (0,1]")
|
||||||
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
|
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
|
||||||
py::return_value_policy::reference_internal);
|
py::return_value_policy::reference_internal, "Properties of the planner");
|
||||||
|
|
||||||
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner", R"pbdoc(
|
|
||||||
PipelinePlanner()
|
|
||||||
|
|
||||||
|
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner", R"(
|
||||||
Use MoveIt's PlanningPipeline to plan a trajectory between to scenes.
|
Use MoveIt's PlanningPipeline to plan a trajectory between to scenes.
|
||||||
|
|
||||||
::
|
::
|
||||||
@ -85,38 +84,22 @@ void export_solvers(py::module& m) {
|
|||||||
move.setGoal("extended")
|
move.setGoal("extended")
|
||||||
task.add(move)
|
task.add(move)
|
||||||
|
|
||||||
)pbdoc")
|
)")
|
||||||
.property<std::string>("planner", R"pbdoc(
|
.property<std::string>("planner", "str: Planner ID")
|
||||||
str: Planner ID.
|
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
|
||||||
)pbdoc")
|
.property<moveit_msgs::WorkspaceParameters>("workspace_parameters", R"(
|
||||||
.property<uint>("num_planning_attempts", R"pbdoc(
|
|
||||||
uint: Number of planning attempts.
|
|
||||||
)pbdoc")
|
|
||||||
.property<moveit_msgs::WorkspaceParameters>("workspace_parameters", R"pbdoc(
|
|
||||||
WorkspaceParameters_: Workspace_parameters for the planning algorithm
|
WorkspaceParameters_: Workspace_parameters for the planning algorithm
|
||||||
|
|
||||||
.. _WorkspaceParameters: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/WorkspaceParameters.html
|
.. _WorkspaceParameters: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/WorkspaceParameters.html
|
||||||
)pbdoc")
|
)")
|
||||||
.property<double>("goal_joint_tolerance", R"pbdoc(
|
.property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
|
||||||
float: Tolerance for reaching joint goals.
|
.property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
|
||||||
)pbdoc")
|
.property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
|
||||||
.property<double>("goal_position_tolerance", R"pbdoc(
|
.property<bool>("display_motion_plans", "bool: Publish generated solutions via a topic")
|
||||||
float: Tolerance for reaching position goals.
|
.property<bool>("publish_planning_requests", "bool: Publish motion planning requests via a topic")
|
||||||
)pbdoc")
|
|
||||||
.property<double>("goal_orientation_tolerance", R"pbdoc(
|
|
||||||
float: 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<>());
|
.def(py::init<>());
|
||||||
|
|
||||||
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"pbdoc(
|
properties::class_<JointInterpolationPlanner, PlannerInterface>(m, "JointInterpolationPlanner", R"(
|
||||||
JointInterpolationPlanner()
|
|
||||||
|
|
||||||
Interpolate a trajectory between states in joint space.
|
Interpolate a trajectory between states in joint space.
|
||||||
Fails if direct joint space interpolation fails.
|
Fails if direct joint space interpolation fails.
|
||||||
|
|
||||||
@ -127,16 +110,12 @@ void export_solvers(py::module& m) {
|
|||||||
# Instantiate joint interpolation planner
|
# Instantiate joint interpolation planner
|
||||||
jointPlanner = core.JointInterpolationPlanner()
|
jointPlanner = core.JointInterpolationPlanner()
|
||||||
jointPlanner.max_step = 0.1
|
jointPlanner.max_step = 0.1
|
||||||
)pbdoc")
|
)")
|
||||||
.property<double>("max_step", R"pbdoc(
|
.property<double>("max_step",
|
||||||
float: Prevent large movements by specifying the maximum step distance
|
" float: Prevent large movements by specifying the maximum step distance in joint space")
|
||||||
in joint space.
|
|
||||||
)pbdoc")
|
|
||||||
.def(py::init<>());
|
.def(py::init<>());
|
||||||
|
|
||||||
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"pbdoc(
|
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
|
||||||
CartesianPath()
|
|
||||||
|
|
||||||
The planner uses MoveIt's ``computeCartesianPath()`` to
|
The planner uses MoveIt's ``computeCartesianPath()`` to
|
||||||
generate a straigh-line path between two scenes.
|
generate a straigh-line path between two scenes.
|
||||||
|
|
||||||
@ -148,19 +127,17 @@ void export_solvers(py::module& m) {
|
|||||||
cartesianPlanner = core.CartesianPath()
|
cartesianPlanner = core.CartesianPath()
|
||||||
cartesianPlanner.step_size = 0.01
|
cartesianPlanner.step_size = 0.01
|
||||||
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
|
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
|
||||||
)pbdoc")
|
)")
|
||||||
.property<double>("step_size", R"pbdoc(
|
.property<double>("step_size", R"(
|
||||||
float: 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.
|
step size between consecutive waypoints with this parameter.
|
||||||
)pbdoc")
|
)")
|
||||||
.property<double>("jump_threshold", R"pbdoc(
|
.property<double>("jump_threshold", R"(
|
||||||
float: 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.
|
acceptable fraction of mean joint motion per step.
|
||||||
)pbdoc")
|
)")
|
||||||
.property<double>("min_fraction", R"pbdoc(
|
.property<double>("min_fraction",
|
||||||
float: Lower threshold of minimum motion per step required for
|
"float: Lower threshold of minimum motion per step required for success in planning")
|
||||||
success in planning.
|
|
||||||
)pbdoc")
|
|
||||||
.def(py::init<>());
|
.def(py::init<>());
|
||||||
}
|
}
|
||||||
} // namespace python
|
} // namespace python
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user