Ported ur_kinematics package from Georgia Tech library. Added ability to...

Ported ur_kinematics package from Georgia Tech library.  Added ability to create ur5 & ur10 kinematics libraries.  Python libaries not untested.  Kinematics still needs to be wrapped within Kinematics plugin interface

ur_kinematics/CMakeLists.txt

+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 "")

ur_kinematics/Makefile

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

ur_kinematics/include/ur_kinematics/ur_kin.h

+/*********************************************************************
+ *
+ * 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

ur_kinematics/mainpage.dox

+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ur_kinematics 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

ur_kinematics/manifest.xml

+<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>
+
+

ur_kinematics/src/ur_kin.cpp

+#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;
+}

ur_kinematics/src/ur_kin_py.cpp

+/*********************************************************************
+ *
+ * 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);
+}

ur_kinematics/src/ur_kinematics/test_analytical_ik.py

+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()