Fix SimpleGrasp/SimpleUnGrasp wrapper

- Wrap common base class SimpleGraspBase to reduce redundancy
- Use correct defaults for stage name
This commit is contained in:
Robert Haschke 2022-12-08 09:37:33 +01:00
parent 7200bbbfb1
commit 274c75f68f

View File

@ -59,6 +59,7 @@ 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)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGraspBase)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp) PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp) PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp)
@ -473,7 +474,36 @@ void export_stages(pybind11::module& m) {
.def(py::init<Stage::pointer&&, const std::string&>(), "place_generator"_a, .def(py::init<Stage::pointer&&, const std::string&>(), "place_generator"_a,
"name"_a = std::string("place")); "name"_a = std::string("place"));
properties::class_<SimpleGrasp, SerialContainer>(m, "SimpleGrasp", R"( properties::class_<SimpleGraspBase, SerialContainer>(m, "SimpleGraspBase", "Abstract base class for grasping and releasing")
.property<std::string>("eef", "str: The end effector of the robot")
.property<std::string>("object", "str: The object to grasp (Must be present in the planning scene)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: Set the frame for which the inverse kinematics is calculated
with respect to each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def<void (SimpleGraspBase::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ 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
)", "transform"_a)
.def<void (SimpleGraspBase::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ 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
)", "pose"_a, "link"_a)
.def<void (SimpleGraspBase::*)(const std::string&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
)", "link"_a)
.def("setMaxIKSolutions", &SimpleGraspBase::setMaxIKSolutions, R"(
Set the maximum number of inverse kinematics solutions that should be computed.
)", "max_ik_solutions"_a)
;
properties::class_<SimpleGrasp, SimpleGraspBase>(m, "SimpleGrasp", R"(
Specialization of SimpleGraspBase to realize grasping. Specialization of SimpleGraspBase to realize grasping.
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look, Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
@ -481,40 +511,10 @@ void export_stages(pybind11::module& m) {
of a task hierarchy that makes use of the of a task hierarchy that makes use of the
``SimpleGrasp`` stage. ``SimpleGrasp`` stage.
)") )")
.property<std::string>("eef", "str: The end effector of the robot")
.property<std::string>("object", "str: The object to grasp (Must be present in the planning scene)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
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
)")
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a, .def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
"name"_a = std::string("grasp generator")) "name"_a = std::string("grasp"));
.def<void (SimpleGrasp::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"(
Set the frame as a PoseStamped_ 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 properties::class_<SimpleUnGrasp, SimpleGraspBase>(m, "SimpleUnGrasp", R"(
)", "transform"_a)
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"(
Set the frame as a PoseStamped_ 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
)", "pose"_a, "link"_a)
.def<void (SimpleGrasp::*)(const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"(
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
)", "link"_a)
.def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"(
Set the maximum number of inverse kinematics solutions that
should be computed.
)", "max_ik_solutions"_a);
properties::class_<SimpleUnGrasp, SerialContainer>(m, "SimpleUnGrasp", R"(
Specialization of SimpleGraspBase to realize ungrasping Specialization of SimpleGraspBase to realize ungrasping
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look, Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
@ -522,42 +522,10 @@ void export_stages(pybind11::module& m) {
of a task hierarchy that makes use of the of a task hierarchy that makes use of the
``SimpleUnGrasp`` stage. ``SimpleUnGrasp`` stage.
)") )")
.property<std::string>("eef", "str: The end effector of the robot")
.property<std::string>("object", "str: The object to grasp (Must be present in the planning scene)")
.property<std::string>("pregrasp", "str: Name of the pre-grasp pose") .property<std::string>("pregrasp", "str: Name of the pre-grasp pose")
.property<std::string>("grasp", "str: Name of the grasp pose") .property<std::string>("grasp", "str: Name of the grasp pose")
.property<geometry_msgs::PoseStamped>("ik_frame", R"( .def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
PoseStamped_: Specify the frame with respect "name"_a = std::string("ungrasp"));
to which the inverse kinematics
should be calculated.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def<void (SimpleUnGrasp::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"(
Set the frame transform as a PoseStamped_ 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
)", "transform"_a)
.def<void (SimpleUnGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"(
Set the frame transform as a PoseStamped_ in reference to a given link
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
)", "pose"_a, "link"_a)
.def<void (SimpleUnGrasp::*)(const std::string&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"(
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
The IK Frame will be placed at the base frame of this link given
as an argument.
)", "link"_a)
.def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"(
Set the maximum number of inverse kinematics solutions that
should be computed.
)", "max_ik_solutions"_a)
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
"name"_a = std::string("place generator"));
} }
} // namespace python } // namespace python
} // namespace moveit } // namespace moveit