Add ICAROS Python Bindings

CMakeLists.txt

@@ -1,13 +1,41 @@
 cmake_minimum_required(VERSION 3.10.0)
 project(libada)
-set(library_VERSION 0.2.0)
+set(library_VERSION 0.3.0)
 
 option(LIBADA_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF)
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
 set(CMAKE_CXX_STANDARD 14)
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 
+#================================================================================
+# Python setup
+#
+
+set(ADAPY_PYTHON_VERSION 3.8)
+find_package(PythonInterp ${ADAPY_PYTHON_VERSION})
+
+if(PythonInterp_FOUND)
+  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
+  )
+
+  find_package(PythonLibs ${ADAPY_PYTHON_VERSION})
+
+  # Find pybind11
+  # Needs to set PYBIND11_PYTHON_VERSION before finding pybind11
+  set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION} CACHE STRING "")
+  find_package(pybind11 2.2.0)
+endif()
+
+if(NOT pybind11_FOUND)
+  message(WARNING "Could not load pybind11. Cannot build python bindings.")
+  return()
+endif()
+
 #================================================================================
 # CodeCov setup
 #
@@ -28,7 +56,6 @@ include(ExternalProject)
 
 find_package(DART 6.8.5 REQUIRED
   COMPONENTS utils utils-urdf optimizer-nlopt
-
 )
 
 find_package(aikido 0.3.0 REQUIRED
@@ -47,6 +74,7 @@ find_package(aikido 0.3.0 REQUIRED
     robot
     statespace
     io
+    rviz
 )
 
 find_package(ada_description REQUIRED)
@@ -75,11 +103,11 @@ endif()
 #
 
 set(sources
-  src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp
-  src/AdaHandKinematicSimulationPositionCommandExecutor.cpp
-  src/AdaHand.cpp
-  src/Ada.cpp
-  src/util.cpp
+  src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp
+  src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp
+  src/libada/AdaHand.cpp
+  src/libada/Ada.cpp
+  src/libada/util.cpp
 )
 
 add_library(libada SHARED ${sources})
@@ -116,6 +144,34 @@ target_link_libraries(libada
     ${urdf_LIBRARIES}
 )
 
+#=============================================================================
+#Pybind
+if(pybind11_FOUND)
+  pybind11_add_module(adapy
+    MODULE
+    python/adapy/adapy.cpp
+    python/adapy/ada_py.cpp
+    python/adapy/aikido_py.cpp
+    python/adapy/dart_py.cpp
+    python/adapy/pr_tsr_py.cpp
+    python/adapy/ik_py.cpp
+  )
+
+  target_include_directories(adapy
+    SYSTEM PUBLIC
+      ${PYTHON_INCLUDE_DIRS}
+      ${pybind11_INCLUDE_DIRS}
+    PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}
+  )
+
+  target_link_libraries(adapy PUBLIC
+    libada
+    ${libaikidopy_LIBRARIES}
+    ${DART_LIBRARIES}
+  )
+endif()
+
 #================================================================================
 # Testing
 #
@@ -164,6 +220,10 @@ install(FILES "package.xml"
   DESTINATION "share/libada")
 
 #install(DIRECTORY resources DESTINATION share)
+if(pybind11_FOUND)
+  install(TARGETS adapy
+        LIBRARY DESTINATION lib/python3/dist-packages)
+endif()
 
 
 #================================================================================

include/libada/Ada.hpp

@@ -89,7 +89,7 @@ class Ada final : public aikido::robot::Robot
       const dart::common::Uri& adaUrdfUri = defaultAdaUrdfUri,
       const dart::common::Uri& adaSrdfUri = defaultAdaSrdfUri,
       const std::string& endEffectorName = "j2n6s200_end_effector",
-      const std::string& armTrajectoryExecutorName = "trajectory_controller",
+      const std::string& armTrajectoryExecutorName = "rewd_trajectory_controller",
       const ::ros::NodeHandle* node = nullptr,
       aikido::common::RNG::result_type rngSeed = std::random_device{}(),
       const dart::common::ResourceRetrieverPtr& retriever
@@ -176,6 +176,16 @@ class Ada final : public aikido::robot::Robot
   // Runs step with current time.
   void update();
 
+  aikido::trajectory::TrajectoryPtr computeRetimedJointSpacePath(
+      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
+      const std::vector<std::pair<double, Eigen::VectorXd>>& waypoints);
+
+  aikido::trajectory::TrajectoryPtr computeSmoothJointSpacePath(
+      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
+      const std::vector<std::pair<double, Eigen::VectorXd>>& waypoints,
+      const aikido::constraint::dart::CollisionFreePtr& collisionFree
+      = nullptr);
+
   /// \copydoc Robot::planToConfiguration.
   aikido::trajectory::TrajectoryPtr planToConfiguration(
       const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,

include/libada/AdaHand.hpp

@@ -83,6 +83,8 @@ class AdaHand : public aikido::robot::Hand
   // Documentation inherited.
   std::future<void> executePreshape(const std::string& preshapeName) override;
 
+  std::future<void> executePreshape(const Eigen::Vector2d& preshape);
+
   // Documentation inherited.
   void step(const std::chrono::system_clock::time_point& timepoint) override;
 

include/libada/detail/Ada-impl.hpp

@@ -10,17 +10,21 @@ aikido::trajectory::UniqueSplinePtr Ada::postProcessPath(
     const Eigen::VectorXd& velocityLimits,
     const Eigen::VectorXd& accelerationLimits)
 {
+  // Don't plan above 70% hard velocity limit
+  // Unless requested explicitly
+  const double DEFAULT_LIMITS_BUFFER = 0.7;
+
   bool velLimitsInvalid
       = (velocityLimits.squaredNorm() == 0.0)
         || velocityLimits.size() != getVelocityLimits().size();
   auto sentVelocityLimits
-      = velLimitsInvalid ? getVelocityLimits() : velocityLimits;
+      = velLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getVelocityLimits() : velocityLimits;
 
   bool accLimitsInvalid
       = (accelerationLimits.squaredNorm() == 0.0)
         || accelerationLimits.size() != getAccelerationLimits().size();
   auto sentAccelerationLimits
-      = accLimitsInvalid ? getAccelerationLimits() : accelerationLimits;
+      = accLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getAccelerationLimits() : accelerationLimits;
 
   return mRobot->postProcessPath<PostProcessor>(
       sentVelocityLimits,

package.xml

@@ -14,6 +14,7 @@
   <depend>roslib</depend>
   <depend>srdfdom</depend>
   <depend>urdf</depend>
+  <depend>roscpp_initializer</depend>
 
   <!-- Workaround to build DART with optional features enabled. -->
   <depend>nlopt</depend>