ur_kin_py.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Python wrapper for UR kinematics
00004  * Author: Kelsey Hawkins (kphawkins@gatech.edu)
00005  *
00006  * Software License Agreement (BSD License)
00007  *
00008  *  Copyright (c) 2013, Georgia Institute of Technology
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of the Georgia Institute of Technology nor the names of
00022  *     its contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *********************************************************************/
00038 
00039 #include <boost/numpy.hpp>
00040 #include <boost/scoped_array.hpp>
00041 
00042 #include <ur_kinematics/ur_kin.h>
00043 
00044 namespace p = boost::python;
00045 namespace np = boost::numpy;
00046 
00047 np::ndarray forward_wrapper(np::ndarray const & q_arr) {
00048   if(q_arr.get_dtype() != np::dtype::get_builtin<double>()) {
00049     PyErr_SetString(PyExc_TypeError, "Incorrect array data type");
00050     p::throw_error_already_set();
00051   }
00052   if(q_arr.get_nd() != 1) {
00053     PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions");
00054     p::throw_error_already_set();
00055   }
00056   if(q_arr.shape(0) != 6) {
00057     PyErr_SetString(PyExc_TypeError, "Incorrect shape (should be 6)");
00058     p::throw_error_already_set();
00059   }
00060   Py_intptr_t shape[2] = { 4, 4 };
00061   np::ndarray result = np::zeros(2,shape,np::dtype::get_builtin<double>()); 
00062   ur_kinematics::forward(reinterpret_cast<double*>(q_arr.get_data()), 
00063                          reinterpret_cast<double*>(result.get_data()));
00064   return result;
00065 }
00066 
00067 np::ndarray inverse_wrapper(np::ndarray const & array, PyObject * q6_des_py) {
00068   if(array.get_dtype() != np::dtype::get_builtin<double>()) {
00069     PyErr_SetString(PyExc_TypeError, "Incorrect array data type");
00070     p::throw_error_already_set();
00071   }
00072   if(array.get_nd() != 2) {
00073     PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions");
00074     p::throw_error_already_set();
00075   }
00076   if(array.shape(0) != 4 || array.shape(1) != 4) {
00077     PyErr_SetString(PyExc_TypeError, "Incorrect shape (should be 4x4)");
00078     p::throw_error_already_set();
00079   }
00080   double* T = reinterpret_cast<double*>(array.get_data());
00081   double* q_sols = (double*) malloc(8*6*sizeof(double));
00082   double q6_des = PyFloat_AsDouble(q6_des_py);
00083   int num_sols = ur_kinematics::inverse(T, q_sols, q6_des);
00084   q_sols = (double*) realloc(q_sols, num_sols*6*sizeof(double));
00085   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());
00086 }
00087 
00088 BOOST_PYTHON_MODULE(ur_kin_py) {
00089   np::initialize();  // have to put this in any module that uses Boost.NumPy
00090   p::def("forward", forward_wrapper);
00091   p::def("inverse", inverse_wrapper);
00092 }


ur_kinematics
Author(s): Kelsey Hawkins
autogenerated on Mon Oct 6 2014 08:24:33