[WIP] Feature: Add Python bindings

robowflex_library/CMakeLists.txt

@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 project(robowflex_library)
 
 set(LIBRARY_NAME ${PROJECT_NAME})
@@ -20,6 +21,7 @@ find_library(YAML yaml-cpp REQUIRED)
 find_package(TinyXML2 REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(HDF5 REQUIRED COMPONENTS C CXX)
+find_package(pybind11 CONFIG)
 
 ##
 ## Catkin setup
@@ -112,6 +114,14 @@ add_library(${LIBRARY_NAME} ${SOURCES})
 set_target_properties(${LIBRARY_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
 target_link_libraries(${LIBRARY_NAME} ${LIBRARIES})
 
+##
+## Python bindings
+##
+if(${pybind11_FOUND})
+  pybind11_add_module(robowflex_library_python src/python_bindings.cpp)
+  set_target_properties(robowflex_library_python PROPERTIES OUTPUT_NAME "robowflex")
+endif()
+
 ##
 ## Scripts
 ##
@@ -154,3 +164,11 @@ add_test_script(robot_scene)
 install_scripts()
 install_library()
 install_directory(yaml)
+if(${pybind11_FOUND})
+  # We do this here because it causes errors with this old version of CMake if we do it before
+  # finding pybind11
+  find_package(Python COMPONENTS Development)
+  install(TARGETS robowflex_library_python
+    COMPONENT python
+    LIBRARY DESTINATION "${Python_SITELIB}/robowflex")
+endif()

robowflex_library/fetch_test.py

@@ -0,0 +1,65 @@
+from sys import argv
+
+import robowflex_library as rf
+
+GROUP = "arm_with_torso"
+
+DEFAULT_ADAPTERS = [
+    "default_planner_request_adapters/AddTimeParameterization",
+    "default_planner_request_adapters/FixWorkspaceBounds",
+    "default_planner_request_adapters/FixStartStateBounds",
+    "default_planner_request_adapters/FixStartStateCollision",
+    "default_planner_request_adapters/FixStartStatePathConstraints"
+]
+
+# Startup ROS
+print('Starting ROS...')
+ros = rf.ROS(len(argv), argv, "robowflex", 1)
+print('ROS running!')
+
+# Create the default Fetch robot
+print('Creating Fetch robot...')
+fetch = rf.FetchRobot()
+fetch.initialize(True)
+
+# Create an empty scene
+print('Creating scene...')
+scene = rf.Scene(fetch)
+
+# Create the default planner for the Fetch
+print('Creating planner...')
+planner = rf.FetchOMPLPipelinePlanner(fetch, "default")
+planner.initialize(rf.Settings(), DEFAULT_ADAPTERS)
+
+# Set the Fetch's base pose
+print('Posing Fetch...')
+fetch.set_base_pose(1, 1, 0.5)
+
+# Set the Fetch's head pose to look at a point
+fetch.point_head([2, 1, 1.5])
+
+# Create a motion planning request with a pose goal
+print('Creating planning request...')
+request = rf.MotionRequestBuilder(planner, GROUP, '')
+fetch.set_group_state(GROUP, [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0])
+request.set_start_configuration(fetch.get_scratch_state())
+
+# Unfurl
+fetch.set_group_state(GROUP, [0.265, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007])
+request.set_goal_configuration(fetch.get_scratch_state())
+request.set_config("RRTConnect")
+
+# Do motion planning!
+print('Motion planning...')
+result = planner.plan(scene, request.get_request())
+# NOTE: We don't export the MoveIt messages library; could probably get the below constant from the
+# Python bindings but I'm lazy
+if result.error_code_.val != 1:
+  raise RuntimeError(f'Motion planning failed with code: {result.error_code_.val}')
+
+# Create a trajectory object for better manipulation
+print('Exporting trajectory...')
+trajectory = rf.Trajectory(result.trajectory_)
+
+# Output path to a file for visualization
+trajectory.to_yaml_file("fetch_path.yml")