diff --git a/ur_kinematics/CMakeLists.txt b/ur_kinematics/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7d7d111cd204235a79c14c547bbd1fbd5ca819f4
--- /dev/null
+++ b/ur_kinematics/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+set(ROS_BUILD_TYPE MinSizeRel)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+##find_package(PythonLibs REQUIRED)
+##include_directories(${PYTHON_INCLUDE_DIRS})
+
+##rosbuild_add_executable(ur_kin src/ur_kin.cpp)
+rosbuild_add_library(ur10_kin src/ur_kin.cpp)
+set_target_properties (ur10_kin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
+
+
+rosbuild_add_library(ur5_kin src/ur_kin.cpp)
+set_target_properties (ur5_kin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
+
+##rosbuild_add_library(ur_kin_py src/ur_kin_py.cpp)
+##target_link_libraries(ur_kin_py ${PYTHON_LIBRARIES} boost_numpy ur_kin)
+##set_target_properties(ur_kin_py PROPERTIES PREFIX "")
diff --git a/ur_kinematics/Makefile b/ur_kinematics/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..b75b928f20ef9ea4f509a17db62e4e31b422c27f
--- /dev/null
+++ b/ur_kinematics/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h
new file mode 100644
index 0000000000000000000000000000000000000000..30b677235030e0d2f36496497159804d0ad73b30
--- /dev/null
+++ b/ur_kinematics/include/ur_kinematics/ur_kin.h
@@ -0,0 +1,97 @@
+/*********************************************************************
+ *
+ * Provides forward and inverse kinematics for Univeral robot designs
+ * Author: Kelsey Hawkins (kphawkins@gatech.edu)
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, Georgia Institute of Technology
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Georgia Institute of Technology nor the names of
+ * its contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#ifndef UR_KIN_H
+#define UR_KIN_H
+#include <math.h>
+#include <stdio.h>
+
+#define ZERO_THRESH 0.00000001
+#define SIGN(x) ( ( (x) > 0 ) - ( (x) < 0 ) )
+#define PI M_PI
+
+//#define UR10_PARAMS
+#ifdef UR10_PARAMS
+#define d1 0.1273
+#define a2 -0.612
+#define a3 -0.5723
+#define d4 0.163941
+#define d5 0.1157
+#define d6 0.0922
+#endif
+
+//#define UR5_PARAMS
+#ifdef UR5_PARAMS
+#define d1 0.089159
+#define a2 -0.42500
+#define a3 -0.39225
+#define d4 0.10915
+#define d5 0.09465
+#define d6 0.0823
+#endif
+
+// These kinematics find the tranfrom from the base link to the end effector.
+// Though the raw D-H parameters specify a transform from the 0th link to the 6th link,
+// offset transforms are specified in this formulation.
+// To work with the raw D-H kinematics, use the inverses of the transforms below.
+
+// Transform from base link to 0th link
+// -1, 0, 0, 0
+// 0, -1, 0, 0
+// 0, 0, 1, 0
+// 0, 0, 0, 1
+
+// Transform from 6th link to end effector
+// 0, -1, 0, 0
+// 0, 0, -1, 0
+// 1, 0, 0, 0
+// 0, 0, 0, 1
+
+namespace ur_kinematics {
+ // @param q The 6 joint values
+ // @param T The 4x4 end effector pose in row-major ordering
+ void forward(const double* q, double* T);
+
+ // @param T The 4x4 end effector pose in row-major ordering
+ // @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI)
+ // @param q6_des An optional parameter which designates what the q6 value should take
+ // in case of an infinite solution on that joint.
+ // @return Number of solutions found (maximum of 8)
+ int inverse(const double* T, double* q_sols, double q6_des=0.0);
+};
+
+#endif //UR_KIN_H
diff --git a/ur_kinematics/mainpage.dox b/ur_kinematics/mainpage.dox
new file mode 100644
index 0000000000000000000000000000000000000000..f1b9bb0082168360f307cab5efadec86a0d9ca9f
--- /dev/null
+++ b/ur_kinematics/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ur_kinematics
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/ur_kinematics/manifest.xml b/ur_kinematics/manifest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..95e18892a972d0eee9bfaeaf167c895c7fe7ffc0
--- /dev/null
+++ b/ur_kinematics/manifest.xml
@@ -0,0 +1,16 @@
+<package>
+ <description brief="ur_kinematics">
+
+ Provides forward and inverse kinematics for Universal robot designs.
+
+ </description>
+ <author>Shaun Edwards</author>
+ <author>Kelsey Hawkins (kphawkins@gatech.edu)</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/ur_kinematics</url>
+ <depend package="roscpp"/>
+
+</package>
+
+
diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5510c83f66db61aaa1a44f4db57c72e0fd0f8a11
--- /dev/null
+++ b/ur_kinematics/src/ur_kin.cpp
@@ -0,0 +1,208 @@
+#include <ur_kinematics/ur_kin.h>
+
+namespace ur_kinematics {
+
+ void forward(const double* q, double* T) {
+ double s1 = sin(*q), c1 = cos(*q); q++;
+ double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
+ double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
+ q234 += *q; q++;
+ double s5 = sin(*q), c5 = cos(*q); q++;
+ double s6 = sin(*q), c6 = cos(*q);
+ double s234 = sin(q234), c234 = cos(q234);
+ *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
+ *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) -
+ (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
+ *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 -
+ s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
+ *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 -
+ d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 -
+ a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
+ *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
+ *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) +
+ s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
+ *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) -
+ s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
+ *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 +
+ (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 -
+ a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
+ *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
+ *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
+ *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
+ *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 -
+ (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
+ *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
+ }
+
+ int inverse(const double* T, double* q_sols, double q6_des) {
+ int num_sols = 0;
+ double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
+ double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++;
+ double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T;
+
+ ////////////////////////////// shoulder rotate joint (q1) //////////////////////////////
+ double q1[2];
+ {
+ double A = d6*T12 - T13;
+ double B = d6*T02 - T03;
+ double R = A*A + B*B;
+ if(fabs(A) < ZERO_THRESH) {
+ double div;
+ if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
+ div = -SIGN(d4)*SIGN(B);
+ else
+ div = -d4/B;
+ double arcsin = asin(div);
+ if(fabs(arcsin) < ZERO_THRESH)
+ arcsin = 0.0;
+ if(arcsin < 0.0)
+ q1[0] = arcsin + 2.0*PI;
+ else
+ q1[0] = arcsin;
+ q1[1] = PI - arcsin;
+ }
+ else if(fabs(B) < ZERO_THRESH) {
+ double div;
+ if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
+ div = SIGN(d4)*SIGN(A);
+ else
+ div = d4/A;
+ double arccos = acos(div);
+ q1[0] = arccos;
+ q1[1] = 2.0*PI - arccos;
+ }
+ else if(d4*d4 > R) {
+ return num_sols;
+ }
+ else {
+ double arccos = acos(d4 / sqrt(R)) ;
+ double arctan = atan2(-B, A);
+ double pos = arccos + arctan;
+ double neg = -arccos + arctan;
+ if(fabs(pos) < ZERO_THRESH)
+ pos = 0.0;
+ if(fabs(neg) < ZERO_THRESH)
+ neg = 0.0;
+ if(pos >= 0.0)
+ q1[0] = pos;
+ else
+ q1[0] = 2.0*PI + pos;
+ if(neg >= 0.0)
+ q1[1] = neg;
+ else
+ q1[1] = 2.0*PI + neg;
+ }
+ }
+ ////////////////////////////////////////////////////////////////////////////////
+
+ ////////////////////////////// wrist 2 joint (q5) //////////////////////////////
+ double q5[2][2];
+ {
+ for(int i=0;i<2;i++) {
+ double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
+ double div;
+ if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
+ div = SIGN(numer) * SIGN(d6);
+ else
+ div = numer / d6;
+ double arccos = acos(div);
+ q5[i][0] = arccos;
+ q5[i][1] = 2.0*PI - arccos;
+ }
+ }
+ ////////////////////////////////////////////////////////////////////////////////
+
+ {
+ for(int i=0;i<2;i++) {
+ for(int j=0;j<2;j++) {
+ double c1 = cos(q1[i]), s1 = sin(q1[i]);
+ double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
+ double q6;
+ ////////////////////////////// wrist 3 joint (q6) //////////////////////////////
+ if(fabs(s5) < ZERO_THRESH)
+ q6 = q6_des;
+ else {
+ q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
+ SIGN(s5)*(T00*s1 - T10*c1));
+ if(fabs(q6) < ZERO_THRESH)
+ q6 = 0.0;
+ if(q6 < 0.0)
+ q6 += 2.0*PI;
+ }
+ ////////////////////////////////////////////////////////////////////////////////
+
+ double q2[2], q3[2], q4[2];
+ ///////////////////////////// RRR joints (q2,q3,q4) ////////////////////////////
+ double c6 = cos(q6), s6 = sin(q6);
+ double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
+ double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
+ double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
+ T03*c1 + T13*s1;
+ double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);
+
+ double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
+ if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
+ c3 = SIGN(c3);
+ else if(fabs(c3) > 1.0) {
+ // TODO NO SOLUTION
+ continue;
+ }
+ double arccos = acos(c3);
+ q3[0] = arccos;
+ q3[1] = 2.0*PI - arccos;
+ double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
+ double s3 = sin(arccos);
+ double A = (a2 + a3*c3), B = a3*s3;
+ q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
+ q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
+ double c23_0 = cos(q2[0]+q3[0]);
+ double s23_0 = sin(q2[0]+q3[0]);
+ double c23_1 = cos(q2[1]+q3[1]);
+ double s23_1 = sin(q2[1]+q3[1]);
+ q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
+ q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
+ ////////////////////////////////////////////////////////////////////////////////
+ for(int k=0;k<2;k++) {
+ if(fabs(q2[k]) < ZERO_THRESH)
+ q2[k] = 0.0;
+ else if(q2[k] < 0.0) q2[k] += 2.0*PI;
+ if(fabs(q4[k]) < ZERO_THRESH)
+ q4[k] = 0.0;
+ else if(q4[k] < 0.0) q4[k] += 2.0*PI;
+ q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k];
+ q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k];
+ q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
+ num_sols++;
+ }
+
+ }
+ }
+ }
+ return num_sols;
+ }
+};
+
+using namespace std;
+using namespace ur_kinematics;
+
+int main(int argc, char* argv[])
+{
+ double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0};
+ double* T = new double[16];
+ forward(q, T);
+ for(int i=0;i<4;i++) {
+ for(int j=i*4;j<(i+1)*4;j++)
+ printf("%1.3f ", T[j]);
+ printf("\n");
+ }
+ double q_sols[8*6];
+ int num_sols;
+ num_sols = inverse(T, q_sols);
+ for(int i=0;i<num_sols;i++)
+ printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n",
+ q_sols[i*6+0], q_sols[i*6+1], q_sols[i*6+2], q_sols[i*6+3], q_sols[i*6+4], q_sols[i*6+5]);
+ for(int i=0;i<=4;i++)
+ printf("%f ", PI/2.0*i);
+ printf("\n");
+ return 0;
+}
diff --git a/ur_kinematics/src/ur_kin_py.cpp b/ur_kinematics/src/ur_kin_py.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5a3e81040c85e454930876d0dd267a5822f981b4
--- /dev/null
+++ b/ur_kinematics/src/ur_kin_py.cpp
@@ -0,0 +1,92 @@
+/*********************************************************************
+ *
+ * Python wrapper for UR kinematics
+ * Author: Kelsey Hawkins (kphawkins@gatech.edu)
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, Georgia Institute of Technology
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Georgia Institute of Technology nor the names of
+ * its contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <boost/numpy.hpp>
+#include <boost/scoped_array.hpp>
+
+#include <ur_kinematics/ur_kin.h>
+
+namespace p = boost::python;
+namespace np = boost::numpy;
+
+np::ndarray forward_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[2] = { 4, 4 };
+ 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 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");
+ p::throw_error_already_set();
+ }
+ if(array.get_nd() != 2) {
+ PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions");
+ p::throw_error_already_set();
+ }
+ if(array.shape(0) != 4 || array.shape(1) != 4) {
+ PyErr_SetString(PyExc_TypeError, "Incorrect shape (should be 4x4)");
+ p::throw_error_already_set();
+ }
+ double* T = reinterpret_cast<double*>(array.get_data());
+ double* q_sols = (double*) malloc(8*6*sizeof(double));
+ double q6_des = PyFloat_AsDouble(q6_des_py);
+ int num_sols = ur_kinematics::inverse(T, q_sols, q6_des);
+ q_sols = (double*) realloc(q_sols, num_sols*6*sizeof(double));
+ 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) {
+ np::initialize(); // have to put this in any module that uses Boost.NumPy
+ p::def("forward", forward_wrapper);
+ p::def("inverse", inverse_wrapper);
+}
diff --git a/ur_kinematics/src/ur_kinematics/test_analytical_ik.py b/ur_kinematics/src/ur_kinematics/test_analytical_ik.py
new file mode 100644
index 0000000000000000000000000000000000000000..c11860bf588dfd9ff17db409d40c7de4d46c8504
--- /dev/null
+++ b/ur_kinematics/src/ur_kinematics/test_analytical_ik.py
@@ -0,0 +1,66 @@
+import numpy as np
+import sys
+import roslib
+roslib.load_manifest("ur_kinematics")
+from ur_kin_py import forward, inverse
+
+def best_sol(sols, q_guess, weights):
+ valid_sols = []
+ for sol in sols:
+ test_sol = np.ones(6)*9999.
+ for i in range(6):
+ for add_ang in [-2.*np.pi, 0, 2.*np.pi]:
+ test_ang = sol[i] + add_ang
+ if (abs(test_ang) <= 2.*np.pi and
+ abs(test_ang - q_guess[i]) < abs(test_sol[i] - q_guess[i])):
+ test_sol[i] = test_ang
+ if np.all(test_sol != 9999.):
+ valid_sols.append(test_sol)
+ if len(valid_sols) == 0:
+ return None
+ best_sol_ind = np.argmin(np.sum((weights*(valid_sols - np.array(q_guess)))**2,1))
+ return valid_sols[best_sol_ind]
+
+def test_q(q):
+ x = forward(q)
+ sols = inverse(np.array(x), float(q[5]))
+
+ qsol = best_sol(sols, q, [1.]*6)
+ if qsol is None:
+ qsol = [999.]*6
+ diff = np.sum(np.abs(np.array(qsol) - q))
+ if diff > 0.001:
+ print np.array(sols)
+ print 'Best q:', qsol
+ print 'Actual:', np.array(q)
+ print 'Diff: ', q - qsol
+ print 'Difdiv:', (q - qsol)/np.pi
+ print i1-3, i2-3, i3-3, i4-3, i5-3, i6-3
+ if raw_input() == 'q':
+ sys.exit()
+
+def main():
+ np.set_printoptions(precision=3)
+ print "Testing multiples of pi/2..."
+ for i1 in range(0,5):
+ for i2 in range(0,5):
+ print i1, i2
+ for i3 in range(0,5):
+ for i4 in range(0,5):
+ for i5 in range(0,5):
+ for i6 in range(0,5):
+ q = np.array([i1*np.pi/2., i2*np.pi/2., i3*np.pi/2.,
+ i4*np.pi/2., i5*np.pi/2., i6*np.pi/2.])
+ test_q(q)
+ print "Testing random configurations..."
+ for i in range(10000):
+ q = (np.random.rand(6)-.5)*4*np.pi
+ test_q(q)
+ print "Done!"
+
+if __name__ == "__main__":
+ if False:
+ import cProfile
+ cProfile.run('main()', 'ik_prof')
+ else:
+ main()