Pybind11 manual bindings

python/CMakeLists.txt

@@ -0,0 +1,117 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(aikidopy)
+set(aikidopy_VERSION 0.1.0)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+if(NOT AIKIDOPY_PYTHON_VERSION)
+  set(AIKIDOPY_PYTHON_VERSION 3.4 CACHE STRING "Choose the target Python version (e.g., 3.4, 2.7)" FORCE)
+endif()
+
+find_package(PythonInterp ${AIKIDOPY_PYTHON_VERSION} REQUIRED)
+
+execute_process(COMMAND ${PYTHON_EXECUTABLE} -c
+  "from distutils.sysconfig import get_python_lib;\
+  print(get_python_lib(plat_specific=True, prefix=''))"
+  OUTPUT_VARIABLE PYTHON_SITE_PACKAGES
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+)
+if(NOT PythonInterp_FOUND)
+  message(STATUS "BUILD_PYTHON is ON, but failed to find PythonInterp. "
+    "Disabling aikidopy."
+  )
+  return()
+endif()
+
+find_package(PythonLibs ${AIKIDOPY_PYTHON_VERSION} QUIET)
+if(NOT PythonLibs_FOUND)
+  message(STATUS "BUILD_PYTHON is ON, but failed to find PythonLibs. "
+    "Disabling aikidopy."
+  )
+  return()
+endif()
+
+# Find pybind11
+# Needs to set PYBIND11_PYTHON_VERSION before finding pybind11
+set(PYBIND11_PYTHON_VERSION ${AIKIDOPY_PYTHON_VERSION})
+find_package(pybind11 2.2.0 REQUIRED)
+if(NOT pybind11_FOUND)
+  message(STATUS "BUILD_PYTHON is ON, but failed to find pybind11 >= "
+    "2.2.0. Disabling aikidopy."
+  )
+  return()
+endif()
+
+#================================================================================
+# Dependencies
+#
+
+include(ExternalProject)
+
+find_package(DART 6.7.2 REQUIRED
+  COMPONENTS utils utils-urdf optimizer-nlopt
+)
+
+find_package(aikido 0.3.0 REQUIRED
+  COMPONENTS
+    common
+    constraint
+    io
+    planner
+    robot
+    statespace
+    rviz
+)
+
+#================================================================================
+
+set(aikidopy_headers
+  eigen_geometry_pybind.h
+  eigen_pybind.h
+)
+set(aikidopy_sources
+  eigen_geometry_pybind.cpp
+  aikidopy.cpp
+  aikido_py.cpp
+  dart_py.cpp
+)
+
+# find Eigen
+find_package(Eigen3 REQUIRED NO_MODULE)
+
+# Build a Python extension module:
+# pybind11_add_module(<name> [MODULE | SHARED] [EXCLUDE_FROM_ALL]
+#                     [NO_EXTRAS] [SYSTEM] [THIN_LTO] source1 [source2 ...])
+#
+pybind11_add_module(aikidopy
+  MODULE
+  ${aikidopy_headers}
+  ${aikidopy_sources}
+)
+
+target_include_directories(aikidopy
+  SYSTEM PUBLIC
+    ${PYTHON_INCLUDE_DIRS}
+    ${pybind11_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${aikido_INCLUDE_DIRS}
+  PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(aikidopy
+  PUBLIC
+    ${aikido_LIBRARIES}
+    ${PYTHON_LIBRARIES}
+)
+
+set_target_properties(aikidopy
+  PROPERTIES
+    PREFIX ""
+    SUFFIX ".so"  # python uses '.so' extension even on macOS
+    DEBUG_POSTFIX ""
+)
+
+install(TARGETS aikidopy
+  LIBRARY DESTINATION "${PYTHON_SITE_PACKAGES}"
+)

python/README.md

@@ -0,0 +1,56 @@
+# This folder contains manual bindings for aikidopy using pybind11.
+
+0. The installation guide have been tested on
+
+- sudo apt installed python 2.7 and python 3.4.
+- anaconda managed python 3.6
+
+If you use system installed python, ensure you have `python2.7-dev` or `python3-dev` correspondingly, or run `sudo apt-get install python3-dev` to obtain it.
+
+
+1. Install [pybind11](https://github.com/pybind/pybind11.git) **from source** following this [instruction](https://pybind11.readthedocs.io/en/master/basics.html#compiling-the-test-cases). (Need version >= 2.2.0).
+
+```
+git clone https://github.com/pybind/pybind11.git
+cd pybind11
+mkdir build
+cd build
+cmake ..
+make -j 4
+```
+
+- For system managed python: `sudo make install`;
+- For anaconda: `pip install -e .`.
+
+You should be able to load `pybind11` in your python.
+
+```
+$ python
+>> import pybind11
+```
+
+2. Build aikido and source the setup file.
+```
+catkin build aikido
+source ../../devel.setup.bash
+```
+
+3. Create a folder `workspace/src/aikido/python/build` (instead of `workspace/src/aikido/build`). Build and install the python binding.
+
+```
+cd build
+cmake .. -DAIKIDOPY_PYTHON_VERSION=3.4
+make -j
+sudo make install
+```
+
+Read the output of `sudo make install` and ensure that `aikidopy.so` get installed in desired location.
+
+4. Try loading.
+```
+python
+>>> import aikidopy
+```
+
+5. Tips for multiple python versions
+- When you have multiple python environment, ensure that `which python` and the python version you passed to cmake refers to the same python.

python/aikido_py.cpp

@@ -0,0 +1,122 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <memory>
+#include <aikido/constraint/dart/CollisionFree.hpp>
+#include <aikido/robot/ConcreteManipulator.hpp>
+#include <aikido/constraint/dart/TSR.hpp>
+#include <aikido/io/CatkinResourceRetriever.hpp>
+#include <aikido/rviz/WorldInteractiveMarkerViewer.hpp>
+#include <aikido/rviz/TSRMarker.hpp>
+#include <aikido/planner/World.hpp>
+#include <dart/dart.hpp>
+#include <dart/utils/urdf/DartLoader.hpp>
+
+
+namespace py = pybind11;
+
+Eigen::Isometry3d vectorToIsometry(std::vector<double>& poseVector)
+{
+  double* ptr = &poseVector[0];
+  Eigen::Map<Eigen::VectorXd> p(ptr, 7);
+
+  Eigen::Isometry3d pose(Eigen::Isometry3d::Identity());
+  pose.translation() = p.head(3);
+  Eigen::Quaterniond q(p[3], p[4], p[5], p[6]);
+  pose.linear() = Eigen::Matrix3d(q);
+  return pose;
+}
+
+namespace aikido {
+namespace python {
+
+
+void CollisionFree(pybind11::module& m)
+{
+  py::class_<aikido::constraint::dart::CollisionFree, std::shared_ptr<aikido::constraint::dart::CollisionFree>>(m, "CollisionFree");
+}
+
+void TSR(pybind11::module& m)
+{
+  py::class_<aikido::constraint::dart::TSR, std::shared_ptr<aikido::constraint::dart::TSR>>(m, "TSR")
+  .def("setPose",
+    [](aikido::constraint::dart::TSR* self,
+          std::vector<double> pose) // x, y, z, qw, qx, qy, qz)
+    {
+      self->mT0_w = vectorToIsometry(pose);
+    })
+  .def("multiplyToEndEffectorTransform",
+    [](aikido::constraint::dart::TSR* self,
+        Eigen::Isometry3d endEffectorTransform)
+    {
+      self->mTw_e.matrix() *= endEffectorTransform.matrix();
+    })
+  .def("getPose",
+    [](aikido::constraint::dart::TSR* self)
+    -> Eigen::Isometry3d
+    {
+      return self->mT0_w;
+    })
+  .def("getEndEffectorTransform",
+    [](aikido::constraint::dart::TSR* self)
+    -> Eigen::Isometry3d
+    {
+      return self->mTw_e;
+    });
+}
+
+void ConcreteManipulator(pybind11::module& m)
+{
+  py::class_<aikido::robot::ConcreteManipulator, std::shared_ptr<aikido::robot::ConcreteManipulator>>(m, "ConcreteManipulator")
+      .def("getName",
+           [](aikido::robot::ConcreteManipulator* self) -> std::string { return self->getName(); });
+}
+
+void WorldInteractiveMarkerViewer(pybind11::module& m)
+{
+  py::class_<aikido::rviz::WorldInteractiveMarkerViewer, std::shared_ptr<aikido::rviz::WorldInteractiveMarkerViewer>>(m, "WorldInteractiveMarkerViewer")
+      .def("addTSRMarker",
+        [](aikido::rviz::WorldInteractiveMarkerViewer* self,
+          std::shared_ptr<aikido::constraint::dart::TSR> tsr)
+        -> aikido::rviz::TSRMarkerPtr
+        {
+          return self->addTSRMarker(*tsr.get());
+        });
+
+}
+
+void TSRMarker(pybind11::module& m)
+{
+  py::class_<aikido::rviz::TSRMarker, std::shared_ptr<aikido::rviz::TSRMarker>>(m, "TSRMarker");
+}
+
+
+void World(pybind11::module& m)
+{
+  py::class_<aikido::planner::World, std::shared_ptr<aikido::planner::World>>(m, "World")
+      .def("addBodyFromURDF",
+        [](aikido::planner::World* self,
+          const std::string& uri,
+          std::vector<double> objectPose) // x, y, z, qw, qx, qy, qz)
+        -> std::shared_ptr<::dart::dynamics::Skeleton>
+        {
+          auto transform = vectorToIsometry(objectPose);
+
+          dart::utils::DartLoader urdfLoader;
+          const auto resourceRetriever
+              = std::make_shared<aikido::io::CatkinResourceRetriever>();
+          const auto skeleton = urdfLoader.parseSkeleton(uri, resourceRetriever);
+
+          if (!skeleton)
+            throw std::runtime_error("unable to load '" + uri + "'");
+
+          dynamic_cast<dart::dynamics::FreeJoint*>(skeleton->getJoint(0))
+              ->setTransform(transform);
+
+          self->addSkeleton(skeleton);
+          return skeleton;
+        }
+        );
+}
+
+} // namespace python
+} // namespace aikido

python/aikidopy.cpp

@@ -0,0 +1,55 @@
+#include <pybind11/pybind11.h>
+
+namespace py = pybind11;
+
+namespace aikido {
+namespace python {
+
+void eigen_geometry(py::module& m);
+
+// DART
+void Skeleton(py::module& m);
+
+void MetaSkeleton(py::module& m);
+
+// AIKIDO
+void CollisionFree(py::module& m);
+
+void ConcreteManipulator(py::module& m);
+
+void TSR(py::module& m);
+
+void WorldInteractiveMarkerViewer(py::module& m);
+
+void TSRMarker(py::module& m);
+
+void World(py::module& m);
+
+PYBIND11_MODULE(aikidopy, m)
+{
+  py::module::import("numpy");
+
+  m.doc() = "aikido python bindings";
+
+  eigen_geometry(m);
+
+  CollisionFree(m);
+
+  ConcreteManipulator(m);
+
+  TSR(m);
+
+  WorldInteractiveMarkerViewer(m);
+
+  TSRMarker(m);
+
+  World(m);
+
+  // DART
+  MetaSkeleton(m);
+
+  Skeleton(m);
+}
+
+} // namespace python
+} // namespace aikido

python/dart_py.cpp

@@ -0,0 +1,27 @@
+#include <pybind11/pybind11.h>
+#include <memory>
+
+#include <dart/dynamics/dynamics.hpp>
+
+namespace py = pybind11;
+
+namespace aikido {
+namespace python {
+
+void MetaSkeleton(pybind11::module& m)
+{
+  py::class_<dart::dynamics::MetaSkeleton, std::shared_ptr<dart::dynamics::MetaSkeleton>>(m, "MetaSkeleton")
+      .def("getName",
+           [](dart::dynamics::MetaSkeleton* self) -> std::string { return self->getName(); });
+}
+
+void Skeleton(pybind11::module& m)
+{
+  py::class_<dart::dynamics::Skeleton, std::shared_ptr<dart::dynamics::Skeleton>>(m, "Skeleton")
+      .def("getName",
+           [](dart::dynamics::Skeleton* self) -> std::string { return self->getName(); });
+}
+
+
+} // namespace python
+} // namespace aikido