mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
stages docs and bindings
- pybind bindings for all the examples
- correctly format docstrings
This commit is contained in:
parent
d6f1b69474
commit
923022c13b
@ -226,5 +226,5 @@ intersphinx_mapping = {"https://docs.python.org/3": None}
|
|||||||
|
|
||||||
# Default options for generating documentation.
|
# Default options for generating documentation.
|
||||||
autodoc_default_options = {
|
autodoc_default_options = {
|
||||||
"exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward"
|
"exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward, MergeMode"
|
||||||
}
|
}
|
||||||
|
|||||||
@ -54,6 +54,7 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveRelative)
|
|||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose)
|
||||||
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place)
|
||||||
@ -80,7 +81,7 @@ std::vector<T> elementOrList(const py::object& arg) {
|
|||||||
void export_stages(pybind11::module& m) {
|
void export_stages(pybind11::module& m) {
|
||||||
// clang-format off
|
// clang-format off
|
||||||
properties::class_<ModifyPlanningScene, Stage>(m, "ModifyPlanningScene", R"pbdoc(
|
properties::class_<ModifyPlanningScene, Stage>(m, "ModifyPlanningScene", R"pbdoc(
|
||||||
ModifyPlanningScene(self, name)
|
ModifyPlanningScene(name)
|
||||||
|
|
||||||
Allows modification of the planning scene.
|
Allows modification of the planning scene.
|
||||||
This stage takes the incoming planning scene and applies previously scheduled changes to it, for example:
|
This stage takes the incoming planning scene and applies previously scheduled changes to it, for example:
|
||||||
@ -91,26 +92,13 @@ void export_stages(pybind11::module& m) {
|
|||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/modify_planning_scene.py
|
||||||
|
:language: python
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# Specify a grasp object by the known name
|
|
||||||
graspObject = "sampleObject"
|
|
||||||
|
|
||||||
# allow collisions
|
|
||||||
allowCollision = stages.ModifyPlanningScene("allow object - hand collision")
|
|
||||||
allowCollision.allowCollisions(
|
|
||||||
graspObject,
|
|
||||||
"right_fingers",
|
|
||||||
True
|
|
||||||
)
|
|
||||||
task.add(allowCollision)
|
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("modify planning scene"))
|
.def(py::init<const std::string&>(), py::arg("name") = std::string("modify planning scene"))
|
||||||
.def("attachObject", &ModifyPlanningScene::attachObject, R"pbdoc(
|
.def("attachObject", &ModifyPlanningScene::attachObject, R"pbdoc(
|
||||||
attachObject(self, name, link)
|
attachObject(name, link)
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
name (str): Name of the object
|
name (str): Name of the object
|
||||||
@ -120,7 +108,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
None
|
None
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("detachObject", &ModifyPlanningScene::detachObject, R"pbdoc(
|
.def("detachObject", &ModifyPlanningScene::detachObject, R"pbdoc(
|
||||||
detachObject(self, name, link)
|
detachObject(name, link)
|
||||||
|
|
||||||
Detach an object from a robot link.
|
Detach an object from a robot link.
|
||||||
|
|
||||||
@ -134,7 +122,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
const std::string& attach_link, bool attach) {
|
const std::string& attach_link, bool attach) {
|
||||||
self.attachObjects(elementOrList<std::string>(names), attach_link, attach);
|
self.attachObjects(elementOrList<std::string>(names), attach_link, attach);
|
||||||
}, py::arg("names"), py::arg("attach_link"), py::arg("attach") = true, R"pbdoc(
|
}, py::arg("names"), py::arg("attach_link"), py::arg("attach") = true, R"pbdoc(
|
||||||
attachObjects(self, attach_link, attach)
|
attachObjects(attach_link, attach)
|
||||||
|
|
||||||
Attach multiple objects to a robot link.
|
Attach multiple objects to a robot link.
|
||||||
|
|
||||||
@ -149,7 +137,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
const std::string& attach_link) {
|
const std::string& attach_link) {
|
||||||
self.attachObjects(elementOrList<std::string>(names), attach_link, false);
|
self.attachObjects(elementOrList<std::string>(names), attach_link, false);
|
||||||
}, py::arg("names"), py::arg("attach_link"), R"pbdoc(
|
}, py::arg("names"), py::arg("attach_link"), R"pbdoc(
|
||||||
detachObjects(self, attach_link)
|
detachObjects(attach_link)
|
||||||
|
|
||||||
Detach multiple objects from a robot link.
|
Detach multiple objects from a robot link.
|
||||||
|
|
||||||
@ -163,7 +151,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
const py::object& first, const py::object& second, bool enable_collision) {
|
const py::object& first, const py::object& second, bool enable_collision) {
|
||||||
self.allowCollisions(elementOrList<std::string>(first), elementOrList<std::string>(second), enable_collision);
|
self.allowCollisions(elementOrList<std::string>(first), elementOrList<std::string>(second), enable_collision);
|
||||||
}, py::arg("first"), py::arg("second"), py::arg("enable_collision") = true, R"pbdoc(
|
}, py::arg("first"), py::arg("second"), py::arg("enable_collision") = true, R"pbdoc(
|
||||||
allowCollisions(self, first, second, enable_collision)
|
allowCollisions(first, second, enable_collision)
|
||||||
|
|
||||||
Allow or disable collisions between links and objects.
|
Allow or disable collisions between links and objects.
|
||||||
|
|
||||||
@ -174,45 +162,49 @@ void export_stages(pybind11::module& m) {
|
|||||||
set to false to disable collision checks.
|
set to false to disable collision checks.
|
||||||
Returns:
|
Returns:
|
||||||
None
|
None
|
||||||
|
)pbdoc")
|
||||||
|
.def("addObject", &ModifyPlanningScene::addObject, R"pbdoc(
|
||||||
|
addObject(collision_object)
|
||||||
|
|
||||||
|
Add an object to the planning scene
|
||||||
|
|
||||||
|
Args:
|
||||||
|
collision_object (CollisionObject_): Object to be added. Must be
|
||||||
|
in the appropriate message format.
|
||||||
|
|
||||||
|
.. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html
|
||||||
|
|
||||||
)pbdoc");
|
)pbdoc");
|
||||||
// clang-format on
|
// clang-format on
|
||||||
|
|
||||||
properties::class_<CurrentState, Stage>(m, "CurrentState", R"pbdoc(
|
properties::class_<CurrentState, Stage>(m, "CurrentState", R"pbdoc(
|
||||||
CurrentState(self, name)
|
CurrentState(name)
|
||||||
|
|
||||||
Fetch the current PlanningScene state via get_planning_scene service.
|
Fetch the current PlanningScene state via the ``get_planning_scene`` service.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/current_state.py
|
||||||
|
:language: python
|
||||||
|
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# create a stage instance
|
|
||||||
currentState = CurrentState('current state')
|
|
||||||
task.add(currentState)
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("current state"));
|
.def(py::init<const std::string&>(), py::arg("name") = std::string("current state"));
|
||||||
|
|
||||||
properties::class_<FixedState, Stage>(m, "FixedState", R"pbdoc(
|
properties::class_<FixedState, Stage>(m, "FixedState", R"pbdoc(
|
||||||
FixedState(self, name)
|
FixedState(name)
|
||||||
|
|
||||||
Spawn a pre-defined PlanningScene state.
|
Spawn a pre-defined PlanningScene state.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/fixed_state.py
|
||||||
|
:language: python
|
||||||
|
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# create a stage instance
|
|
||||||
fixedState = FixedState('fixed state')
|
|
||||||
task.add(fixedState)
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("setState", &FixedState::setState, R"pbdoc(
|
.def("setState", &FixedState::setState, R"pbdoc(
|
||||||
setState(self, scene)
|
setState(scene)
|
||||||
|
|
||||||
Use a planning scene pointer to specify which state the Fixed State
|
Use a planning scene pointer to specify which state the Fixed State
|
||||||
stage should have.
|
stage should have.
|
||||||
@ -233,7 +225,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
;
|
;
|
||||||
|
|
||||||
properties::class_<ComputeIK, Stage>(m, "ComputeIK", R"pbdoc(
|
properties::class_<ComputeIK, Stage>(m, "ComputeIK", R"pbdoc(
|
||||||
ComputeIK(self, name, stage)
|
ComputeIK(name, stage)
|
||||||
|
|
||||||
Wrapper for any pose generator stage to compute the inverse
|
Wrapper for any pose generator stage to compute the inverse
|
||||||
kinematics for a pose in Cartesian space.
|
kinematics for a pose in Cartesian space.
|
||||||
@ -255,17 +247,8 @@ void export_stages(pybind11::module& m) {
|
|||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
stage: Stage that contains the robot state for IK calculation.
|
stage: Stage that contains the robot state for IK calculation.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/compute_ik.py
|
||||||
|
:language: python
|
||||||
# create a task
|
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# get the current robot state
|
|
||||||
currentState = stages.CurrentState("current state")
|
|
||||||
|
|
||||||
# calculate the inverse kinematics for the current robot state
|
|
||||||
computeIK = stages.ComputeIK("compute IK", currentState)
|
|
||||||
task.add(computeIK)
|
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef", R"pbdoc(
|
.property<std::string>("eef", R"pbdoc(
|
||||||
@ -289,15 +272,17 @@ void export_stages(pybind11::module& m) {
|
|||||||
the planning scene are allowed.
|
the planning scene are allowed.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
||||||
PoseStamped_ : Specify the frame with respect
|
PoseStamped_: Specify the frame with respect
|
||||||
to which the inverse kinematics should be calculated.
|
to which the inverse kinematics
|
||||||
|
should be calculated.
|
||||||
|
|
||||||
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<geometry_msgs::PoseStamped>("target_pose", R"pbdoc(
|
.property<geometry_msgs::PoseStamped>("target_pose", R"pbdoc(
|
||||||
PoseStamped_ : Specify the pose on which
|
PoseStamped_: Specify the pose on which
|
||||||
the inverse kinematics should be
|
the inverse kinematics should be
|
||||||
calculated on. Since this property should almost always be set
|
calculated on. Since this property should
|
||||||
|
almost always be set
|
||||||
in the Interface State which is sent by the child,
|
in the Interface State which is sent by the child,
|
||||||
if possible, avoid setting it manually.
|
if possible, avoid setting it manually.
|
||||||
|
|
||||||
@ -307,7 +292,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
.def(py::init<const std::string&, Stage::pointer&&>());
|
.def(py::init<const std::string&, Stage::pointer&&>());
|
||||||
|
|
||||||
properties::class_<MoveTo, PropagatingEitherWay, PyMoveTo<>>(m, "MoveTo", R"pbdoc(
|
properties::class_<MoveTo, PropagatingEitherWay, PyMoveTo<>>(m, "MoveTo", R"pbdoc(
|
||||||
MoveTo(self, name, planner)
|
MoveTo(name, planner)
|
||||||
|
|
||||||
Compute a trajectory between the robot state from the
|
Compute a trajectory between the robot state from the
|
||||||
interface state of the preceeding stage and a specified
|
interface state of the preceeding stage and a specified
|
||||||
@ -317,44 +302,28 @@ void export_stages(pybind11::module& m) {
|
|||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
planner (PlannerInterface): Planner that is used to compute the path of motion.
|
planner (PlannerInterface): Planner that is used to compute the path of motion.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/cartesian.py
|
||||||
|
:language: python
|
||||||
|
:lines: 51-55
|
||||||
|
|
||||||
# create a planner instance
|
|
||||||
jointspace = core.JointInterpolationPlanner()
|
|
||||||
|
|
||||||
# specify planning group
|
|
||||||
group = "foo_robot"
|
|
||||||
|
|
||||||
# create a task
|
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# get the current robot state
|
|
||||||
currentState = stages.CurrentState("current state")
|
|
||||||
task.add(currentState)
|
|
||||||
|
|
||||||
# moveTo named posture, using joint-space interplation
|
|
||||||
move = stages.MoveTo("moveTo ready", jointspace)
|
|
||||||
move.group = group
|
|
||||||
move.setGoal("ready")
|
|
||||||
task.add(move)
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("group", R"pbdoc(
|
.property<std::string>("group", R"pbdoc(
|
||||||
str: Planning group which should be utilized for planning and execution.
|
str: Planning group which should be utilized for planning and execution.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
||||||
PoseStamped_ : IK reference frame for the goal pose.
|
PoseStamped_: IK reference frame for the goal pose.
|
||||||
|
|
||||||
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<moveit_msgs::Constraints>("path_constraints", R"pbdoc(
|
.property<moveit_msgs::Constraints>("path_constraints", R"pbdoc(
|
||||||
Constraints_ : Set path constraints via the corresponding moveit message type.
|
Constraints_: Set path constraints via the corresponding moveit message type.
|
||||||
|
|
||||||
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
|
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>())
|
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>())
|
||||||
.def("setGoal", py::overload_cast<const geometry_msgs::PoseStamped&>(&MoveTo::setGoal), R"pbdoc(
|
.def("setGoal", py::overload_cast<const geometry_msgs::PoseStamped&>(&MoveTo::setGoal), R"pbdoc(
|
||||||
setGoal(self, goal)
|
setGoal(goal)
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
goal (PoseStamped_): Desired configuration.
|
goal (PoseStamped_): Desired configuration.
|
||||||
@ -367,7 +336,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("setGoal", py::overload_cast<const geometry_msgs::PointStamped&>(&MoveTo::setGoal), R"pbdoc(
|
.def("setGoal", py::overload_cast<const geometry_msgs::PointStamped&>(&MoveTo::setGoal), R"pbdoc(
|
||||||
setGoal(self, goal)
|
setGoal(goal)
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
goal (PointStamped_): Desired configuration.
|
goal (PointStamped_): Desired configuration.
|
||||||
@ -410,58 +379,42 @@ void export_stages(pybind11::module& m) {
|
|||||||
)pbdoc");
|
)pbdoc");
|
||||||
|
|
||||||
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative", R"pbdoc(
|
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative", R"pbdoc(
|
||||||
MoveRelative(self, name, planner)
|
MoveRelative(name, planner)
|
||||||
|
|
||||||
Perform a Cartesian motion relative to some link .
|
Perform a Cartesian motion relative to some link.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
planner (PlannerInterface): Planner that is used to compute the path of motion.
|
planner (PlannerInterface): Planner that is used to compute the path of motion.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/cartesian.py
|
||||||
|
:language: python
|
||||||
# Planning group
|
:lines: 26-31
|
||||||
group = "panda_arm"
|
|
||||||
|
|
||||||
# Cartesian planner
|
|
||||||
cartesian = core.CartesianPath()
|
|
||||||
|
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# start from current robot state
|
|
||||||
task.add(stages.CurrentState("current state"))
|
|
||||||
|
|
||||||
# move along x
|
|
||||||
move = stages.MoveRelative("x +0.2", cartesian)
|
|
||||||
move.group = group
|
|
||||||
header = Header(frame_id="world")
|
|
||||||
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
|
|
||||||
task.add(move)
|
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("group", R"pbdoc(
|
.property<std::string>("group", R"pbdoc(
|
||||||
str: Planning group which should be utilized for planning and execution.
|
str: Planning group which should be utilized for planning and execution.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
||||||
PoseStamped_ : IK reference frame for the goal pose.
|
PoseStamped_: IK reference frame for the goal pose.
|
||||||
|
|
||||||
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<double>("min_distance", R"pbdoc(
|
.property<double>("min_distance", R"pbdoc(
|
||||||
double: Set the minimum distance to move.
|
float: Set the minimum distance to move.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<double>("max_distance", R"pbdoc(
|
.property<double>("max_distance", R"pbdoc(
|
||||||
double: Set the maximum distance to move.
|
float: Set the maximum distance to move.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<moveit_msgs::Constraints>("path_constraints", R"pbdoc(
|
.property<moveit_msgs::Constraints>("path_constraints", R"pbdoc(
|
||||||
Constraints_ : These are the path constraints.
|
Constraints_: These are the path constraints.
|
||||||
|
|
||||||
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
|
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>())
|
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>())
|
||||||
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection),
|
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection),
|
||||||
R"pbdoc(
|
R"pbdoc(
|
||||||
setDirection(self, twist)
|
setDirection(twist)
|
||||||
|
|
||||||
Perform twist motion on specified link.
|
Perform twist motion on specified link.
|
||||||
|
|
||||||
@ -472,13 +425,13 @@ void export_stages(pybind11::module& m) {
|
|||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection),
|
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection),
|
||||||
R"pbdoc(
|
R"pbdoc(
|
||||||
setDirection(self, direction)
|
setDirection(direction)
|
||||||
|
|
||||||
Translate link along given direction.
|
Translate link along given direction.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection),
|
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection),
|
||||||
R"pbdoc(
|
R"pbdoc(
|
||||||
setDirection(self, joint_deltas)
|
setDirection(joint_deltas)
|
||||||
|
|
||||||
Move specified joint variables by given amount.
|
Move specified joint variables by given amount.
|
||||||
)pbdoc");
|
)pbdoc");
|
||||||
@ -496,7 +449,7 @@ void export_stages(pybind11::module& m) {
|
|||||||
PropertyConverter<stages::Connect::MergeMode>();
|
PropertyConverter<stages::Connect::MergeMode>();
|
||||||
|
|
||||||
properties::class_<Connect, Stage>(m, "Connect", R"pbdoc(
|
properties::class_<Connect, Stage>(m, "Connect", R"pbdoc(
|
||||||
Connect(self, name, planners)
|
Connect(name, planners)
|
||||||
|
|
||||||
Connect arbitrary InterfaceStates by motion planning.
|
Connect arbitrary InterfaceStates by motion planning.
|
||||||
You can specify the planning groups and the planners you
|
You can specify the planning groups and the planners you
|
||||||
@ -514,23 +467,18 @@ void export_stages(pybind11::module& m) {
|
|||||||
Name (str): Name of the stage.
|
Name (str): Name of the stage.
|
||||||
Planners (list): List of the planner - group associations.
|
Planners (list): List of the planner - group associations.
|
||||||
|
|
||||||
::
|
The example below contains a snippet from the :ref:`pick pipeline example<pick>`.
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
:lines: 48-60
|
||||||
|
|
||||||
# Create a planner instance
|
|
||||||
samplingPlanner = PipelinePlanner()
|
|
||||||
# Specify group-planner combinations
|
|
||||||
planners = [
|
|
||||||
('foo_group', samplingPlanner),
|
|
||||||
('bar_group', samplingPlanner)
|
|
||||||
]
|
|
||||||
# create a stage instance
|
|
||||||
connect = Connect('connect', planners)
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&, const Connect::GroupPlannerVector&>(),
|
.def(py::init<const std::string&, const Connect::GroupPlannerVector&>(),
|
||||||
py::arg("name") = std::string("connect"), py::arg("planners"));
|
py::arg("name") = std::string("connect"), py::arg("planners"));
|
||||||
|
|
||||||
properties::class_<FixCollisionObjects, Stage>(m, "FixCollisionObjects", R"pbdoc(
|
properties::class_<FixCollisionObjects, Stage>(m, "FixCollisionObjects", R"pbdoc(
|
||||||
FixCollisionObjects(self, name)
|
FixCollisionObjects(name)
|
||||||
|
|
||||||
Test for collisions and find a correction for applicable objects.
|
Test for collisions and find a correction for applicable objects.
|
||||||
Move the objects out of the way along the correction direction.
|
Move the objects out of the way along the correction direction.
|
||||||
@ -538,13 +486,8 @@ void export_stages(pybind11::module& m) {
|
|||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/fix_collision_objects.py
|
||||||
|
:language: python
|
||||||
task = core.Task()
|
|
||||||
|
|
||||||
# check for collisions and find corrections
|
|
||||||
fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects")
|
|
||||||
task.add(fixCollisionObjects)
|
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<double>("max_penetration", R"pbdoc(
|
.property<double>("max_penetration", R"pbdoc(
|
||||||
@ -552,6 +495,34 @@ void export_stages(pybind11::module& m) {
|
|||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("fix collisions"));
|
.def(py::init<const std::string&>(), py::arg("name") = std::string("fix collisions"));
|
||||||
|
|
||||||
|
properties::class_<GeneratePlacePose, MonitoringGenerator>(m, "GeneratePlacePose", R"pbdoc(
|
||||||
|
GeneratePlacePose(name)
|
||||||
|
|
||||||
|
GeneratePlacePose stage derives from monitoring generator and generates poses
|
||||||
|
for the place pipeline. Notice that whilst GenerateGraspPose spawns poses with an
|
||||||
|
``angle_delta`` intervall, GeneratePlacePose samples a fixed amount, which is dependent
|
||||||
|
on the objects shape.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
name (str): Name of the stage.
|
||||||
|
|
||||||
|
The example below contains a snippet from the :ref:`pick pipeline example<pick>`.
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
:lines: 115-122
|
||||||
|
|
||||||
|
)pbdoc")
|
||||||
|
.property<std::string>("object", R"pbdoc(
|
||||||
|
str: Name of the object in the planning scene,
|
||||||
|
attached to the robot which should be placed.
|
||||||
|
)pbdoc")
|
||||||
|
.property<std::string>("eef", R"pbdoc(
|
||||||
|
str: Name of the end effector that should be used for grasping.
|
||||||
|
)pbdoc")
|
||||||
|
.property<geometry_msgs::PoseStamped>("pose")
|
||||||
|
.def(py::init<const std::string&>(), py::arg("name") = std::string("Generate Place Pose"));
|
||||||
|
|
||||||
properties::class_<GenerateGraspPose, MonitoringGenerator>(m, "GenerateGraspPose", R"pbdoc(
|
properties::class_<GenerateGraspPose, MonitoringGenerator>(m, "GenerateGraspPose", R"pbdoc(
|
||||||
GenerateGraspPose(name)
|
GenerateGraspPose(name)
|
||||||
|
|
||||||
@ -562,6 +533,12 @@ void export_stages(pybind11::module& m) {
|
|||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
|
The example below contains a snippet from the :ref:`pick pipeline example<pick>`.
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
:lines: 62-68
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("object", R"pbdoc(
|
.property<std::string>("object", R"pbdoc(
|
||||||
str: Name of the Object in the planning scene, which should be grasped.
|
str: Name of the Object in the planning scene, which should be grasped.
|
||||||
@ -576,12 +553,12 @@ void export_stages(pybind11::module& m) {
|
|||||||
str: Name of the grasp pose.
|
str: Name of the grasp pose.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<double>("angle_delta", R"pbdoc(
|
.property<double>("angle_delta", R"pbdoc(
|
||||||
double: Angular step distance in rad with which positions around the object are sampled.
|
float: Angular step distance in rad with which positions around the object are sampled.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&>(), py::arg("name") = std::string("Generate Grasp Pose"));
|
.def(py::init<const std::string&>(), py::arg("name") = std::string("Generate Grasp Pose"));
|
||||||
|
|
||||||
properties::class_<GeneratePose, MonitoringGenerator>(m, "GeneratePose", R"pbdoc(
|
properties::class_<GeneratePose, MonitoringGenerator>(m, "GeneratePose", R"pbdoc(
|
||||||
GeneratePose(self, name)
|
GeneratePose(name)
|
||||||
|
|
||||||
Monitoring generator stage which can be used to generate a pose, based on solutions provided
|
Monitoring generator stage which can be used to generate a pose, based on solutions provided
|
||||||
by the monitored stage.
|
by the monitored stage.
|
||||||
@ -589,23 +566,29 @@ void export_stages(pybind11::module& m) {
|
|||||||
Args:
|
Args:
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
::
|
.. literalinclude:: ./../../../demo/scripts/generate_pose.py
|
||||||
|
:language: python
|
||||||
task = core.Task()
|
:lines: 35-48
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<geometry_msgs::PoseStamped>("pose", R"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: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<const std::string&>());
|
.def(py::init<const std::string&>());
|
||||||
|
|
||||||
properties::class_<Pick, Stage>(m, "Pick", R"pbdoc(
|
properties::class_<Pick, Stage>(m, "Pick", R"pbdoc(
|
||||||
Pick(self, grasp_generator, name)
|
Pick(grasp_generator, name)
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
grasp_generator ():
|
grasp_generator (SimpleGrasp): Simple Grasp stage to provide
|
||||||
|
poses and inverse kinematics solutions.
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
|
.. _pick:
|
||||||
|
|
||||||
The Pick stage is a specialization of the PickPlaceBase class, which
|
The Pick stage is a specialization of the PickPlaceBase class, which
|
||||||
wraps the pipeline to pick or place an object with a given end effector.
|
wraps the pipeline to pick or place an object with a given end effector.
|
||||||
|
|
||||||
@ -618,40 +601,77 @@ void export_stages(pybind11::module& m) {
|
|||||||
The end effector postures corresponding to pre-grasp and grasp as well
|
The end effector postures corresponding to pre-grasp and grasp as well
|
||||||
as the end effector's cartesian pose needs to be provided by an external
|
as the end effector's cartesian pose needs to be provided by an external
|
||||||
grasp stage.
|
grasp stage.
|
||||||
|
|
||||||
|
The example below combines pick and place routines in a single pickplace pipeline.
|
||||||
|
Please refer to the demonstration scripts of the moveit task constructor repository
|
||||||
|
for reference.
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("object", R"pbdoc(
|
.property<std::string>("object", R"pbdoc(
|
||||||
Name of object to pick.
|
str: Name of object to pick.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef", R"pbdoc(
|
.property<std::string>("eef", R"pbdoc(
|
||||||
End effector name.
|
str: The End effector name.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef_frame", R"pbdoc(
|
.property<std::string>("eef_frame", R"pbdoc(
|
||||||
Name of the end effector frame.
|
str: Name of the end effector frame.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef_group", R"pbdoc(
|
.property<std::string>("eef_group", R"pbdoc(
|
||||||
Joint model group of the end effector.
|
str: Joint model group of the end effector.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef_parent_group", R"pbdoc(
|
.property<std::string>("eef_parent_group", R"pbdoc(
|
||||||
Joint model group of the eef's parent.
|
str: Joint model group of the eef's parent.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("grasp_generator"),
|
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("grasp_generator"),
|
||||||
py::arg("name") = std::string("pick"))
|
py::arg("name") = std::string("pick"))
|
||||||
.def("setApproachMotion", &Pick::setApproachMotion, R"pbdoc(
|
.def("setApproachMotion", &Pick::setApproachMotion, R"pbdoc(
|
||||||
|
setApproachMotion(motion, min_distance, max_distance)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motion (Twist_): The twist, which represents the
|
||||||
|
approach motion.
|
||||||
|
min_distance (float): Minimum allowed distance.
|
||||||
|
max_distance (float): Maximum allowed distance.
|
||||||
|
|
||||||
|
The approaching motion towards the grasping state is represented
|
||||||
|
by a twist message.
|
||||||
|
|
||||||
|
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("setLiftMotion",
|
.def("setLiftMotion",
|
||||||
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion), R"pbdoc(
|
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion), R"pbdoc(
|
||||||
|
setLiftMotion(motion, min_distance, max_distance)
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def("setLiftMotion", py::overload_cast<const std::map<std::string, double>&>(&Pick::setLiftMotion), R"pbdoc(
|
.def("setLiftMotion", py::overload_cast<const std::map<std::string, double>&>(&Pick::setLiftMotion), R"pbdoc(
|
||||||
|
setLiftMotion(place)
|
||||||
|
|
||||||
|
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.
|
||||||
)pbdoc");
|
)pbdoc");
|
||||||
|
|
||||||
properties::class_<Place, Stage>(m, "Place", R"pbdoc(
|
properties::class_<Place, Stage>(m, "Place", R"pbdoc(
|
||||||
Place(self, place_generator, name)
|
Place(place_generator, name)
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
place_generator ():
|
place_generator (SimpleUnGrasp): SimpleUnGrasp Wrapper for pose generation
|
||||||
name (str): Name of the stage.
|
name (str): Name of the stage.
|
||||||
|
|
||||||
The Place stage is a specialization of the PickPlaceBase class, which
|
The Place stage is a specialization of the PickPlaceBase class, which
|
||||||
@ -666,54 +686,224 @@ void export_stages(pybind11::module& m) {
|
|||||||
The end effector postures corresponding to pre-grasp and grasp as well
|
The end effector postures corresponding to pre-grasp and grasp as well
|
||||||
as the end effector's Cartesian pose needs to be provided by an external
|
as the end effector's Cartesian pose needs to be provided by an external
|
||||||
grasp stage.
|
grasp stage.
|
||||||
|
|
||||||
|
For a working example, please consider the Pick_ Stage.
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
:lines: 131-135
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("object", R"pbdoc(
|
.property<std::string>("object", R"pbdoc(
|
||||||
Name of object to pick.
|
str: Name of object to pick.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef", R"pbdoc(
|
.property<std::string>("eef", R"pbdoc(
|
||||||
End effector name.
|
str: The End effector name.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef_frame", R"pbdoc(
|
.property<std::string>("eef_frame", R"pbdoc(
|
||||||
Name of the end effector frame.
|
str: Name of the end effector frame.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef_group", R"pbdoc(
|
.property<std::string>("eef_group", R"pbdoc(
|
||||||
Joint model group of the end effector.
|
str: Joint model group of the end effector.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef_parent_group", R"pbdoc(
|
.property<std::string>("eef_parent_group", R"pbdoc(
|
||||||
Joint model group of the eef's parent.
|
str: Joint model group of the eef's parent.
|
||||||
|
)pbdoc")
|
||||||
|
.def("setRetractMotion", &Place::setRetractMotion, R"pbdoc(
|
||||||
|
setRetractMotion(motion, min_distance, max_distance)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motion (Twist_): The twist, which represents the
|
||||||
|
retract motion.
|
||||||
|
min_distance (float): Minimum allowed distance.
|
||||||
|
max_distance (float): Maximum allowed distance.
|
||||||
|
|
||||||
|
The retract motion towards the final state is represented
|
||||||
|
by a twist message.
|
||||||
|
|
||||||
|
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
|
||||||
|
)pbdoc")
|
||||||
|
.def("setPlaceMotion",
|
||||||
|
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Place::setPlaceMotion), R"pbdoc(
|
||||||
|
setPlaceMotion(motion, min_distance, max_distance)
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
.. _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)
|
||||||
|
|
||||||
|
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.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("place_generator"),
|
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("place_generator"),
|
||||||
py::arg("name") = std::string("place"));
|
py::arg("name") = std::string("place"));
|
||||||
|
|
||||||
properties::class_<SimpleGrasp, Stage>(m, "SimpleGrasp", R"pbdoc(
|
properties::class_<SimpleGrasp, Stage>(m, "SimpleGrasp", R"pbdoc(
|
||||||
Specialization of SimpleGraspBase to realize grasping
|
SimpleGrasp(pose_generator, name)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
pose_generator (GenerateGraspPose): Generator stage to
|
||||||
|
sample possible grasp poses.
|
||||||
|
name (str): Name of the stage.
|
||||||
|
|
||||||
|
Specialization of SimpleGraspBase to realize grasping.
|
||||||
|
Refer to the pick_ stage for a minimum code example:
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
:lines: 70-79
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("eef", R"pbdoc(
|
.property<std::string>("eef", R"pbdoc(
|
||||||
|
str: The end effector of the robot.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.property<std::string>("object", R"pbdoc(
|
.property<std::string>("object", R"pbdoc(
|
||||||
|
str: The object to grasp (Must be present in the planning scene).
|
||||||
|
)pbdoc")
|
||||||
|
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
||||||
|
PoseStamped_: Set the frame for which
|
||||||
|
the inverse kinematics are calculated
|
||||||
|
with respect to each pose generated by
|
||||||
|
the pose_generator.
|
||||||
|
|
||||||
|
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("pose_generator"),
|
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("pose_generator"),
|
||||||
py::arg("name") = std::string("grasp generator"))
|
py::arg("name") = std::string("grasp generator"))
|
||||||
.def<void (SimpleGrasp::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc(
|
.def<void (SimpleGrasp::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc(
|
||||||
|
setIKFrame(transform)
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame,
|
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame,
|
||||||
R"pbdoc(
|
R"pbdoc(
|
||||||
|
setIKFrame(pose, link)
|
||||||
|
|
||||||
|
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.
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def<void (SimpleGrasp::*)(const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc(
|
.def<void (SimpleGrasp::*)(const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc(
|
||||||
|
setIKFrame(link)
|
||||||
|
|
||||||
|
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.
|
||||||
|
)pbdoc")
|
||||||
|
.def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"pbdoc(
|
||||||
|
setMaxIKSolutions(max_ik_solutions)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
max_ik_solutions (int):
|
||||||
|
|
||||||
|
Set the maximum number of inverse kinematics solutions that
|
||||||
|
should be computed.
|
||||||
|
|
||||||
)pbdoc");
|
)pbdoc");
|
||||||
|
|
||||||
properties::class_<SimpleUnGrasp, Stage>(m, "SimpleUnGrasp", R"pbdoc(
|
properties::class_<SimpleUnGrasp, Stage>(m, "SimpleUnGrasp", R"pbdoc(
|
||||||
|
SimpleUnGrasp(pose_generator, name)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
pose_generator (GeneratePlacePose): Generator stage to
|
||||||
|
sample possible Place
|
||||||
|
poses.
|
||||||
|
name (str): Name of the stage.
|
||||||
|
|
||||||
Specialization of SimpleGraspBase to realize ungrasping
|
Specialization of SimpleGraspBase to realize ungrasping
|
||||||
)pbdoc")
|
Refer to the place_ stage for a minimum code example.
|
||||||
.property<std::string>("eef", R"pbdoc(
|
Make shure to set the ``grasp`` and ``pregrasp`` properties
|
||||||
|
here again since they are not forwarded through the stage
|
||||||
|
hierarchy.
|
||||||
|
|
||||||
|
.. literalinclude:: ./../../../demo/scripts/pickplace.py
|
||||||
|
:language: python
|
||||||
|
:lines: 124-129
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
|
.property<std::string>("eef", R"pbdoc(
|
||||||
|
str: The end effector of the robot.
|
||||||
|
)pbdoc")
|
||||||
.property<std::string>("object", R"pbdoc(
|
.property<std::string>("object", R"pbdoc(
|
||||||
|
str: The object to grasp (Must be present in the planning scene).
|
||||||
|
)pbdoc")
|
||||||
|
.property<std::string>("pregrasp", R"pbdoc(
|
||||||
|
str: Name of the pre-grasp pose.
|
||||||
|
)pbdoc")
|
||||||
|
.property<std::string>("grasp", R"pbdoc(
|
||||||
|
str: Name of the grasp pose.
|
||||||
|
)pbdoc")
|
||||||
|
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
|
||||||
|
PoseStamped_: Specify the frame with respect
|
||||||
|
to which the inverse kinematics
|
||||||
|
should be calculated.
|
||||||
|
|
||||||
|
.. _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)
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
.. _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)
|
||||||
|
|
||||||
|
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.
|
||||||
|
)pbdoc")
|
||||||
|
.def<void (SimpleUnGrasp::*)(const std::string&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc(
|
||||||
|
setIKFrame(link)
|
||||||
|
|
||||||
|
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.
|
||||||
|
)pbdoc")
|
||||||
|
.def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"pbdoc(
|
||||||
|
setMaxIKSolutions(max_ik_solutions)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
max_ik_solutions (int):
|
||||||
|
|
||||||
|
Set the maximum number of inverse kinematics solutions that
|
||||||
|
should be computed.
|
||||||
|
|
||||||
)pbdoc")
|
)pbdoc")
|
||||||
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("pose_generator"),
|
.def(py::init<Stage::pointer&&, const std::string&>(), py::arg("pose_generator"),
|
||||||
|
|||||||
@ -4,7 +4,6 @@
|
|||||||
from moveit.task_constructor import core, stages
|
from moveit.task_constructor import core, stages
|
||||||
from moveit_commander import PlanningSceneInterface
|
from moveit_commander import PlanningSceneInterface
|
||||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||||
from std_msgs.msg import Header
|
|
||||||
import rospy
|
import rospy
|
||||||
import time
|
import time
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user