Python bindings for moveit_core (moveit#2547)

Provide python bindings for basic classes of moveit_core: - collision_detection: Contact, CollisionRequest, CollisionResult, AllowedCollisionMatrix - kinematic_constraints: ConstraintEvaluationResult, KinematicConstraintSet, constructGoalConstraints - planning_scene: PlanningScene, - robot_model: RobotModel, JointModelGroup - robot_state: RobotState - transforms: Transforms
UM-ARM-Lab · May 8, 2021 · 97dcd93 · 97dcd93
1 parent 46f1104
commit 97dcd93
Show file tree

Hide file tree

Showing 29 changed files with 1,227 additions and 0 deletions.
diff --git a/.gitignore b/.gitignore
@@ -58,3 +58,6 @@ CATKIN_IGNORE
 .moveit_ci
 
 *.pyc
+
+_build
+_autosummary
diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
@@ -69,8 +69,11 @@ COMPONENTS
   urdf
   visualization_msgs
   xmlrpcpp
+  pybind11_catkin
 )
 
+catkin_python_setup()
+
 set(VERSION_FILE_PATH "${CATKIN_DEVEL_PREFIX}/include")
 # Pass the folder of the generated version.h to catkin_package() for export in devel-space
 # This is how gencpp adds the folder of generated message code to the include dirs, see:
@@ -101,6 +104,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     planning_request_adapter/include
     planning_scene/include
     profiler/include
+    python/tools/include
     sensor_manager/include
     trajectory_processing/include
     utils/include
@@ -125,6 +129,7 @@ catkin_package(
     moveit_constraint_samplers
     moveit_planning_request_adapter
     moveit_profiler
+    moveit_python_tools
     moveit_trajectory_processing
     moveit_distance_field
     moveit_collision_distance_field
@@ -219,3 +224,29 @@ add_subdirectory(distance_field)
 add_subdirectory(collision_distance_field)
 add_subdirectory(kinematics_metrics)
 add_subdirectory(dynamics_solver)
+
+add_subdirectory(python)
+set(pymoveit_libs
+  moveit_collision_detection
+  moveit_kinematic_constraints
+  moveit_planning_scene
+  moveit_python_tools
+  moveit_robot_model
+  moveit_robot_state
+  moveit_transforms
+)
+
+pybind_add_module(pymoveit_core
+    python/pymoveit_core.cpp
+    collision_detection/src/pycollision_detection.cpp
+    robot_model/src/pyrobot_model.cpp
+    robot_state/src/pyrobot_state.cpp
+    transforms/src/pytransforms.cpp
+    planning_scene/src/pyplanning_scene.cpp
+    kinematic_constraints/src/pykinematic_constraint.cpp
+)
+target_include_directories(pymoveit_core SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
+target_link_libraries(pymoveit_core PRIVATE ${pymoveit_libs} ${catkin_LIBRARIES})
+
+#catkin_lint: ignore_once undefined_target (pymoveit_core is defined by pybind_add_module)
+install(TARGETS pymoveit_core LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})
diff --git a/moveit_core/collision_detection/src/pycollision_detection.cpp b/moveit_core/collision_detection/src/pycollision_detection.cpp
@@ -0,0 +1,104 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Peter Mitrano */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/eigen.h>
+#include <moveit/collision_detection/world.h>
+#include <moveit/collision_detection/collision_matrix.h>
+#include <moveit/collision_detection/collision_common.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+namespace py = pybind11;
+
+using namespace collision_detection;
+
+void def_collision_detection_bindings(py::module& m)
+{
+  m.doc() = "contains collision detection, the world, and allowed collision matrices";
+  py::enum_<BodyType>(m, "BodyType")
+      .value("ROBOT_ATTACHED", BodyType::ROBOT_ATTACHED)
+      .value("ROBOT_LINK", BodyType::ROBOT_LINK)
+      .value("WORLD_OBJECT", BodyType::WORLD_OBJECT)
+      .export_values();
+  py::class_<Contact>(m, "Contact")
+      .def(py::init<>())
+      .def_readwrite("body_name_1", &Contact::body_name_1)
+      .def_readwrite("body_name_2", &Contact::body_name_2)
+      .def_readwrite("body_type_1", &Contact::body_type_1)
+      .def_readwrite("body_type_2", &Contact::body_type_2)
+      .def_readwrite("depth", &Contact::depth)
+      .def_property_readonly("nearest_points",
+                             [](const Contact& contact) {
+                               std::vector<Eigen::Vector3d> v{ contact.nearest_points[0], contact.nearest_points[1] };
+                               return v;
+                             })
+      .def_readwrite("normal", &Contact::normal)
+      .def_readwrite("percent_interpolation", &Contact::percent_interpolation)
+      .def_readwrite("pos", &Contact::pos)
+      //
+      ;
+  py::class_<CollisionRequest>(m, "CollisionRequest")
+      .def(py::init<>())
+      .def_readwrite("contacts", &CollisionRequest::contacts)
+      .def_readwrite("cost", &CollisionRequest::cost)
+      .def_readwrite("distance", &CollisionRequest::distance)
+      .def_readwrite("group_name", &CollisionRequest::group_name)
+      .def_readwrite("is_done", &CollisionRequest::is_done)
+      .def_readwrite("max_contacts", &CollisionRequest::max_contacts)
+      .def_readwrite("max_contacts_per_pair", &CollisionRequest::max_contacts_per_pair)
+      .def_readwrite("max_cost_sources", &CollisionRequest::max_cost_sources)
+      .def_readwrite("verbose", &CollisionRequest::verbose)
+      //
+      ;
+  py::class_<CollisionResult>(m, "CollisionResult")
+      .def(py::init<>())
+      .def_readwrite("collision", &CollisionResult::collision)
+      .def_readwrite("contact_count", &CollisionResult::contact_count)
+      .def_readwrite("contacts", &CollisionResult::contacts)
+      .def_readwrite("cost_sources", &CollisionResult::cost_sources)
+      .def_readwrite("distance", &CollisionResult::distance)
+      .def("clear", &CollisionResult::clear)
+      //
+      ;
+  py::class_<AllowedCollisionMatrix>(m, "AllowedCollisionMatrix")
+      .def(py::init<>())
+      .def("setEntry",
+           py::overload_cast<const std::string&, const std::string&, bool>(&AllowedCollisionMatrix::setEntry))
+      //
+      ;
+  py::class_<World, WorldPtr>(m, "World").def(py::init<>());
+}
diff --git a/moveit_core/doc/_templates/custom-class-template.rst b/moveit_core/doc/_templates/custom-class-template.rst
@@ -0,0 +1,34 @@
+{{ fullname | escape | underline}}
+
+.. currentmodule:: {{ module }}
+
+.. autoclass:: {{ objname }}
+   :members:
+   :show-inheritance:
+   :inherited-members:
+   :special-members: __call__, __add__, __mul__
+
+   {% block methods %}
+   {% if methods %}
+   .. rubric:: {{ _('Methods') }}
+
+   .. autosummary::
+      :nosignatures:
+   {% for item in methods %}
+      {%- if not item.startswith('_') %}
+      ~{{ name }}.{{ item }}
+      {%- endif -%}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block attributes %}
+   {% if attributes %}
+   .. rubric:: {{ _('Attributes') }}
+
+   .. autosummary::
+   {% for item in attributes %}
+      ~{{ name }}.{{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
diff --git a/moveit_core/doc/_templates/custom-module-template.rst b/moveit_core/doc/_templates/custom-module-template.rst
@@ -0,0 +1,66 @@
+{{ fullname | escape | underline}}
+
+.. automodule:: {{ fullname }}
+
+   {% block attributes %}
+   {% if attributes %}
+   .. rubric:: Module attributes
+
+   .. autosummary::
+      :toctree:
+   {% for item in attributes %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block functions %}
+   {% if functions %}
+   .. rubric:: {{ _('Functions') }}
+
+   .. autosummary::
+      :toctree:
+      :nosignatures:
+   {% for item in functions %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block classes %}
+   {% if classes %}
+   .. rubric:: {{ _('Classes') }}
+
+   .. autosummary::
+      :toctree:
+      :template: custom-class-template.rst
+      :nosignatures:
+   {% for item in classes %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block exceptions %}
+   {% if exceptions %}
+   .. rubric:: {{ _('Exceptions') }}
+
+   .. autosummary::
+      :toctree:
+   {% for item in exceptions %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+{% block modules %}
+{% if modules %}
+.. autosummary::
+   :toctree:
+   :template: custom-module-template.rst
+   :recursive:
+{% for item in modules %}
+   {{ item }}
+{%- endfor %}
+{% endif %}
+{% endblock %}
diff --git a/moveit_core/doc/api.rst b/moveit_core/doc/api.rst
@@ -0,0 +1,6 @@
+.. autosummary::
+   :toctree: _autosummary
+   :template: custom-module-template.rst
+   :recursive:
+
+   moveit.core