ur_kinematics python bindings cmake fixes

ur_kinematics/CMakeLists.txt

@@ -8,11 +8,16 @@ find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core moveit_
 
 find_package(Boost REQUIRED COMPONENTS system)
 
+## For python bindings
+find_package(Boost REQUIRED COMPONENTS python numpy)
+find_package(PythonLibs 2.7 REQUIRED)
+
 catkin_python_setup()
 
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ur3_kin ur5_kin ur10_kin ur3_moveit_plugin ur5_moveit_plugin ur10_moveit_plugin
+    ur10_kin_py ur5_kin_py ur3_kin_py
   CATKIN_DEPENDS roscpp geometry_msgs moveit_core moveit_kinematics moveit_ros_planning
     pluginlib tf_conversions
   DEPENDS Boost
@@ -25,6 +30,7 @@ catkin_package(
 
 include_directories(SYSTEM ${Boost_INCLUDE_DIR})
 include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(${PYTHON_INCLUDE_DIRS})
 
 add_library(ur3_kin src/ur_kin.cpp)
 set_target_properties(ur3_kin PROPERTIES COMPILE_DEFINITIONS "UR3_PARAMS")
@@ -35,6 +41,32 @@ set_target_properties(ur5_kin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
 add_library(ur10_kin src/ur_kin.cpp)
 set_target_properties(ur10_kin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
 
+add_library(ur10_kin_py src/ur_kin_py.cpp
+)
+set_target_properties(ur10_kin_py PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
+target_link_libraries(ur10_kin_py ur10_kin ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES})
+set_target_properties(ur10_kin_py PROPERTIES
+  PREFIX ""
+  LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)
+
+add_library(ur5_kin_py src/ur_kin_py.cpp
+)
+set_target_properties(ur5_kin_py PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
+target_link_libraries(ur5_kin_py ur5_kin ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES})
+set_target_properties(ur5_kin_py PROPERTIES
+  PREFIX ""
+  LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)
+
+add_library(ur3_kin_py src/ur_kin_py.cpp
+)
+set_target_properties(ur3_kin_py PROPERTIES COMPILE_DEFINITIONS "UR3_PARAMS")
+target_link_libraries(ur3_kin_py ur3_kin ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES})
+set_target_properties(ur3_kin_py PROPERTIES
+  PREFIX ""
+  LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)
 
 add_library(ur3_moveit_plugin src/ur_moveit_plugin.cpp)
 set_target_properties(ur3_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR3_PARAMS")
@@ -68,6 +100,21 @@ install(TARGETS ur3_kin ur5_kin ur10_kin ur3_moveit_plugin ur5_moveit_plugin ur1
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+install(TARGETS ur10_kin_py
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)
+
+install(TARGETS ur5_kin_py
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)
+
+install(TARGETS ur3_kin_py
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)
+
 # install header files
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

ur_kinematics/include/ur_kinematics/ur_kin.h

@@ -55,6 +55,13 @@
 //  1,  0,  0,  0
 //  0,  0,  0,  1
 
+// The size 6 array of joint values are always in this order (as defined in ur_description package)
+// ["shoulder_pan_joint",
+//  "shoulder_lift_joint",
+//  "elbow_joint",
+//  "wrist_1_joint",
+//  "wrist_2_joint",
+//  "wrist_3_joint"]
 namespace ur_kinematics {
   // @param q       The 6 joint values 
   // @param T       The 4x4 end effector pose in row-major ordering

ur_kinematics/src/ur_kinematics/test_analytical_ik.py → ur_kinematics/scripts/test_analytical_ik.py

@@ -1,8 +1,7 @@
+#!/usr/bin/env python
 import numpy as np
 import sys
-import roslib
-roslib.load_manifest("ur_kinematics")
-from ur_kin_py import forward, inverse
+from ur_kinematics.ur10_kin_py import (forward, inverse)
 
 def best_sol(sols, q_guess, weights):
     valid_sols = []
@@ -64,3 +63,4 @@ def main():
         cProfile.run('main()', 'ik_prof')
     else:
         main()
+

ur_kinematics/src/ur_kin_py.cpp

@@ -35,14 +35,26 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-
-#include <boost/numpy.hpp>
+#include <boost/python.hpp>
+#include <boost/python/numpy.hpp>
 #include <boost/scoped_array.hpp>
 
 #include <ur_kinematics/ur_kin.h>
 
+#ifdef UR10_PARAMS
+#define MODULE_NAME ur10_kin_py
+#endif
+
+#ifdef UR5_PARAMS
+#define MODULE_NAME ur5_kin_py
+#endif
+
+#ifdef UR3_PARAMS
+#define MODULE_NAME ur3_kin_py
+#endif
+
 namespace p = boost::python;
-namespace np = boost::numpy;
+namespace np = boost::python::numpy;
 
 np::ndarray forward_wrapper(np::ndarray const & q_arr) {
   if(q_arr.get_dtype() != np::dtype::get_builtin<double>()) {
@@ -58,12 +70,39 @@ np::ndarray forward_wrapper(np::ndarray const & q_arr) {
     p::throw_error_already_set();
   }
   Py_intptr_t shape[2] = { 4, 4 };
-  np::ndarray result = np::zeros(2,shape,np::dtype::get_builtin<double>()); 
+  np::ndarray result = np::zeros(2, shape, np::dtype::get_builtin<double>()); 
   ur_kinematics::forward(reinterpret_cast<double*>(q_arr.get_data()), 
                          reinterpret_cast<double*>(result.get_data()));
   return result;
 }
 
+np::ndarray forward_all_wrapper(np::ndarray const & q_arr) {
+  if(q_arr.get_dtype() != np::dtype::get_builtin<double>()) {
+    PyErr_SetString(PyExc_TypeError, "Incorrect array data type");
+    p::throw_error_already_set();
+  }
+  if(q_arr.get_nd() != 1) {
+    PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions");
+    p::throw_error_already_set();
+  }
+  if(q_arr.shape(0) != 6) {
+    PyErr_SetString(PyExc_TypeError, "Incorrect shape (should be 6)");
+    p::throw_error_already_set();
+  }
+  Py_intptr_t shape[3] = { 6, 4, 4 };
+  np::ndarray result = np::zeros(3, shape,np::dtype::get_builtin<double>()); 
+
+  double* result_ptr = reinterpret_cast<double*>(result.get_data());
+  ur_kinematics::forward_all(reinterpret_cast<double*>(q_arr.get_data()),
+    result_ptr,
+    result_ptr + 16,
+    result_ptr + (16 * 2),
+    result_ptr + (16 * 3),
+    result_ptr + (16 * 4),
+    result_ptr + (16 * 5));
+  return result;
+}
+
 np::ndarray inverse_wrapper(np::ndarray const & array, PyObject * q6_des_py) {
   if(array.get_dtype() != np::dtype::get_builtin<double>()) {
     PyErr_SetString(PyExc_TypeError, "Incorrect array data type");
@@ -85,8 +124,9 @@ np::ndarray inverse_wrapper(np::ndarray const & array, PyObject * q6_des_py) {
   return np::from_data(q_sols, np::dtype::get_builtin<double>() , p::make_tuple(num_sols, 6), p::make_tuple(6*sizeof(double), sizeof(double)), p::object());
 }
 
-BOOST_PYTHON_MODULE(ur_kin_py) {
+BOOST_PYTHON_MODULE(MODULE_NAME) {
   np::initialize();  // have to put this in any module that uses Boost.NumPy
   p::def("forward", forward_wrapper);
+  p::def("forward_all", forward_all_wrapper);
   p::def("inverse", inverse_wrapper);
 }