diff --git a/.ci/install_linux_catkin.sh b/.ci/install_linux_catkin.sh index 8851e1d9b9..84cc768dda 100755 --- a/.ci/install_linux_catkin.sh +++ b/.ci/install_linux_catkin.sh @@ -9,3 +9,26 @@ cp -r "${TRAVIS_BUILD_DIR}" src if [ $BUILD_NAME = TRUSTY_FULL_DEBUG ]; then sudo apt-get install -y clang-format-3.8 fi + +if [ "$BUILD_AIKIDOPY" = "ON" ]; then + $SUDO apt-get -y install python3-dev python3-numpy + $SUDO apt-get -y install python3-pip -y + $SUDO pip3 install pytest -U + + if [ $(lsb_release -sc) = "trusty" ] || [ $(lsb_release -sc) = "xenial" ] || [ $(lsb_release -sc) = "bionic" ]; then + git clone https://github.com/pybind/pybind11 -b 'v2.3.0' --single-branch --depth 1 + cd pybind11 + mkdir build + cd build + cmake .. -DCMAKE_BUILD_TYPE=Release -DPYBIND11_TEST=OFF + make -j4 + $SUDO make install + cd ../.. + elif [ $(lsb_release -sc) = "cosmic" ] || [ $(lsb_release -sc) = "disco" ]; then + $SUDO apt-get -y install pybind11-dev python3 libpython3-dev python3-pytest \ + python3-distutils + else + echo -e "$(lsb_release -sc) is not supported." + exit 1 + fi +fi diff --git a/.ci/script_catkin.sh b/.ci/script_catkin.sh index ef44fec10d..b0833f9a45 100755 --- a/.ci/script_catkin.sh +++ b/.ci/script_catkin.sh @@ -17,6 +17,13 @@ if [ $BUILD_NAME = TRUSTY_FULL_DEBUG ]; then ./scripts/internal-run.sh catkin build --no-status --no-deps -p 1 -i --make-args check-format -- aikido fi +# Build aikidopy and pytest +if [ $BUILD_AIKIDOPY = ON ]; then + ./scripts/internal-run.sh catkin build --no-status --no-deps -p 1 -i --cmake-args -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DTREAT_WARNINGS_AS_ERRORS=OFF -DCODECOV=OFF --make-args aikidopy -- aikido + ./scripts/internal-run.sh make -C build/aikido pytest + exit 0 +fi + # Manually build Aikido's tests; they are not built automatically because it is not a Catkin package. if [ $BUILD_NAME = TRUSTY_FULL_DEBUG ]; then ./scripts/internal-run.sh catkin build --no-status --no-deps -p 1 -i --cmake-args -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DTREAT_WARNINGS_AS_ERRORS=ON -DCODECOV=ON --make-args tests -- aikido diff --git a/.ci/script_cmake.sh b/.ci/script_cmake.sh index 199a633b82..63367f8234 100755 --- a/.ci/script_cmake.sh +++ b/.ci/script_cmake.sh @@ -6,6 +6,13 @@ unset -f cd; # Disable rvm override of cd (see https://github.com/travis-ci/trav mkdir build cd build -cmake -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DTREAT_WARNINGS_AS_ERRORS=ON .. -make -j4 tests -make test + +if [ $BUILD_AIKIDOPY = ON ]; then + cmake -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DTREAT_WARNINGS_AS_ERRORS=ON -DBUILD_AIKIDOPY=ON .. + make -j4 aikidopy + make pytest +else + cmake -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DTREAT_WARNINGS_AS_ERRORS=ON .. + make -j4 tests + make test +fi \ No newline at end of file diff --git a/.travis.yml b/.travis.yml index 04bef241fb..3a67a11dc4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -83,6 +83,25 @@ matrix: - USE_CATKIN=OFF services: docker + - os: linux + env: + - BUILD_NAME=BIONIC_CATKIN_FULL_RELEASE_AIKIDOPY + - DOCKER_FILE="ubuntu-bionic" + - BUILD_TYPE=Release + - CATKIN_CONFIG_OPTIONS="-DCMAKE_BUILD_TYPE=Release -DTREAT_WARNINGS_AS_ERRORS=ON -DBUILD_AIKIDOPY=ON" + - USE_CATKIN=ON + - BUILD_AIKIDOPY=ON + services: docker + + - os: linux + env: + - BUILD_NAME=BIONIC_CMAKE_RELEASE_AIKIDOPY + - DOCKER_FILE="ubuntu-bionic" + - BUILD_TYPE=Release + - USE_CATKIN=OFF + - BUILD_AIKIDOPY=ON + services: docker + - os: osx osx_image: xcode11 compiler: clang @@ -150,6 +169,14 @@ matrix: - USE_CATKIN=ON services: docker + - os: osx + osx_image: xcode11 + compiler: clang + env: + - BUILD_NAME=XCODE11_CMAKE_RELEASE + - BUILD_TYPE=Release + - USE_CATKIN=OFF + before_install: - if [ -n "$DOCKER_FILE" ]; then docker build -t "$DOCKER_FILE" -f ".ci/docker/$DOCKER_FILE" .; diff --git a/CMakeLists.txt b/CMakeLists.txt index bf4d381444..12205897e5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,6 +36,11 @@ set(CONFIG_INSTALL_DIR "${LIBRARY_INSTALL_DIR}/${PROJECT_NAME}/cmake") option(CODECOV "Enable codecov support" OFF) option(DOWNLOAD_TAGFILES "Download Doxygen tagfiles for dependencies" OFF) option(TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) +option(BUILD_AIKIDOPY "Build aikidopy (the python binding)" OFF) + +if(BUILD_AIKIDOPY) + set(BUILD_SHARED_LIBS OFF) +endif() #============================================================================== # codecov Setup @@ -134,6 +139,8 @@ add_custom_target(headers SOURCES ${aikido_headers}) add_subdirectory("src") +add_subdirectory("python") + enable_testing() add_subdirectory("tests" EXCLUDE_FROM_ALL) diff --git a/include/aikido/robot.hpp b/include/aikido/robot.hpp new file mode 100644 index 0000000000..8b2b8329f3 --- /dev/null +++ b/include/aikido/robot.hpp @@ -0,0 +1,7 @@ +#include "robot/ConcreteManipulator.hpp" +#include "robot/ConcreteRobot.hpp" +#include "robot/GrabMetadata.hpp" +#include "robot/Hand.hpp" +#include "robot/Manipulator.hpp" +#include "robot/Robot.hpp" +#include "robot/util.hpp" diff --git a/include/aikido/rviz.hpp b/include/aikido/rviz.hpp index 158767645e..dabb61eb54 100644 --- a/include/aikido/rviz.hpp +++ b/include/aikido/rviz.hpp @@ -1,10 +1,10 @@ #include "rviz/BodyNodeMarker.hpp" #include "rviz/FrameMarker.hpp" #include "rviz/InteractiveMarkerViewer.hpp" +#include "rviz/pointers.hpp" #include "rviz/ResourceServer.hpp" +#include "rviz/shape_conversions.hpp" #include "rviz/ShapeFrameMarker.hpp" #include "rviz/SkeletonMarker.hpp" -#include "rviz/SmartPointers.hpp" #include "rviz/TSRMarker.hpp" #include "rviz/WorldInteractiveMarkerViewer.hpp" -#include "rviz/shape_conversions.hpp" diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000000..b3eab64ee9 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,4 @@ +set(AIKIDOPY_BUILD_DIR "${CMAKE_CURRENT_BINARY_DIR}/aikidopy") + +add_subdirectory(aikidopy) +add_subdirectory(tests) diff --git a/python/README.md b/python/README.md new file mode 100644 index 0000000000..806eb42190 --- /dev/null +++ b/python/README.md @@ -0,0 +1,55 @@ +# aikidopy + +This folder contains manual bindings for aikidopy using pybind11. The installation guide have been tested on +- sudo apt installed Python 2.7 and Python 3.4. +- anaconda managed Python 3.6 + +0. If you use system installed Python, ensure you have `python2.7-dev` or `python3-dev` correspondingly. Run `$ sudo apt-get install python3-dev` to obtain it. + + +1. Install [pybind11](https://github.com/pybind/pybind11.git) **from source** following these [instructions](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 workspace/devel/setup.bash +``` + +3. Create a folder `workspace/src/aikido/python/build` (instead of `workspace/src/aikido/build`). Build and install the python binding, passing the desired python version using `-DAIKIDOPY_PYTHON_VERSION`. + +``` +$ cd build +$ cmake .. -DAIKIDOPY_PYTHON_VERSION=2.7 # or 3.4, 3.6 etc +$ make -j +$ sudo make install +``` + +Read the output of `sudo make install` and ensure that `aikidopy.so` get installed in the python library path (i.e. `/usr/lib/python2.7/dist-packages/aikidopy.so` or `anaconda3/envs/YOUR_ENV/lib/python3.6/site-packages/aikidopy.so`). + +4. Try loading. +``` +$ python +>>> import aikidopy +``` + +## Tips +- If you have multiple python environment, ensure that `which python` and the python version you passed to cmake refers to the same python. diff --git a/python/aikidopy/CMakeLists.txt b/python/aikidopy/CMakeLists.txt new file mode 100644 index 0000000000..bc74db28cd --- /dev/null +++ b/python/aikidopy/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 2.8.12) + +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_AIKIDOPY 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 QUIET) +if(NOT pybind11_FOUND) + message(STATUS "BUILD_PYTHON is ON, but failed to find pybind11 >= " + "2.2.0. Disabling aikidopy." + ) + return() +endif() + +#================================================================================ +# Dependencies +# + +#================================================================================ + +file(GLOB_RECURSE aikidopy_headers "*.h" "*.hpp") +file(GLOB_RECURSE aikidopy_sources "*.cpp") + +# Build a Python extension module: +# pybind11_add_module( [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} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(aikidopy + PUBLIC + aikido_common + aikido_constraint + aikido_control + aikido_distance + aikido_io + aikido_planner + aikido_robot + aikido_statespace + aikido_trajectory + ${PYTHON_LIBRARIES} +) +if(TARGET aikido_control_ros) + target_link_libraries(aikidopy PUBLIC aikido_control_ros) +endif() +if(TARGET aikido_perception) + target_link_libraries(aikidopy PUBLIC aikido_perception) +endif() +if(TARGET aikido_rviz) + target_link_libraries(aikidopy PUBLIC aikido_rviz) +endif() + +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}" +) + +clang_format_add_sources(${aikidopy_headers} ${aikidopy_sources}) diff --git a/python/aikidopy/aikidopy.cpp b/python/aikidopy/aikidopy.cpp new file mode 100644 index 0000000000..70f4f58bf5 --- /dev/null +++ b/python/aikidopy/aikidopy.cpp @@ -0,0 +1,37 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void eigen_geometry(py::module& m); + +void aikidopy_common(py::module& m); +void aikidopy_statespace(py::module& m); +void aikidopy_constraint(py::module& m); +void aikidopy_planner(py::module& m); +void aikidopy_robot(py::module& m); +#ifdef AIKIDO_HAS_RVIZ +void aikidopy_rviz(py::module& m); +#endif // AIKIDO_HAS_RVIZ + +PYBIND11_MODULE(aikidopy, m) +{ + py::module::import("numpy"); + + m.doc() = "aikidopy is a Python library for solving robotic motion planning " + "and decision making problems."; + + aikidopy_common(m); + aikidopy_statespace(m); + aikidopy_constraint(m); + aikidopy_planner(m); + aikidopy_robot(m); +#ifdef AIKIDO_HAS_RVIZ + aikidopy_rviz(m); +#endif // AIKIDO_HAS_RVIZ +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/common/module.cpp b/python/aikidopy/common/module.cpp new file mode 100644 index 0000000000..8e001313b5 --- /dev/null +++ b/python/aikidopy/common/module.cpp @@ -0,0 +1,14 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void aikidopy_common(py::module& m) +{ + auto sm = m.def_submodule("common"); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/constraint/dart/CollisionFree.cpp b/python/aikidopy/constraint/dart/CollisionFree.cpp new file mode 100644 index 0000000000..5e445a89c4 --- /dev/null +++ b/python/aikidopy/constraint/dart/CollisionFree.cpp @@ -0,0 +1,16 @@ +#include +#include +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void CollisionFree(py::module& m) +{ + py::class_>(m, "CollisionFree"); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/constraint/dart/TSR.cpp b/python/aikidopy/constraint/dart/TSR.cpp new file mode 100644 index 0000000000..bf9e8c4b0d --- /dev/null +++ b/python/aikidopy/constraint/dart/TSR.cpp @@ -0,0 +1,41 @@ +#include +#include +#include +#include "utils.hpp" + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void TSR(py::module& m) +{ + py::class_>(m, "TSR") + .def("setPose", + [](aikido::constraint::dart::TSR* self, + std::vector 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; + }); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/constraint/dart/module.cpp b/python/aikidopy/constraint/dart/module.cpp new file mode 100644 index 0000000000..33c65a3be5 --- /dev/null +++ b/python/aikidopy/constraint/dart/module.cpp @@ -0,0 +1,20 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void CollisionFree(py::module& sm); +void TSR(py::module& sm); + +void aikidopy_constraint_dart(py::module& m) +{ + auto sm = m.def_submodule("dart"); + + CollisionFree(sm); + TSR(sm); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/constraint/module.cpp b/python/aikidopy/constraint/module.cpp new file mode 100644 index 0000000000..242b44a7f7 --- /dev/null +++ b/python/aikidopy/constraint/module.cpp @@ -0,0 +1,18 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void aikidopy_constraint_dart(py::module& sm); + +void aikidopy_constraint(py::module& m) +{ + auto sm = m.def_submodule("constraint"); + + aikidopy_constraint_dart(sm); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/planner/World.cpp b/python/aikidopy/planner/World.cpp new file mode 100644 index 0000000000..cd62f694cc --- /dev/null +++ b/python/aikidopy/planner/World.cpp @@ -0,0 +1,41 @@ +#include +#include +#include +#include +#include "utils.hpp" + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void World(py::module& m) +{ + py::class_>(m, "World") + .def("addBodyFromURDF", + [](aikido::planner::World* self, + const std::string& uri, + std::vector 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(); + const auto skeleton = urdfLoader.parseSkeleton(uri, resourceRetriever); + + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); + + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); + + self->addSkeleton(skeleton); + return skeleton; + } + ); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/planner/module.cpp b/python/aikidopy/planner/module.cpp new file mode 100644 index 0000000000..6c5cb37183 --- /dev/null +++ b/python/aikidopy/planner/module.cpp @@ -0,0 +1,18 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void World(py::module& sm); + +void aikidopy_planner(py::module& m) +{ + auto sm = m.def_submodule("planner"); + + World(sm); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/robot/ConcreteManipulator.cpp b/python/aikidopy/robot/ConcreteManipulator.cpp new file mode 100644 index 0000000000..a7adc5f4bb --- /dev/null +++ b/python/aikidopy/robot/ConcreteManipulator.cpp @@ -0,0 +1,17 @@ +#include +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void ConcreteManipulator(py::module& m) +{ + py::class_>(m, "ConcreteManipulator") + .def("getName", + [](aikido::robot::ConcreteManipulator* self) -> std::string { return self->getName(); }); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/robot/module.cpp b/python/aikidopy/robot/module.cpp new file mode 100644 index 0000000000..b4c1b0cff6 --- /dev/null +++ b/python/aikidopy/robot/module.cpp @@ -0,0 +1,18 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void ConcreteManipulator(py::module& sm); + +void aikidopy_robot(py::module& m) +{ + auto sm = m.def_submodule("robot"); + + ConcreteManipulator(sm); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/rviz/TSRMarker.cpp b/python/aikidopy/rviz/TSRMarker.cpp new file mode 100644 index 0000000000..951bd159c4 --- /dev/null +++ b/python/aikidopy/rviz/TSRMarker.cpp @@ -0,0 +1,19 @@ +#ifdef AIKIDO_HAS_RVIZ + +#include +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void TSRMarker(py::module& m) +{ + py::class_>(m, "TSRMarker"); +} + +} // namespace python +} // namespace aikido + +#endif // AIKIDO_HAS_RVIZ diff --git a/python/aikidopy/rviz/WorldInteractiveMarkerViewer.cpp b/python/aikidopy/rviz/WorldInteractiveMarkerViewer.cpp new file mode 100644 index 0000000000..404bf73510 --- /dev/null +++ b/python/aikidopy/rviz/WorldInteractiveMarkerViewer.cpp @@ -0,0 +1,26 @@ +#ifdef AIKIDO_HAS_RVIZ + +#include +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void WorldInteractiveMarkerViewer(py::module& m) +{ + py::class_>(m, "WorldInteractiveMarkerViewer") + .def("addTSRMarker", + [](aikido::rviz::WorldInteractiveMarkerViewer* self, + std::shared_ptr tsr) + -> aikido::rviz::TSRMarkerPtr + { + return self->addTSRMarker(*tsr.get()); + }); +} + +} // namespace python +} // namespace aikido + +#endif // AIKIDO_HAS_RVIZ diff --git a/python/aikidopy/rviz/module.cpp b/python/aikidopy/rviz/module.cpp new file mode 100644 index 0000000000..fef5f8b3d1 --- /dev/null +++ b/python/aikidopy/rviz/module.cpp @@ -0,0 +1,24 @@ +#ifdef AIKIDO_HAS_RVIZ + +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void TSRMarker(py::module& sm); +void WorldInteractiveMarkerViewer(py::module& sm); + +void aikidopy_rviz(py::module& m) +{ + auto sm = m.def_submodule("rviz"); + + TSRMarker(sm); + WorldInteractiveMarkerViewer(sm); +} + +} // namespace python +} // namespace aikido + +#endif // AIKIDO_HAS_RVIZ diff --git a/python/aikidopy/statespace/module.cpp b/python/aikidopy/statespace/module.cpp new file mode 100644 index 0000000000..34414f9f3f --- /dev/null +++ b/python/aikidopy/statespace/module.cpp @@ -0,0 +1,14 @@ +#include + +namespace py = pybind11; + +namespace aikido { +namespace python { + +void aikidopy_statespace(py::module& m) +{ + auto sm = m.def_submodule("statespace"); +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/utils.cpp b/python/aikidopy/utils.cpp new file mode 100644 index 0000000000..545208aa45 --- /dev/null +++ b/python/aikidopy/utils.cpp @@ -0,0 +1,16 @@ +#include "utils.hpp" + +namespace aikido { +namespace python { + +Eigen::Isometry3d vectorToIsometry(const std::vector& poseVector) +{ + Eigen::Map p(poseVector.data(), static_cast(poseVector.size())); + Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); + pose.translation() = p.head<3>(); + pose.linear() = Eigen::Quaterniond(p[3], p[4], p[5], p[6]).toRotationMatrix(); + return pose; +} + +} // namespace python +} // namespace aikido diff --git a/python/aikidopy/utils.hpp b/python/aikidopy/utils.hpp new file mode 100644 index 0000000000..08a237f1f1 --- /dev/null +++ b/python/aikidopy/utils.hpp @@ -0,0 +1,15 @@ +#ifndef AIKIDOPY_UTILS_HPP_ +#define AIKIDOPY_UTILS_HPP_ + +#include +#include + +namespace aikido { +namespace python { + +Eigen::Isometry3d vectorToIsometry(const std::vector& poseVector); + +} // namespace python +} // namespace aikido + +#endif // AIKIDOPY_UTILS_HPP_ diff --git a/python/tests/CMakeLists.txt b/python/tests/CMakeLists.txt new file mode 100644 index 0000000000..27c78d03c6 --- /dev/null +++ b/python/tests/CMakeLists.txt @@ -0,0 +1,58 @@ +# Make sure pytest is installed +execute_process( + COMMAND ${PYTHON_EXECUTABLE} -c "import pytest; print(pytest.__version__)" + RESULT_VARIABLE pytest_not_found + OUTPUT_VARIABLE pytest_version + ERROR_QUIET +) +if(pytest_not_found) + message(STATUS "Running the tests requires pytest. Please install it manually" + " (try: ${PYTHON_EXECUTABLE} -m pip install pytest)" + ) + set(AIKIDOPY_PYTEST_FOUND FALSE) +elseif(pytest_version VERSION_LESS 3.0) + message(STATUS "Running the tests requires pytest >= 3.0. Found: ${pytest_version}" + "Please update it (try: ${PYTHON_EXECUTABLE} -m pip install -U pytest)" + ) + set(AIKIDOPY_PYTEST_FOUND FALSE) +else() + set(AIKIDOPY_PYTEST_FOUND TRUE) +endif() + +set(aikidopy_test_utils + util.py +) + +file(GLOB_RECURSE aikidopy_test_files "test_*.py") + +# Add custom target to run the tests +add_custom_target(pytest + COMMAND ${CMAKE_COMMAND} -E echo "Running pytest by: PYTHONPATH=${AIKIDO_AIKIDOPY_BUILD_DIR} ${PYTHON_EXECUTABLE} -m pytest [sources]" + COMMAND PYTHONPATH=${AIKIDOPY_BUILD_DIR} ${PYTHON_EXECUTABLE} -m pytest ${aikidopy_test_files} -v + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + SOURCES ${aikidopy_test_files} ${aikidopy_test_utils} +) + +#=============================================================================== +# Usage: +# aikidopy_add_test(test_name) # assumed source is test_name.py +# aikidopy_add_test(test_name main.py) +#=============================================================================== +function(aikidopy_add_test test_name) # ARGN for source file + if(${ARGC} GREATER 1) + set(source ${ARGN}) + else() + set(source "${test_name}.py") + endif() + add_custom_target(${test_name} + COMMAND ${CMAKE_COMMAND} -E echo "Running pytest by: PYTHONPATH=${AIKIDO_AIKIDOPY_BUILD_DIR} ${PYTHON_EXECUTABLE} ${source}" + COMMAND PYTHONPATH=${AIKIDO_AIKIDOPY_BUILD_DIR} ${PYTHON_EXECUTABLE} ${source} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + SOURCES ${source} + ) +endfunction() + +foreach(aikidopy_test_file ${aikidopy_test_files}) + get_filename_component(aikidopy_test_name ${aikidopy_test_file} NAME_WE) + aikidopy_add_test(py_${aikidopy_test_name} ${aikidopy_test_file}) +endforeach() diff --git a/python/tests/test_common.py b/python/tests/test_common.py new file mode 100644 index 0000000000..eca6ad67ad --- /dev/null +++ b/python/tests/test_common.py @@ -0,0 +1,9 @@ +import aikidopy as aikido + + +def test_empty(): + pass + + +if __name__ == "__main__": + pytest.main() diff --git a/python/tests/util.py b/python/tests/util.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/control/ros/CMakeLists.txt b/src/control/ros/CMakeLists.txt index 7491cee76b..cf2ed1ca4c 100644 --- a/src/control/ros/CMakeLists.txt +++ b/src/control/ros/CMakeLists.txt @@ -60,6 +60,9 @@ target_link_libraries("${PROJECT_NAME}_control_ros" ${control_msgs_LIBRARIES} ${roscpp_LIBRARIES} ) +target_compile_definitions("${PROJECT_NAME}_control_ros" + PUBLIC AIKIDO_HAS_CONTROL_ROS +) add_component(${PROJECT_NAME} control_ros) add_component_targets(${PROJECT_NAME} control_ros "${PROJECT_NAME}_control_ros") diff --git a/src/perception/CMakeLists.txt b/src/perception/CMakeLists.txt index b8af95d793..cc575b3d9e 100644 --- a/src/perception/CMakeLists.txt +++ b/src/perception/CMakeLists.txt @@ -83,6 +83,9 @@ if(DART_HAS_VOXELGRIDSHAPE) PUBLIC DART_HAS_VOXELGRIDSHAPE ) endif() +target_compile_definitions("${PROJECT_NAME}_perception" + PUBLIC AIKIDO_HAS_PERCEPTION +) add_component(${PROJECT_NAME} perception) add_component_targets(${PROJECT_NAME} perception "${PROJECT_NAME}_perception") diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt deleted file mode 100644 index 5accd68d43..0000000000 --- a/src/python/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -find_package(chimera QUIET) -aikido_check_package(chimera "Python bindings" "Chimera") - -find_package(PythonInterp QUIET) -aikido_check_package(PYTHONINTERP "Python bindings" "Python interpreter") - -find_package(PythonLibs QUIET) -aikido_check_package(PYTHONLIBS "Python bindings" "Python libraries") - -aikido_check_package(Boost_PYTHON "Python bindings" "Boost.Python") - -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 -) - -include_directories(SYSTEM - ${Boost_INCLUDE_DIRS} - ${DART_INCLUDE_DIRS} - ${PYTHON_INCLUDE_DIRS} -) -include_directories( - "${PROJECT_SOURCE_DIR}/include" - "${CMAKE_CURRENT_SOURCE_DIR}" -) - -get_property(AIKIDO_LIBRARIES GLOBAL PROPERTY AIKIDO_LIBRARIES) - -add_chimera_binding(TARGET "${PROJECT_NAME}_python" - MODULE "${PROJECT_NAME}" - CONFIGURATION "${CMAKE_CURRENT_SOURCE_DIR}/chimera.yml" - NAMESPACES "aikido" - SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/chimera.cpp" - DEBUG -) -target_link_libraries("${PROJECT_NAME}_python" - ${AIKIDO_LIBRARIES} - ${Boost_PYTHON_LIBRARIES} - ${DART_LIBRARIES} - ${PYTHON_LIBRARIES} -) -set_target_properties("${PROJECT_NAME}_python" PROPERTIES - OUTPUT_NAME "${PROJECT_NAME}" - PREFIX "" -) - -install(TARGETS "${PROJECT_NAME}_python" - LIBRARY DESTINATION "${PYTHON_SITE_PACKAGES}" -) diff --git a/src/python/chimera.cpp b/src/python/chimera.cpp deleted file mode 100644 index 9ca3bec1bf..0000000000 --- a/src/python/chimera.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include -#include -#include -#include -#include -#include diff --git a/src/python/chimera.yml b/src/python/chimera.yml deleted file mode 100644 index 9d69f3889c..0000000000 --- a/src/python/chimera.yml +++ /dev/null @@ -1,43 +0,0 @@ -template: - module: - source: - 'templates/module.mstch.cpp' - cxx_record: - source: - 'templates/cxx_record.mstch.cpp' - enum: - source: - 'templates/enum.mstch.cpp' - function: - source: - 'templates/function.mstch.cpp' - variable: - source: - 'templates/variable.mstch.cpp' - file: - header: | - #include "get_signature.hpp" - #include "chimera.cpp" - main: - precontent: | - boost::python::import("boost_numpy_eigen"); - boost::python::import("dartpy"); -namespaces: - 'aikido': - name: null # Collapse the 'aikido' namespace into its parent. -types: - 'void *': null - # TODO: Wrap in a ScopedState. - 'aikido::statespace::StateSpace::State *': null - 'ompl::base::State *': null - # TODO: Wrap in a JointPtr - 'dart::dynamics::Joint *': - return_value_policy: boost::python::reference_existing_object - # TODO: Convert to NumPy - 'const aikido::statespace::SE2::State::Isometry2d &': - return_value_policy: boost::python::copy_const_reference - 'const aikido::statespace::SE3::State::Isometry3d &': - return_value_policy: boost::python::copy_const_reference - 'const aikido::statespace::SO3::State::Quaternion &': - return_value_policy: boost::python::copy_const_reference -classes: [] diff --git a/src/python/get_signature.hpp b/src/python/get_signature.hpp deleted file mode 100644 index 0dafd1048a..0000000000 --- a/src/python/get_signature.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef AIKIDO_PYTHON_GET_SIGNATURE_H_ -#define AIKIDO_PYTHON_GET_SIGNATURE_H_ -#include -#include -#include -#include -#include -#include -#include - -// Source: https://mail.python.org/pipermail/cplusplus-sig/2014-February/017103.html - -namespace boost { -namespace python { -namespace detail { - -template -struct fun_sign -{ - typedef T type; -}; - -template -struct fun_sign< - T, - typename enable_if< - mpl::greater< - mpl::size, - mpl::std::size_t<1> - > - >::type -> -{ - typedef typename mpl::erase< - T, - typename mpl::next< - typename mpl::begin< - T - >::type - >::type - >::type type; -}; - -template -typename disable_if, typename fun_sign::types>::type>::type -get_signature(T, void* = 0) -{ -return typename fun_sign::types>::type(); -} - -} // namespace detail -} // namespace python -} // namespace boost - -#endif // ifndef AIKIDO_PYTHON_GET_SIGNATURE_H_ diff --git a/src/python/templates/cxx_record.mstch.cpp b/src/python/templates/cxx_record.mstch.cpp deleted file mode 100644 index 3d78f19643..0000000000 --- a/src/python/templates/cxx_record.mstch.cpp +++ /dev/null @@ -1,103 +0,0 @@ -{{{header}}} -{{#includes}} -#include <{{{.}}}> -{{/includes}} -{{{precontent}}} -#include -#include - -namespace { - -{{#class.comment?}}{{! -}}constexpr char {{class.mangled_name}}_docstring[] = R"CHIMERA_STRING({{! -}}{{#class.comment}}{{! -}}{{{.}}} -{{/class.comment}}{{! -}})CHIMERA_STRING"; -{{/class.comment?}} - -{{#class.methods}}{{! -}}{{#comment?}}{{! -}}constexpr char {{mangled_name}}_docstring[] = R"CHIMERA_STRING({{! -}}{{#comment}}{{! -}}{{{.}}} -{{/comment}}{{! -}})CHIMERA_STRING"; - -{{/comment?}}{{! -}}{{/class.methods}} - -} // namespace - -void {{class.mangled_name}}() -{ -::boost::python::object parent_object(::boost::python::scope(){{! - }}{{#class.scope}}{{#name}}.attr("{{name}}"){{/name}}{{/class.scope}}); -::boost::python::scope parent_scope(parent_object); - -::boost::python::class_<{{{class.type}}}{{^class.is_copyable}}, {{! - }}::boost::noncopyable{{/class.is_copyable}}{{#class.held_type}}, {{! - }}{{{.}}}{{/class.held_type}}{{#class.bases?}}, {{! - }}::boost::python::bases<{{! - }}{{#class.bases}}{{{qualified_name}}}{{^last}}, {{/last}}{{/class.bases}}{{! - }} >{{/class.bases?}} >("{{class.name}}"{{! - }}{{#class.comment?}}, {{class.mangled_name}}_docstring{{/class.comment?}}{{! - }}, boost::python::no_init){{! - -/* constructors */}} -{{#class.constructors}}{{! -}}{{#overloads}}{{! - }}.def("__init__", ::boost::python::make_constructor({{! - }}[]({{#params}}{{{type}}} {{name}}{{^last}}, {{/last}}{{/params}}){{! - }} -> {{{class.type}}} * { {{! - }}return new {{{class.type}}}({{#params}}{{name}}{{^last}}, {{/last}}{{/params}}); }{{! - }}, ::boost::python::default_call_policies(){{! - }}{{#params?}}, ({{#params}}::boost::python::arg("{{name}}"){{^last}}, {{/last}}{{/params}}){{/params?}})) -{{/overloads}}{{! -}}{{/class.constructors}}{{! - -/* member functions */}} -{{#class.methods}}{{! -}}{{^is_static}}{{! -}}{{#overloads}}{{! - }}.def("{{name}}", []({{#is_const}}const {{/is_const}}{{{class.type}}} *self{{#params}}, {{{type}}} {{name}}{{/params}}) -> {{{return_type}}} { {{! - }}return self->{{{call}}}({{#params}}{{name}}{{^last}}, {{/last}}{{/params}}); }{{! - }}{{#return_value_policy}}, ::boost::python::return_value_policy<{{{.}}} >(){{/return_value_policy}}{{! - }}{{#comment?}}, {{mangled_name}}_docstring{{/comment?}}{{! - }}{{#params?}}, ({{#params}}::boost::python::arg("{{name}}"){{^last}}, {{/last}}{{/params}}){{/params?}}) -{{/overloads}}{{! -}}{{/is_static}}{{! -}}{{/class.methods}}{{! - -/* static member functions */}} -{{#class.methods}}{{! -}}{{#is_static}}{{! -}}{{#overloads}}{{! - }}.def("{{name}}", []({{#params}}{{{type}}} {{name}}{{^last}}, {{/last}}{{/params}}) -> {{{return_type}}} { {{! - }}return {{{qualified_call}}}({{#params}}{{name}}{{^last}}, {{/last}}{{/params}}); }{{! - }}{{#return_value_policy}}, ::boost::python::return_value_policy<{{{.}}} >(){{/return_value_policy}}{{! - }}{{#params?}}, ({{#params}}::boost::python::arg("{{name}}"){{^last}}, {{/last}}{{/params}}){{/params?}}) -{{/overloads}}{{! -}}{{/is_static}}{{! -}}{{/class.methods}} -{{#class.static_methods}}{{! - }}.staticmethod("{{.}}") -{{/class.static_methods}}{{! - -/* fields */}} -{{#class.fields}}{{! - }}{{#is_assignable}}.def_readwrite{{/is_assignable}}{{! - }}{{^is_assignable}}.def_readonly{{/is_assignable}}{{! - }}("{{name}}", &{{{qualified_name}}}) -{{/class.fields}}{{! - -/* static fields */ -/* TODO: Add make_setter if this property is assignable */}} -{{#class.static_fields}}{{! - }}.add_static_property("{{name}}", {{! - }}::boost::python::make_getter({{{qualified_name}}})) -{{/class.static_fields}} -; -} -{{{postcontent}}} -{{{footer}}} diff --git a/src/python/templates/enum.mstch.cpp b/src/python/templates/enum.mstch.cpp deleted file mode 100644 index 2d9e64416c..0000000000 --- a/src/python/templates/enum.mstch.cpp +++ /dev/null @@ -1,22 +0,0 @@ -{{{header}}} -{{#includes}} -#include <{{{.}}}> -{{/includes}} -{{{precontent}}} -#include -#include - -/* postinclude */ - -void {{enum.mangled_name}}() -{ -::boost::python::object parent_object(::boost::python::scope(){{! - }}{{#enum.scope}}{{#name}}.attr("{{name}}"){{/name}}{{/enum.scope}}); -::boost::python::scope parent_scope(parent_object); - -::boost::python::enum_<{{{enum.type}}}>("{{enum.name}}"){{#enum.values}} -.value("{{name}}", {{{qualified_name}}}){{/enum.values}} -; -} -{{{postcontent}}} -{{{footer}}} diff --git a/src/python/templates/function.mstch.cpp b/src/python/templates/function.mstch.cpp deleted file mode 100644 index 275ecdec1d..0000000000 --- a/src/python/templates/function.mstch.cpp +++ /dev/null @@ -1,26 +0,0 @@ -{{{header}}} -{{#includes}} -#include <{{{.}}}> -{{/includes}} -{{{precontent}}} -#include -#include - -/* postinclude */ - -void {{function.mangled_name}}() -{ -::boost::python::object parent_object(::boost::python::scope(){{! - }}{{#function.scope}}{{#name}}.attr("{{name}}"){{/name}}{{/function.scope}}); -::boost::python::scope parent_scope(parent_object); - -{{#function.overloads}}{{! - }}::boost::python::def("{{name}}", []({{#params}}{{{type}}} {{name}}{{^last}}, {{/last}}{{/params}}) -> {{{return_type}}} { {{! - }}return {{{qualified_call}}}({{#params}}{{name}}{{^last}}, {{/last}}{{/params}}); }{{! - }}{{#return_value_policy}}, ::boost::python::return_value_policy<{{{.}}} >(){{/return_value_policy}}{{! - }}{{#params?}}, ({{#params}}::boost::python::arg("{{name}}"){{^last}}, {{/last}}{{/params}}){{/params?}}); -{{/function.overloads}} - -} -{{{postcontent}}} -{{{footer}}} diff --git a/src/python/templates/module.mstch.cpp b/src/python/templates/module.mstch.cpp deleted file mode 100644 index 4589041df4..0000000000 --- a/src/python/templates/module.mstch.cpp +++ /dev/null @@ -1,31 +0,0 @@ -{{{header}}} -{{#includes}} -#include <{{{.}}}> -{{/includes}} - -#include -#include - -/* main postinclude */ - -BOOST_PYTHON_MODULE({{module.name}}) -{ -::boost::python::docstring_options options(true, true, false); - -{{{precontent}}} -{{#module.namespaces}}{{#name}} - ::boost::python::scope(){{! - }}{{#scope}}{{#name}}.attr("{{name}}"){{/name}}{{/scope}}.attr("{{name}}") = {{! - }}::boost::python::object(::boost::python::handle<>({{! - }}::boost::python::borrowed(::PyImport_AddModule({{! - }}"{{module.name}}{{#scope}}{{#name}}.{{name}}{{/name}}{{/scope}}.{{name}}")))); -{{/name}}{{/module.namespaces}} - -{{#module.bindings}} - void {{.}}(); - {{.}}(); - -{{/module.bindings}} -} -{{{postcontent}}} -{{{footer}}} diff --git a/src/python/templates/variable.mstch.cpp b/src/python/templates/variable.mstch.cpp deleted file mode 100644 index b651aabf03..0000000000 --- a/src/python/templates/variable.mstch.cpp +++ /dev/null @@ -1,20 +0,0 @@ -{{{header}}} -{{#includes}} -#include <{{{.}}}> -{{/includes}} -{{{precontent}}} -#include -#include - -/* postinclude */ - -void {{variable.mangled_name}}() -{ -::boost::python::object parent_object(::boost::python::scope(){{! - }}{{#variable.scope}}{{#name}}.attr("{{name}}"){{/name}}{{/variable.scope}}); -::boost::python::scope parent_scope(parent_object); - -::boost::python::scope().attr("{{variable.name}}") = {{{variable.qualified_name}}}; -} -{{{postcontent}}} -{{{footer}}} diff --git a/src/robot/util.cpp b/src/robot/util.cpp index 15de36b41e..6989431072 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -69,8 +69,6 @@ using dart::dynamics::MetaSkeleton; using dart::dynamics::MetaSkeletonPtr; using dart::dynamics::SkeletonPtr; -static const double collisionResolution = 0.1; - //============================================================================== trajectory::TrajectoryPtr planToConfiguration( const MetaSkeletonStateSpacePtr& space, @@ -201,7 +199,7 @@ trajectory::TrajectoryPtr planToTSR( tsr, createSampleableBounds(space, rng->clone()), ik, - maxNumTrials); + static_cast(maxNumTrials)); auto generator = ikSampleable.createSampleGenerator(); @@ -439,7 +437,7 @@ trajectory::TrajectoryPtr planToEndEffectorOffset( std::chrono::duration(timelimit)); if (traj) - return std::move(traj); + return traj; return planToEndEffectorOffsetByCRRT( space, diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt index 36caedda95..b406fc86ce 100644 --- a/src/rviz/CMakeLists.txt +++ b/src/rviz/CMakeLists.txt @@ -65,6 +65,9 @@ target_link_libraries("${PROJECT_NAME}_rviz" target_compile_options("${PROJECT_NAME}_rviz" PUBLIC ${AIKIDO_CXX_STANDARD_FLAGS} ) +target_compile_definitions("${PROJECT_NAME}_rviz" + PUBLIC AIKIDO_HAS_RVIZ +) add_component(${PROJECT_NAME} rviz) add_component_targets(${PROJECT_NAME} rviz "${PROJECT_NAME}_rviz") diff --git a/src/statespace/dart/MetaSkeletonStateSpace.cpp b/src/statespace/dart/MetaSkeletonStateSpace.cpp index 7126640b5c..8682ffdb71 100644 --- a/src/statespace/dart/MetaSkeletonStateSpace.cpp +++ b/src/statespace/dart/MetaSkeletonStateSpace.cpp @@ -27,7 +27,7 @@ std::vector convertVectorType(const std::vector& input) for (const auto& x : input) output.emplace_back(x); - return std::move(output); + return output; } //==============================================================================