Skip to content

Commit

Permalink
Replace SWIG bindings with Pybind11, and remove SWIG dependency (Robo…
Browse files Browse the repository at this point in the history
…tLocomotion#4949)

* hack-y first pass at using pybind11 in drake

* evil hacks to make pybind11 work

* most of the RBT tests now pass with pybind11

* autodiff types work in pybind11 wrappers

* pybind11 bindings now work for rbtree and IK tests

* work around weird IK constraint argument requirements

* add ik interfaces needed by director

* remove swig_matlab and swigmake dependencies

* move new pybind11 bindings into the bindings folder

* remove all #ifndef SWIG checks

* slightly fewer evil build hacks

* only build pybind11 bindings if pybind11 was found

* fix drake autogenerated path file

* localize build system hacks to the pybind11 folder

* fix pybind11 linking on osx

* clean up and simplify the pydrake autodiff interface

Rather than exposing raw autodiff wrappers to the user, we now provide
the pydrake.forwarddiff module, which has the following functions:

derivative(f, x)
gradient(f, x)
jacobian(f, x)

which all take a function f and an argument x and compute the relevant
quantity. The exciting part is that now that function f can include any
mix of drake code and python code, and the derivatives should still just
work. However, this feature will require some additional bindings (like
operator overloading for autodiff types) in order to be useful.

* add basic operator support to forwarddiff.py

* remove accidentally-committed file

* clean up

* more cleanup

* fix issues identified by cpplint

* rename all bindings files with pydrake prefix

* various fixes for director's pydrake IK

* comments

* make python boilerplate consistent

* attempt to avoid having to call find_package(pybind11) early

* fix linting issues

* restore default symbol visibility to fix cross-module pybind11 errors

* Use mwoehlke-kitware's automatic numpy-eigen conversions for autodiff

This substantially simplifies the python interface code and removes all
of the monkey-patching that I had previously done to make autodiff types
work naturally with numpy arrays.

* todos

* move python module suffix lookup to FindPython.cmake

* make PYTHON_MODULE_EXTENSION required

* fix cmake syntax

* only set python_module_extension on success

* the 0 of success

* rename result var for clarity

* address some comments

* try to handle finding numpy

* one more unused import

* update iris for pybind11 compatibility
  • Loading branch information
rdeits authored Feb 13, 2017
1 parent c75cfb2 commit 09bf14b
Show file tree
Hide file tree
Showing 50 changed files with 749 additions and 582 deletions.
8 changes: 1 addition & 7 deletions .gitmodules
Original file line number Diff line number Diff line change
Expand Up @@ -52,12 +52,6 @@
[submodule "externals/meshconverters"]
path = externals/meshconverters
url = https://github.com/RobotLocomotion/meshConverters.git
[submodule "externals/swigmake"]
path = externals/swigmake
url = https://github.com/rdeits/swigmake.git
[submodule "externals/swig_matlab"]
path = externals/swig_matlab
url = https://github.com/jaeandersson/swig.git
[submodule "externals/yaml_cpp"]
path = externals/yaml_cpp
url = https://github.com/jbeder/yaml-cpp.git
Expand Down Expand Up @@ -116,4 +110,4 @@
url = git://vtk.org/VTK.git
[submodule "externals/pybind11"]
path = externals/pybind11
url = https://github.com/pybind/pybind11
url = https://github.com/RobotLocomotion/pybind11
11 changes: 0 additions & 11 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ drake_add_external(qt_property_browser CMAKE QT)
drake_add_external(sedumi PUBLIC CMAKE MATLAB)
drake_add_external(spdlog PUBLIC CMAKE)
drake_add_external(spotless PUBLIC CMAKE MATLAB)
drake_add_external(swigmake PUBLIC CMAKE MATLAB PYTHON)
drake_add_external(xfoil PUBLIC CMAKE FORTRAN)
drake_add_external(yalmip PUBLIC CMAKE MATLAB)

Expand Down Expand Up @@ -175,12 +174,6 @@ drake_add_external(fcl PUBLIC CMAKE
CMAKE_ARGS -DFCL_BUILD_TESTS=OFF
DEPENDS ccd eigen octomap)

# swig_matlab
drake_add_external(swig_matlab PUBLIC AUTOTOOLS
PATCH_COMMAND ./autogen.sh
AUTOTOOLS_CONFIGURE_ARGS
--without-alllang)

# textbook
drake_add_external(textbook PUBLIC CMAKE MATLAB PYTHON
CMAKE_ARGS -DBUILD_TESTING=${Matlab_FOUND}
Expand Down Expand Up @@ -284,8 +277,6 @@ drake_add_external(drake LOCAL PUBLIC CMAKE ALWAYS MATLAB PYTHON VTK
-DWITH_SNOPT:BOOL=${WITH_SNOPT}
-DWITH_SPDLOG:BOOL=${WITH_SPDLOG}
-DWITH_SPOTLESS:BOOL=${WITH_SPOTLESS}
-DWITH_SWIG:BOOL=${WITH_SWIG_MATLAB} # TODO(jamiesnape): Fix naming inconsistency
-DWITH_SWIGMAKE:BOOL=${WITH_SWIGMAKE}
-DWITH_TEXTBOOK:BOOL=${WITH_TEXTBOOK}
-DWITH_XFOIL:BOOL=${WITH_XFOIL}
-DWITH_YALMIP:BOOL=${WITH_YALMIP}
Expand Down Expand Up @@ -321,8 +312,6 @@ drake_add_external(drake LOCAL PUBLIC CMAKE ALWAYS MATLAB PYTHON VTK
snopt
spdlog
spotless
swig_matlab
swigmake
textbook
vtk
xfoil
Expand Down
15 changes: 14 additions & 1 deletion cmake/modules/FindPython.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,19 @@ if(PYTHON_EXECUTABLE)
find_package(PythonLibs ${Python_FIND_VERSION} ${PYTHON_EXACT} MODULE ${PYTHON_QUIET})
endif()

# Look up the python module suffix (which varies by python version).
# This uses the method from FindPythonLibsNew.cmake shipped with pybind11.
execute_process(
COMMAND "${PYTHON_EXECUTABLE}" -c "from distutils import sysconfig; print(sysconfig.get_config_var('SO'))"
RESULT_VARIABLE PYTHON_MODULE_EXTENSION_RESULT
OUTPUT_VARIABLE PYTHON_MODULE_EXTENSION_OUTPUT
ERROR_VARIABLE PYTHON_MODULE_EXTENSION_ERROR_VALUE
OUTPUT_STRIP_TRAILING_WHITESPACE)

if(PYTHON_MODULE_EXTENSION_RESULT EQUAL 0)
set(PYTHON_MODULE_EXTENSION ${PYTHON_MODULE_EXTENSION_OUTPUT})
endif()

find_package_handle_standard_args(Python
REQUIRED_VARS PYTHON_EXECUTABLE PYTHON_INCLUDE_DIR PYTHON_LIBRARY
REQUIRED_VARS PYTHON_EXECUTABLE PYTHON_INCLUDE_DIR PYTHON_LIBRARY PYTHON_MODULE_EXTENSION
VERSION_VAR PYTHON_VERSION_STRING)
9 changes: 0 additions & 9 deletions cmake/options.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -303,15 +303,6 @@ macro(drake_setup_options)
drake_optional_external(SPDLOG ON
"Fast C++ text logging facility\; disabling will turn off text logging")

drake_optional_external(SWIG_MATLAB ON
DEPENDS "NOT DISABLE_MATLAB OR NOT DISABLE_PYTHON"
"A version of SWIG with MATLAB support")

drake_optional_external(SWIGMAKE ON
DEPENDS "NOT DISABLE_MATLAB OR NOT DISABLE_PYTHON\;WITH_SWIG_MATLAB"
"Helper tools to build Python & MATLAB wrappers"
"for C++ libraries with Eigen")

# END external projects that are ON by default
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# BEGIN external projects that are only needed when MATLAB is in use
Expand Down
1 change: 1 addition & 0 deletions cmake/packages.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,7 @@ macro(drake_find_packages)
drake_find_package(meshconverters CONFIG)
drake_find_package(mosek CONFIG)
drake_find_package(NLopt CONFIG)
drake_find_package(NumPy CONFIG)
drake_find_package(octomap CONFIG)
drake_find_package(pybind11 CONFIG)
drake_find_package(robotlocomotion-lcmtypes CONFIG)
Expand Down
9 changes: 7 additions & 2 deletions drake/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -173,8 +173,13 @@ if(Matlab_FOUND)
endif()
add_subdirectory(examples)

if(Matlab_FOUND OR NOT DISABLE_PYTHON)
add_subdirectory(bindings)
message("Numpy found: ${NUMPY_FOUND}")
if(pybind11_FOUND AND NOT DISABLE_PYTHON AND NUMPY_FOUND)
if (NUMPY_VERSION VERSION_LESS 1.7)
message(WARNING "Python bindings are disabled because numpy version is less than 1.7")
else()
add_subdirectory(bindings)
endif()
endif()

if(BUILD_TESTING)
Expand Down
19 changes: 2 additions & 17 deletions drake/bindings/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,17 +1,2 @@
# Look for the swig provided by our superbuild. If it doesn't exist,
# don't fall back to system swig because it may be the wrong version
# and likely does not support C++11
find_program(SWIG_EXECUTABLE swig
PATHS ${CMAKE_INSTALL_PREFIX}/bin NO_DEFAULT_PATH)
if(SWIG_EXECUTABLE)
add_subdirectory(swig)
if(NOT DISABLE_PYTHON)
add_subdirectory(python)
endif()
else()
message("
--------------------------------------------------------------------------------
*** IMPORTANT: swig not found; disabling all swig bindings. ***
--------------------------------------------------------------------------------
")
endif()
add_subdirectory(python)
add_subdirectory(pybind11)
25 changes: 25 additions & 0 deletions drake/bindings/pybind11/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
set(PYDRAKE_INSTALL_DIR lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/dist-packages/pydrake)

if(APPLE)
string(REPLACE "-Wl,-undefined -Wl,error" "" CMAKE_MODULE_LINKER_FLAGS ${CMAKE_MODULE_LINKER_FLAGS})
else()
string(REPLACE "-Wl,--no-undefined" "" CMAKE_MODULE_LINKER_FLAGS ${CMAKE_MODULE_LINKER_FLAGS})
endif()

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-shadow")
include_directories("${NUMPY_INCLUDE_DIRS}")

pybind11_add_module(_pydrake_rbtree pydrake_rbtree.cc)
target_link_libraries(_pydrake_rbtree PRIVATE drakeRBM drakeMultibodyParsers Eigen3::Eigen)
target_compile_options(_pydrake_rbtree PRIVATE "-fvisibility=default")
install(TARGETS _pydrake_rbtree DESTINATION ${PYDRAKE_INSTALL_DIR})

pybind11_add_module(_pydrake_autodiffutils pydrake_autodiffutils.cc)
target_link_libraries(_pydrake_autodiffutils PRIVATE drakeRBM drakeMultibodyParsers Eigen3::Eigen)
target_compile_options(_pydrake_autodiffutils PRIVATE "-fvisibility=default")
install(TARGETS _pydrake_autodiffutils DESTINATION ${PYDRAKE_INSTALL_DIR})

pybind11_add_module(_pydrake_ik pydrake_ik.cc)
target_link_libraries(_pydrake_ik PRIVATE drakeIK Eigen3::Eigen)
target_compile_options(_pydrake_autodiffutils PRIVATE "-fvisibility=default")
install(TARGETS _pydrake_ik DESTINATION ${PYDRAKE_INSTALL_DIR}/solvers)
13 changes: 13 additions & 0 deletions drake/bindings/pybind11/pydrake_autodiff_types.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
#include <pybind11/pybind11.h>

#include <Eigen/Core>
#include <unsupported/Eigen/AutoDiff>

typedef Eigen::AutoDiffScalar<Eigen::VectorXd> AutoDiffXd;
PYBIND11_NUMPY_OBJECT_DTYPE(AutoDiffXd);

typedef Eigen::Matrix<AutoDiffXd, Eigen::Dynamic, 1> VectorXAutoDiffXd;

typedef Eigen::Matrix<AutoDiffXd, 3, Eigen::Dynamic> Matrix3XAutoDiffXd;

typedef Eigen::Matrix<AutoDiffXd, 4, 4> Matrix44AutoDiffXd;
81 changes: 81 additions & 0 deletions drake/bindings/pybind11/pydrake_autodiffutils.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#include "drake/bindings/pybind11/pydrake_autodiff_types.h"


namespace py = pybind11;

using std::sin;
using std::cos;

/**
* Force Eigen to evaluate an autodiff expression. We need this function
* because, for example, adding two Eigen::AutoDiffXd values produces an
* Eigen::AutoDiffScalar<Eigen::CWiseBinaryOp> which cannot be returned to
* python. This just forces an evaluation and conversion to AutoDiffXd which
* would normally happen automatically in C++.
*/
template <typename Derived>
AutoDiffXd eval(const Eigen::AutoDiffScalar<Derived>& x) {
return AutoDiffXd(x.value(), x.derivatives());
}

PYBIND11_PLUGIN(_pydrake_autodiffutils) {
py::module m("_pydrake_autodiffutils", "Bindings for Eigen AutoDiff Scalars");

py::class_<AutoDiffXd>(m, "AutoDiffXd")
.def("__init__",
[](AutoDiffXd& self,
double value,
const Eigen::VectorXd& derivatives) {
new (&self) AutoDiffXd(value, derivatives);
})
.def("value", [](const AutoDiffXd& self) {
return self.value();
})
.def("derivatives", [](const AutoDiffXd& self) {
return self.derivatives();
})
.def("sin", [](const AutoDiffXd& self) { return eval(sin(self)); })
.def("cos", [](const AutoDiffXd& self) { return eval(cos(self)); })
.def("__add__", [](const AutoDiffXd& self, const AutoDiffXd& other) {
return eval(self + other);
})
.def("__add__", [](const AutoDiffXd& self, double other) {
return eval(self + other);
})
.def("__radd__", [](const AutoDiffXd& self, double other) {
return eval(other + self);
})
.def("__sub__", [](const AutoDiffXd& self, const AutoDiffXd& other) {
return eval(self - other);
})
.def("__sub__", [](const AutoDiffXd& self, double other) {
return eval(self - other);
})
.def("__rsub__", [](const AutoDiffXd& self, double other) {
return eval(other - self);
})
.def("__mul__", [](const AutoDiffXd& self, const AutoDiffXd& other) {
return eval(self * other);
})
.def("__mul__", [](const AutoDiffXd& self, double other) {
return eval(self * other);
})
.def("__rmul__", [](const AutoDiffXd& self, double other) {
return eval(other * self);
})
.def("__truediv__", [](const AutoDiffXd& self, const AutoDiffXd& other) {
return eval(self / other);
})
.def("__truediv__", [](const AutoDiffXd& self, double other) {
return eval(self / other);
})
.def("__rtruediv__", [](const AutoDiffXd& self, double other) {
return eval(other / self);
});

return m.ptr();
}
Loading

0 comments on commit 09bf14b

Please sign in to comment.