diff --git a/ur5_moveit/CMakeLists.txt b/ur5_moveit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..10c89d8a510246414bf5a1596829681106205c9d
--- /dev/null
+++ b/ur5_moveit/CMakeLists.txt
@@ -0,0 +1,34 @@
+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 RelWithDebInfo)
+
+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()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+rosbuild_add_library(universal_robot_kinematics_lib
+  src/universal_robot_arm_ikfast_plugin.cpp
+  )
\ No newline at end of file
diff --git a/ur5_moveit/Makefile b/ur5_moveit/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..b75b928f20ef9ea4f509a17db62e4e31b422c27f
--- /dev/null
+++ b/ur5_moveit/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/ur5_moveit/config/collision_matrix.yaml b/ur5_moveit/config/collision_matrix.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f82feacbd8f4f77bfda2817b6b8b8e649abcccb6
--- /dev/null
+++ b/ur5_moveit/config/collision_matrix.yaml
@@ -0,0 +1,100 @@
+default_collision_operations:
+  - object1: base_link
+    object2: shoulder_link
+    operation: disable  # Adjacent in collision
+  - object1: shoulder_link
+    object2: upper_arm_link
+    operation: disable  # Adjacent in collision
+  - object1: upper_arm_link
+    object2: elbow_collision_link
+    operation: disable  # Adjacent in collision
+  - object1: upper_arm_link
+    object2: forearm_link
+    operation: disable  # Adjacent in collision
+  - object1: forearm_link
+    object2: wrist_1_link
+    operation: disable  # Adjacent in collision
+  - object1: wrist_1_link
+    object2: wrist_2_link
+    operation: disable  # Adjacent in collision
+  - object1: wrist_2_link
+    object2: wrist_3_link
+    operation: disable  # Adjacent in collision
+  - object1: wrist_3_link
+    object2: ee_link
+    operation: disable  # Adjacent in collision
+  - object1: upper_arm_link
+    object2: shoulder_collision_link
+    operation: disable  # Adjacent in collision
+  - object1: base_link
+    object2: elbow_collision_link
+    operation: disable  # Never in collision
+  - object1: base_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: base_link
+    object2: upper_arm_link
+    operation: disable  # Never in collision
+  - object1: elbow_collision_link
+    object2: forearm_link
+    operation: disable  # Never in collision
+  - object1: elbow_collision_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: elbow_collision_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: elbow_collision_link
+    object2: wrist_1_link
+    operation: disable  # Never in collision
+  - object1: elbow_collision_link
+    object2: wrist_2_link
+    operation: disable  # Never in collision
+  - object1: elbow_collision_link
+    object2: wrist_3_link
+    operation: disable  # Never in collision
+  - object1: forearm_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: forearm_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: forearm_link
+    object2: wrist_2_link
+    operation: disable  # Never in collision
+  - object1: forearm_link
+    object2: wrist_3_link
+    operation: disable  # Never in collision
+  - object1: shoulder_collision_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: shoulder_collision_link
+    object2: wrist_1_link
+    operation: disable  # Never in collision
+  - object1: shoulder_collision_link
+    object2: wrist_2_link
+    operation: disable  # Never in collision
+  - object1: shoulder_collision_link
+    object2: wrist_3_link
+    operation: disable  # Never in collision
+  - object1: shoulder_link
+    object2: wrist_1_link
+    operation: disable  # Never in collision
+  - object1: shoulder_link
+    object2: wrist_2_link
+    operation: disable  # Never in collision
+  - object1: shoulder_link
+    object2: wrist_3_link
+    operation: disable  # Never in collision
+  - object1: upper_arm_link
+    object2: wrist_1_link
+    operation: disable  # Never in collision
+  - object1: upper_arm_link
+    object2: wrist_2_link
+    operation: disable  # Never in collision
+  - object1: upper_arm_link
+    object2: wrist_3_link
+    operation: disable  # Never in collision
+  - object1: wrist_1_link
+    object2: wrist_3_link
+    operation: disable  # Never in collision
\ No newline at end of file
diff --git a/ur5_moveit/config/joint_limits.yaml b/ur5_moveit/config/joint_limits.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d79d4994d76da413eff8427d4f0b9c288bf42406
--- /dev/null
+++ b/ur5_moveit/config/joint_limits.yaml
@@ -0,0 +1,43 @@
+joint_limits:
+  shoulder_pan_joint:
+    has_position_limits: true
+    has_velocity_limits: true
+    max_velocity: 3.14159265
+    has_acceleration_limits: true
+    max_acceleration: 1
+    angle_wraparound: false
+  shoulder_lift_joint:
+    has_position_limits: true
+    has_velocity_limits: true
+    max_velocity: 3.14159265
+    has_acceleration_limits: true
+    max_acceleration: 1
+    angle_wraparound: false
+  elbow_joint:
+    has_position_limits: true
+    has_velocity_limits: true
+    max_velocity: 3.14159265
+    has_acceleration_limits: true
+    max_acceleration: 1
+    angle_wraparound: false
+  wrist_1_joint:
+    has_position_limits: true
+    has_velocity_limits: true
+    max_velocity: 3.14159265
+    has_acceleration_limits: true
+    max_acceleration: 1
+    angle_wraparound: false
+  wrist_2_joint:
+    has_position_limits: true
+    has_velocity_limits: true
+    max_velocity: 3.14159265
+    has_acceleration_limits: true
+    max_acceleration: 1
+    angle_wraparound: false
+  wrist_3_joint:
+    has_position_limits: true
+    has_velocity_limits: true
+    max_velocity: 3.14159265
+    has_acceleration_limits: true
+    max_acceleration: 1
+    angle_wraparound: false
\ No newline at end of file
diff --git a/ur5_moveit/config/kinematics.yaml b/ur5_moveit/config/kinematics.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..db2415fd81f0cfb92da482eb554c8efe2101e9d1
--- /dev/null
+++ b/ur5_moveit/config/kinematics.yaml
@@ -0,0 +1,5 @@
+arm:
+  kinematics_solver: universal_robot_arm_kinematics/IKFastKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  tip_name: link0
+  root_name: link6
diff --git a/ur5_moveit/config/ompl_planning.yaml b/ur5_moveit/config/ompl_planning.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fb01cc21b2a8c9c4e4771183e999a5c440200efd
--- /dev/null
+++ b/ur5_moveit/config/ompl_planning.yaml
@@ -0,0 +1,14 @@
+manipulator:
+  planner_configs:
+    - SBLkConfigDefault
+    - LBKPIECEkConfigDefault
+  projection_evaluator: joints(shoulder_lift_link)
+  longest_valid_segment_fraction: 0.05
+
+planner_configs:
+
+  SBLkConfigDefault:
+    type: geometric::SBL
+
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
diff --git a/ur5_moveit/config/ur5.srdf b/ur5_moveit/config/ur5.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..d7e561145192dc4c7d6e236a9017fafc06c6505b
--- /dev/null
+++ b/ur5_moveit/config/ur5.srdf
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+
+<robot name="universal_robot">
+
+   <virtual_joint name="world_joint" type="floating" parent_frame="odom_combined" child_link="base_link"/> 
+
+   <group name="arm">
+      <chain base_link="base_link" tip_link="wrist_3_link"/>
+   </group>
+
+</robot>
diff --git a/ur5_moveit/config/ur5_or.srdf b/ur5_moveit/config/ur5_or.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..fbdaa321678d3f19259aa46416ed4db467a5d1d2
--- /dev/null
+++ b/ur5_moveit/config/ur5_or.srdf
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+
+<robot name="universal_robot">
+
+   <group name="arm">
+      <chain base_link="link0" tip_link="link6"/>
+   </group>
+
+</robot>
diff --git a/ur5_moveit/kinematics_plugins.xml b/ur5_moveit/kinematics_plugins.xml
new file mode 100644
index 0000000000000000000000000000000000000000..eaa5c7ee9bf91271eb7ab732c81a4266e3d3e78c
--- /dev/null
+++ b/ur5_moveit/kinematics_plugins.xml
@@ -0,0 +1,6 @@
+<?xml version='1.0' encoding='ASCII'?>
+<library path="lib/libuniversal_robot_kinematics_lib">
+  <class name="universal_robot_arm_kinematics/IKFastKinematicsPlugin" type="universal_robot_arm_kinematics::IKFastKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    <description>A plugin created by using OpenRAVE's IK Fast component</description>
+  </class>
+</library>
diff --git a/ur5_moveit/launch/ur5_moveit_visualization.launch b/ur5_moveit/launch/ur5_moveit_visualization.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2b20088e18c72220163636553a16ed1a43929a62
--- /dev/null
+++ b/ur5_moveit/launch/ur5_moveit_visualization.launch
@@ -0,0 +1,19 @@
+<launch>
+  <param name="robot_description" textfile="$(find ur5_description)/urdf/robot.urdf.xml" />
+  <!--<param name="robot_description" textfile="$(find ur5_description)/urdf/universal_or.dae" /-->
+  <param name="robot_description_semantic" textfile="$(find ur5_moveit)/config/ur5.srdf" />
+
+  <group ns="robot_description_planning">
+    <rosparam command="load" file="$(find ur5_moveit)/config/collision_matrix.yaml"/>
+    <rosparam command="load" file="$(find ur5_moveit)/config/joint_limits.yaml"/>
+  </group>
+  
+  <node pkg="robot_state_publisher" type="state_publisher" name="rob_st_pub" />
+  <node pkg="moveit_visualization_ros" name="wall_clock_server" type="fake_time.py" />
+
+  <node pkg="moveit_visualization_ros" type="moveit_visualizer" name="moveit_visualizer" output="screen">
+    <rosparam command="load" file="$(find ur5_moveit)/config/ompl_planning.yaml"/>
+    <rosparam command="load" file="$(find ur5_moveit)/config/kinematics.yaml"/>
+  </node>
+
+</launch>
diff --git a/ur5_moveit/mainpage.dox b/ur5_moveit/mainpage.dox
new file mode 100644
index 0000000000000000000000000000000000000000..2513c5bad956a3fd681afe8289918c083d998124
--- /dev/null
+++ b/ur5_moveit/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ur5_moveit 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/ur5_moveit/manifest.xml b/ur5_moveit/manifest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..57d7152be26ab3c8ab34e5af261fc82753a1f069
--- /dev/null
+++ b/ur5_moveit/manifest.xml
@@ -0,0 +1,21 @@
+<package>
+  <description brief="ur5_moveit">
+
+     ur5_moveit
+
+  </description>
+  <author>Gil Jones</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ur5_moveit</url>
+  <depend package="kinematics_base"/>
+  <depend package="moveit_configuration_tools"/>
+  <depend package="pluginlib"/>
+  <depend package="ur5_description"/>
+  <export>
+    <kinematics_base plugin="${prefix}/kinematics_plugins.xml"/>
+  </export>
+
+</package>
+
+
diff --git a/ur5_moveit/src/universal_robot_arm_ikfast_output.cpp b/ur5_moveit/src/universal_robot_arm_ikfast_output.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b47754e5eaa4acbcbed0c21263ca9b52b82d15f2
--- /dev/null
+++ b/ur5_moveit/src/universal_robot_arm_ikfast_output.cpp
@@ -0,0 +1,1102 @@
+/// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE
+/// \author Rosen Diankov
+///
+/// Licensed under the Apache License, Version 2.0 (the "License");
+/// you may not use this file except in compliance with the License.
+/// You may obtain a copy of the License at
+///     http://www.apache.org/licenses/LICENSE-2.0
+/// 
+/// Unless required by applicable law or agreed to in writing, software
+/// distributed under the License is distributed on an "AS IS" BASIS,
+/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+/// See the License for the specific language governing permissions and
+/// limitations under the License.
+///
+/// ikfast version 54 generated on 2012-03-27 13:04:03.830721
+/// To compile with gcc:
+///     gcc -lstdc++ ik.cpp
+/// To compile without any main function as a shared object (might need -llapack):
+///     gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp
+#include <cmath>
+#include <vector>
+#include <limits>
+#include <algorithm>
+#include <complex>
+
+#ifdef IKFAST_HEADER
+#include IKFAST_HEADER
+#endif
+
+#ifndef IKFAST_ASSERT
+#include <stdexcept>
+#include <sstream>
+#include <iostream>
+
+#ifdef _MSC_VER
+#ifndef __PRETTY_FUNCTION__
+#define __PRETTY_FUNCTION__ __FUNCDNAME__
+#endif
+#endif
+
+#ifndef __PRETTY_FUNCTION__
+#define __PRETTY_FUNCTION__ __func__
+#endif
+
+#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
+
+#endif
+
+#if defined(_MSC_VER)
+#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
+#else
+#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
+#endif
+
+#define IK2PI  ((IKReal)6.28318530717959)
+#define IKPI  ((IKReal)3.14159265358979)
+#define IKPI_2  ((IKReal)1.57079632679490)
+
+#ifdef _MSC_VER
+#ifndef isnan
+#define isnan _isnan
+#endif
+#endif // _MSC_VER
+
+// defined when creating a shared object/dll
+#ifdef IKFAST_CLIBRARY
+#ifdef _MSC_VER
+#define IKFAST_API extern "C" __declspec(dllexport)
+#else
+#define IKFAST_API extern "C"
+#endif
+#else
+#define IKFAST_API
+#endif
+
+// lapack routines
+extern "C" {
+  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
+  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
+  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
+  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
+  void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
+  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
+}
+
+using namespace std; // necessary to get std math routines
+
+#ifdef IKFAST_NAMESPACE
+namespace IKFAST_NAMESPACE {
+#endif
+
+#ifdef IKFAST_REAL
+typedef IKFAST_REAL IKReal;
+#else
+typedef double IKReal;
+#endif
+
+class IKSolution
+{
+public:
+    /// Gets a solution given its free parameters
+    /// \param pfree The free parameters required, range is in [-pi,pi]
+    void GetSolution(IKReal* psolution, const IKReal* pfree) const {
+        for(std::size_t i = 0; i < basesol.size(); ++i) {
+            if( basesol[i].freeind < 0 )
+                psolution[i] = basesol[i].foffset;
+            else {
+                IKFAST_ASSERT(pfree != NULL);
+                psolution[i] = pfree[basesol[i].freeind]*basesol[i].fmul + basesol[i].foffset;
+                if( psolution[i] > IKPI ) {
+                    psolution[i] -= IK2PI;
+                }
+                else if( psolution[i] < -IKPI ) {
+                    psolution[i] += IK2PI;
+                }
+            }
+        }
+    }
+
+    /// Gets the free parameters the solution requires to be set before a full solution can be returned
+    /// \return vector of indices indicating the free parameters
+    const std::vector<int>& GetFree() const { return vfree; }
+
+    struct VARIABLE
+    {
+        VARIABLE() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) {
+            indices[0] = indices[1] = -1;
+        }
+        IKReal fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset
+        signed char freeind; ///< if >= 0, mimics another joint
+        unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself
+        unsigned char indices[2]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root
+    };
+
+    std::vector<VARIABLE> basesol;       ///< solution and their offsets if joints are mimiced
+    std::vector<int> vfree;
+
+    bool Validate() const {
+        for(size_t i = 0; i < basesol.size(); ++i) {
+            if( basesol[i].maxsolutions == (unsigned char)-1) {
+                return false;
+            }
+            if( basesol[i].maxsolutions > 0 ) {
+                if( basesol[i].indices[0] >= basesol[i].maxsolutions ) {
+                    return false;
+                }
+                if( basesol[i].indices[1] != (unsigned char)-1 && basesol[i].indices[1] >= basesol[i].maxsolutions ) {
+                    return false;
+                }
+            }
+        }
+        return true;
+    }
+
+    void GetSolutionIndices(std::vector<unsigned int>& v) const {
+        v.resize(0);
+        v.push_back(0);
+        for(int i = (int)basesol.size()-1; i >= 0; --i) {
+            if( basesol[i].maxsolutions != (unsigned char)-1 && basesol[i].maxsolutions > 1 ) {
+                for(size_t j = 0; j < v.size(); ++j) {
+                    v[j] *= basesol[i].maxsolutions;
+                }
+                size_t orgsize=v.size();
+                if( basesol[i].indices[1] != (unsigned char)-1 ) {
+                    for(size_t j = 0; j < orgsize; ++j) {
+                        v.push_back(v[j]+basesol[i].indices[1]);
+                    }
+                }
+                if( basesol[i].indices[0] != (unsigned char)-1 ) {
+                    for(size_t j = 0; j < orgsize; ++j) {
+                        v[j] += basesol[i].indices[0];
+                    }
+                }
+            }
+        }
+    }
+};
+
+inline float IKabs(float f) { return fabsf(f); }
+inline double IKabs(double f) { return fabs(f); }
+
+inline float IKsqr(float f) { return f*f; }
+inline double IKsqr(double f) { return f*f; }
+
+inline float IKlog(float f) { return logf(f); }
+inline double IKlog(double f) { return log(f); }
+
+// allows asin and acos to exceed 1
+#ifndef IKFAST_SINCOS_THRESH
+#define IKFAST_SINCOS_THRESH ((IKReal)0.000001)
+#endif
+
+// used to check input to atan2 for degenerate cases
+#ifndef IKFAST_ATAN2_MAGTHRESH
+#define IKFAST_ATAN2_MAGTHRESH ((IKReal)2e-6)
+#endif
+
+// minimum distance of separate solutions
+#ifndef IKFAST_SOLUTION_THRESH
+#define IKFAST_SOLUTION_THRESH ((IKReal)1e-6)
+#endif
+
+inline float IKasin(float f)
+{
+IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
+if( f <= -1 ) return -IKPI_2;
+else if( f >= 1 ) return IKPI_2;
+return asinf(f);
+}
+inline double IKasin(double f)
+{
+IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
+if( f <= -1 ) return -IKPI_2;
+else if( f >= 1 ) return IKPI_2;
+return asin(f);
+}
+
+// return positive value in [0,y)
+inline float IKfmod(float x, float y)
+{
+    while(x < 0) {
+        x += y;
+    }
+    return fmodf(x,y);
+}
+
+// return positive value in [0,y)
+inline float IKfmod(double x, double y)
+{
+    while(x < 0) {
+        x += y;
+    }
+    return fmod(x,y);
+}
+
+inline float IKacos(float f)
+{
+IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
+if( f <= -1 ) return IKPI;
+else if( f >= 1 ) return 0;
+return acosf(f);
+}
+inline double IKacos(double f)
+{
+IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
+if( f <= -1 ) return IKPI;
+else if( f >= 1 ) return 0;
+return acos(f);
+}
+inline float IKsin(float f) { return sinf(f); }
+inline double IKsin(double f) { return sin(f); }
+inline float IKcos(float f) { return cosf(f); }
+inline double IKcos(double f) { return cos(f); }
+inline float IKtan(float f) { return tanf(f); }
+inline double IKtan(double f) { return tan(f); }
+inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
+inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
+inline float IKatan2(float fy, float fx) {
+    if( isnan(fy) ) {
+        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
+        return IKPI_2;
+    }
+    else if( isnan(fx) ) {
+        return 0;
+    }
+    return atan2f(fy,fx);
+}
+inline double IKatan2(double fy, double fx) {
+    if( isnan(fy) ) {
+        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
+        return IKPI_2;
+    }
+    else if( isnan(fx) ) {
+        return 0;
+    }
+    return atan2(fy,fx);
+}
+
+inline float IKsign(float f) {
+    if( f > 0 ) {
+        return 1.0f;
+    }
+    else if( f < 0 ) {
+        return -1.0f;
+    }
+    return 0;
+}
+
+inline double IKsign(double f) {
+    if( f > 0 ) {
+        return 1.0;
+    }
+    else if( f < 0 ) {
+        return -1.0;
+    }
+    return 0;
+}
+
+/// solves the forward kinematics equations.
+/// \param pfree is an array specifying the free joints of the chain.
+IKFAST_API void fk(const IKReal* j, IKReal* eetrans, IKReal* eerot) {
+IKReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53;
+x0=IKcos(j[0]);
+x1=IKcos(j[1]);
+x2=IKsin(j[2]);
+x3=IKcos(j[2]);
+x4=IKsin(j[1]);
+x5=IKsin(j[3]);
+x6=((x0)*(x1)*(x3));
+x7=((x0)*(x2)*(x4));
+x8=((x0)*(x1)*(x2));
+x9=((x0)*(x3)*(x4));
+x10=((x9)+(x8));
+x11=IKcos(j[3]);
+x12=IKsin(j[4]);
+x13=((x7)+(((-1.00000000000000)*(x6))));
+x14=((x11)*(x13));
+x15=((x10)*(x5));
+x16=((x15)+(x14));
+x17=IKcos(j[4]);
+x18=IKsin(j[0]);
+x19=IKsin(j[5]);
+x20=((x16)*(x17));
+x21=((x12)*(x18));
+x22=((((-1.00000000000000)*(x21)))+(x20));
+x23=IKcos(j[5]);
+x24=((x6)+(((-1.00000000000000)*(x7))));
+x25=((x24)*(x5));
+x26=((x10)*(x11));
+x27=((x25)+(x26));
+x28=((x1)*(x18)*(x2));
+x29=((x18)*(x3)*(x4));
+x30=((x28)+(x29));
+x31=((x18)*(x2)*(x4));
+x32=((x1)*(x18)*(x3));
+x33=((x30)*(x5));
+x34=((((-1.00000000000000)*(x32)))+(x31));
+x35=((x11)*(x34));
+x36=((x33)+(x35));
+x37=((x0)*(x12));
+x38=((x17)*(x36));
+x39=((x38)+(x37));
+x40=((((-1.00000000000000)*(x31)))+(x32));
+x41=((x40)*(x5));
+x42=((x11)*(x30));
+x43=((x42)+(x41));
+x44=((x1)*(x3));
+x45=((x2)*(x4));
+x46=((((-1.00000000000000)*(x45)))+(x44));
+x47=((x1)*(x2));
+x48=((x3)*(x4));
+x49=((x48)+(x47));
+x50=((x46)*(x5));
+x51=((x11)*(x49));
+x52=((x51)+(x50));
+x53=((-1.00000000000000)*(x49));
+eerot[0]=((((x22)*(x23)))+(((x19)*(x27))));
+eerot[1]=((((-1.00000000000000)*(x17)*(x18)))+(((-1.00000000000000)*(x12)*(x16))));
+eerot[2]=((((-1.00000000000000)*(x23)*(x27)))+(((x19)*(x22))));
+eetrans[0]=((((-0.109150000000000)*(x18)))+(((x11)*(((((-0.0946500000000000)*(x9)))+(((-0.0946500000000000)*(x8)))))))+(((0.392250000000000)*(x6)))+(((0.425000000000000)*(x0)*(x1)))+(((x5)*(((((0.0946500000000000)*(x7)))+(((-0.0946500000000000)*(x6)))))))+(((-0.392250000000000)*(x7))));
+eerot[3]=((((x19)*(x43)))+(((x23)*(x39))));
+eerot[4]=((((x0)*(x17)))+(((-1.00000000000000)*(x12)*(x36))));
+eerot[5]=((((x19)*(x39)))+(((-1.00000000000000)*(x23)*(x43))));
+eetrans[1]=((((x5)*(((((-0.0946500000000000)*(x32)))+(((0.0946500000000000)*(x31)))))))+(((0.425000000000000)*(x1)*(x18)))+(((0.392250000000000)*(x32)))+(((0.109150000000000)*(x0)))+(((x11)*(((((-0.0946500000000000)*(x29)))+(((-0.0946500000000000)*(x28)))))))+(((-0.392250000000000)*(x31))));
+eerot[6]=((((x17)*(x23)*(x52)))+(((x19)*(((((x11)*(x46)))+(((x5)*(x53))))))));
+eerot[7]=((-1.00000000000000)*(x12)*(x52));
+eerot[8]=((((x23)*(((((-1.00000000000000)*(x5)*(x53)))+(((-1.00000000000000)*(x11)*(x46)))))))+(((x17)*(x19)*(x52))));
+eetrans[2]=((0.0891590000000000)+(((x11)*(((((-0.0946500000000000)*(x44)))+(((0.0946500000000000)*(x45)))))))+(((-0.425000000000000)*(x4)))+(((x5)*(((((0.0946500000000000)*(x47)))+(((0.0946500000000000)*(x48)))))))+(((-0.392250000000000)*(x48)))+(((-0.392250000000000)*(x47))));
+}
+
+IKFAST_API int getNumFreeParameters() { return 0; }
+IKFAST_API int* getFreeParameters() { return NULL; }
+IKFAST_API int getNumJoints() { return 6; }
+
+IKFAST_API int getIKRealSize() { return sizeof(IKReal); }
+
+IKFAST_API int getIKType() { return 0x67000001; }
+
+class IKSolver {
+public:
+IKReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
+unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
+
+bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
+j0=numeric_limits<IKReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IKReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IKReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IKReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IKReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IKReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; 
+for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
+    vsolutions.resize(0); vsolutions.reserve(8);
+r00 = eerot[0*3+0];
+r01 = eerot[0*3+1];
+r02 = eerot[0*3+2];
+r10 = eerot[1*3+0];
+r11 = eerot[1*3+1];
+r12 = eerot[1*3+2];
+r20 = eerot[2*3+0];
+r21 = eerot[2*3+1];
+r22 = eerot[2*3+2];
+px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
+
+new_r00=r00;
+new_r01=((-1.00000000000000)*(r02));
+new_r02=r01;
+new_px=px;
+new_r10=r10;
+new_r11=((-1.00000000000000)*(r12));
+new_r12=r11;
+new_py=py;
+new_r20=r20;
+new_r21=((-1.00000000000000)*(r22));
+new_r22=r21;
+new_pz=((-0.0891590000000000)+(pz));
+r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
+pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
+npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
+npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
+npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
+rxp0_0=((((-1.00000000000000)*(py)*(r20)))+(((pz)*(r10))));
+rxp0_1=((((px)*(r20)))+(((-1.00000000000000)*(pz)*(r00))));
+rxp0_2=((((-1.00000000000000)*(px)*(r10)))+(((py)*(r00))));
+rxp1_0=((((-1.00000000000000)*(py)*(r21)))+(((pz)*(r11))));
+rxp1_1=((((px)*(r21)))+(((-1.00000000000000)*(pz)*(r01))));
+rxp1_2=((((-1.00000000000000)*(px)*(r11)))+(((py)*(r01))));
+rxp2_0=((((-1.00000000000000)*(py)*(r22)))+(((pz)*(r12))));
+rxp2_1=((((px)*(r22)))+(((-1.00000000000000)*(pz)*(r02))));
+rxp2_2=((((-1.00000000000000)*(px)*(r12)))+(((py)*(r02))));
+IKReal op[8+1], zeror[8];
+int numroots;
+IKReal gconst0;
+gconst0=((-1.00000000000000)*(r11));
+IKReal gconst1;
+gconst1=((-2.00000000000000)*(r10));
+IKReal gconst2;
+gconst2=r11;
+IKReal gconst3;
+gconst3=((-0.109150000000000)+(py)+(((0.0946500000000000)*(r11))));
+IKReal gconst4;
+gconst4=((0.189300000000000)*(r10));
+IKReal gconst5;
+gconst5=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(py));
+IKReal gconst6;
+gconst6=((-1.00000000000000)*(r11));
+IKReal gconst7;
+gconst7=((-2.00000000000000)*(r10));
+IKReal gconst8;
+gconst8=r11;
+IKReal gconst9;
+gconst9=((-0.109150000000000)+(py)+(((0.0946500000000000)*(r11))));
+IKReal gconst10;
+gconst10=((0.189300000000000)*(r10));
+IKReal gconst11;
+gconst11=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(py));
+IKReal gconst12;
+gconst12=((2.00000000000000)*(r01));
+IKReal gconst13;
+gconst13=((4.00000000000000)*(r00));
+IKReal gconst14;
+gconst14=((-2.00000000000000)*(r01));
+IKReal gconst15;
+gconst15=((((-0.189300000000000)*(r01)))+(((-2.00000000000000)*(px))));
+IKReal gconst16;
+gconst16=((-0.378600000000000)*(r00));
+IKReal gconst17;
+gconst17=((((0.189300000000000)*(r01)))+(((-2.00000000000000)*(px))));
+IKReal gconst18;
+gconst18=((2.00000000000000)*(r01));
+IKReal gconst19;
+gconst19=((4.00000000000000)*(r00));
+IKReal gconst20;
+gconst20=((-2.00000000000000)*(r01));
+IKReal gconst21;
+gconst21=((((-0.189300000000000)*(r01)))+(((-2.00000000000000)*(px))));
+IKReal gconst22;
+gconst22=((-0.378600000000000)*(r00));
+IKReal gconst23;
+gconst23=((((0.189300000000000)*(r01)))+(((-2.00000000000000)*(px))));
+IKReal gconst24;
+gconst24=r11;
+IKReal gconst25;
+gconst25=((2.00000000000000)*(r10));
+IKReal gconst26;
+gconst26=((-1.00000000000000)*(r11));
+IKReal gconst27;
+gconst27=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(((-1.00000000000000)*(py))));
+IKReal gconst28;
+gconst28=((-0.189300000000000)*(r10));
+IKReal gconst29;
+gconst29=((-0.109150000000000)+(((-1.00000000000000)*(py)))+(((0.0946500000000000)*(r11))));
+IKReal gconst30;
+gconst30=r11;
+IKReal gconst31;
+gconst31=((2.00000000000000)*(r10));
+IKReal gconst32;
+gconst32=((-1.00000000000000)*(r11));
+IKReal gconst33;
+gconst33=((-0.109150000000000)+(((-0.0946500000000000)*(r11)))+(((-1.00000000000000)*(py))));
+IKReal gconst34;
+gconst34=((-0.189300000000000)*(r10));
+IKReal gconst35;
+gconst35=((-0.109150000000000)+(((-1.00000000000000)*(py)))+(((0.0946500000000000)*(r11))));
+op[0]=((((gconst24)*(gconst29)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst32)*(gconst33)))+(((gconst26)*(gconst28)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst24)*(gconst29)*(gconst30)*(gconst35)))+(((gconst26)*(gconst27)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst26)*(gconst28)*(gconst30)*(gconst34)))+(((gconst25)*(gconst29)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst31)*(gconst33))));
+op[1]=((((gconst12)*(gconst29)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst16)*(gconst26)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst27)*(gconst32)))+(((gconst21)*(gconst26)*(gconst28)*(gconst31)))+(((-1.00000000000000)*(gconst18)*(gconst26)*(gconst28)*(gconst34)))+(((gconst23)*(gconst26)*(gconst27)*(gconst30)))+(((gconst15)*(gconst26)*(gconst30)*(gconst35)))+(((gconst13)*(gconst29)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst18)*(gconst24)*(gconst29)*(gconst35)))+(((gconst18)*(gconst26)*(gconst27)*(gconst35)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst31)*(gconst33)))+(((gconst17)*(gconst25)*(gconst30)*(gconst34)))+(((gconst16)*(gconst26)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst29)*(gconst31)))+(((-1.00000000000000)*(gconst17)*(gconst24)*(gconst30)*(gconst35)))+(((gconst22)*(gconst25)*(gconst29)*(gconst30)))+(((gconst14)*(gconst27)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst31)*(gconst33)))+(((gconst19)*(gconst26)*(gconst28)*(gconst33)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst27)*(gconst33)))+(((gconst14)*(gconst28)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst32)*(gconst33)))+(((gconst17)*(gconst24)*(gconst32)*(gconst33)))+(((gconst20)*(gconst24)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst12)*(gconst29)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst29)*(gconst30)))+(((gconst18)*(gconst25)*(gconst29)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst32)*(gconst33)))+(((gconst21)*(gconst24)*(gconst29)*(gconst32)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst28)*(gconst30)*(gconst34))));
+op[2]=((((-1.00000000000000)*(gconst20)*(gconst21)*(gconst26)*(gconst27)))+(((gconst14)*(gconst19)*(gconst28)*(gconst33)))+(((gconst13)*(gconst22)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst26)*(gconst34)))+(((-1.00000000000000)*(gconst24)*(gconst29)*(gconst35)*(gconst6)))+(((gconst24)*(gconst29)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst31)*(gconst33)))+(((gconst16)*(gconst19)*(gconst26)*(gconst33)))+(((-1.00000000000000)*(gconst0)*(gconst29)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst16)*(gconst22)*(gconst26)*(gconst30)))+(((gconst24)*(gconst32)*(gconst33)*(gconst5)))+(((gconst26)*(gconst3)*(gconst30)*(gconst35)))+(((gconst12)*(gconst17)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst26)*(gconst33)))+(((gconst26)*(gconst28)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst17)*(gconst19)*(gconst25)*(gconst33)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst28)*(gconst34)))+(((-1.00000000000000)*(gconst24)*(gconst30)*(gconst35)*(gconst5)))+(((gconst26)*(gconst28)*(gconst33)*(gconst7)))+(((gconst15)*(gconst18)*(gconst26)*(gconst35)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst30)*(gconst35)))+(((gconst12)*(gconst21)*(gconst29)*(gconst32)))+(((gconst13)*(gconst18)*(gconst29)*(gconst34)))+(((-1.00000000000000)*(gconst26)*(gconst30)*(gconst34)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst27)*(gconst33)))+(((-1.00000000000000)*(gconst2)*(gconst28)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst26)*(gconst28)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst31)*(gconst9)))+(((gconst16)*(gconst21)*(gconst26)*(gconst31)))+(((gconst17)*(gconst18)*(gconst25)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst27)*(gconst32)))+(((gconst24)*(gconst29)*(gconst32)*(gconst9)))+(((gconst12)*(gconst20)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst32)*(gconst33)))+(((gconst14)*(gconst21)*(gconst28)*(gconst31)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst33)*(gconst8)))+(((gconst17)*(gconst22)*(gconst25)*(gconst30)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst29)*(gconst33)))+(((gconst14)*(gconst16)*(gconst31)*(gconst33)))+(((gconst18)*(gconst23)*(gconst26)*(gconst27)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst24)*(gconst29)))+(((gconst14)*(gconst23)*(gconst27)*(gconst30)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst24)*(gconst35)))+(((gconst14)*(gconst18)*(gconst27)*(gconst35)))+(((-1.00000000000000)*(gconst17)*(gconst21)*(gconst25)*(gconst31)))+(((gconst1)*(gconst29)*(gconst30)*(gconst34)))+(((gconst2)*(gconst28)*(gconst31)*(gconst33)))+(((gconst13)*(gconst17)*(gconst30)*(gconst34)))+(((gconst2)*(gconst27)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst29)*(gconst31)))+(((gconst20)*(gconst21)*(gconst24)*(gconst29)))+(((gconst18)*(gconst22)*(gconst25)*(gconst29)))+(((gconst25)*(gconst30)*(gconst34)*(gconst5)))+(((gconst19)*(gconst21)*(gconst26)*(gconst28)))+(((gconst25)*(gconst29)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst29)*(gconst35)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst30)*(gconst34)))+(((gconst10)*(gconst25)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst32)*(gconst33)))+(((gconst17)*(gconst20)*(gconst24)*(gconst33)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst32)*(gconst33)))+(((gconst26)*(gconst31)*(gconst33)*(gconst4)))+(((-1.00000000000000)*(gconst15)*(gconst21)*(gconst26)*(gconst32)))+(((gconst26)*(gconst27)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst26)*(gconst28)))+(((gconst15)*(gconst23)*(gconst26)*(gconst30)))+(((-1.00000000000000)*(gconst19)*(gconst21)*(gconst25)*(gconst29)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst25)*(gconst31)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst23)*(gconst24)*(gconst30)))+(((gconst17)*(gconst21)*(gconst24)*(gconst32)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst31)*(gconst33)))+(((gconst14)*(gconst15)*(gconst30)*(gconst35)))+(((gconst0)*(gconst29)*(gconst32)*(gconst33)))+(((gconst11)*(gconst26)*(gconst27)*(gconst30))));
+op[3]=((((gconst12)*(gconst17)*(gconst21)*(gconst32)))+(((gconst12)*(gconst20)*(gconst21)*(gconst29)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst21)*(gconst29)))+(((-1.00000000000000)*(gconst18)*(gconst26)*(gconst34)*(gconst4)))+(((gconst12)*(gconst29)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst23)*(gconst29)))+(((gconst16)*(gconst26)*(gconst33)*(gconst7)))+(((gconst15)*(gconst18)*(gconst23)*(gconst26)))+(((gconst14)*(gconst28)*(gconst33)*(gconst7)))+(((gconst20)*(gconst24)*(gconst29)*(gconst9)))+(((gconst14)*(gconst15)*(gconst23)*(gconst30)))+(((gconst13)*(gconst30)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst21)*(gconst32)))+(((gconst17)*(gconst24)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst24)*(gconst35)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst18)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst28)*(gconst30)))+(((gconst14)*(gconst16)*(gconst21)*(gconst31)))+(((gconst2)*(gconst21)*(gconst28)*(gconst31)))+(((gconst15)*(gconst26)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst17)*(gconst24)*(gconst30)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst18)*(gconst35)))+(((gconst18)*(gconst25)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst23)*(gconst30)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst17)*(gconst18)*(gconst23)*(gconst24)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst30)*(gconst5)))+(((gconst10)*(gconst18)*(gconst25)*(gconst29)))+(((gconst14)*(gconst19)*(gconst21)*(gconst28)))+(((gconst11)*(gconst14)*(gconst27)*(gconst30)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst29)*(gconst9)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst21)*(gconst26)))+(((gconst19)*(gconst26)*(gconst33)*(gconst4)))+(((gconst16)*(gconst19)*(gconst21)*(gconst26)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst31)*(gconst9)))+(((gconst17)*(gconst18)*(gconst22)*(gconst25)))+(((gconst20)*(gconst24)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst27)*(gconst8)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst21)*(gconst27)))+(((gconst12)*(gconst17)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst31)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst30)*(gconst34)))+(((gconst22)*(gconst25)*(gconst30)*(gconst5)))+(((gconst16)*(gconst2)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst29)*(gconst35)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst21)*(gconst31)))+(((-1.00000000000000)*(gconst12)*(gconst30)*(gconst35)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst28)*(gconst34)*(gconst6)))+(((gconst14)*(gconst27)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst17)*(gconst24)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst29)*(gconst30)))+(((gconst12)*(gconst29)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst19)*(gconst33)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst29)*(gconst6)))+(((gconst17)*(gconst25)*(gconst34)*(gconst6)))+(((gconst11)*(gconst15)*(gconst26)*(gconst30)))+(((gconst18)*(gconst26)*(gconst3)*(gconst35)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst26)*(gconst28)))+(((gconst21)*(gconst26)*(gconst28)*(gconst7)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst28)*(gconst6)))+(((gconst21)*(gconst24)*(gconst32)*(gconst5)))+(((gconst10)*(gconst13)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst32)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst32)*(gconst33)))+(((gconst19)*(gconst2)*(gconst28)*(gconst33)))+(((-1.00000000000000)*(gconst11)*(gconst18)*(gconst24)*(gconst29)))+(((gconst22)*(gconst25)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst26)*(gconst30)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst32)*(gconst9)))+(((gconst1)*(gconst17)*(gconst30)*(gconst34)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst22)*(gconst26)))+(((gconst18)*(gconst2)*(gconst27)*(gconst35)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst33)*(gconst8)))+(((gconst23)*(gconst26)*(gconst3)*(gconst30)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst27)*(gconst32)))+(((gconst0)*(gconst17)*(gconst32)*(gconst33)))+(((gconst0)*(gconst20)*(gconst29)*(gconst33)))+(((gconst12)*(gconst32)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst3)*(gconst32)))+(((gconst2)*(gconst23)*(gconst27)*(gconst30)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst29)*(gconst33)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst31)*(gconst33)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst29)*(gconst7)))+(((gconst14)*(gconst28)*(gconst31)*(gconst9)))+(((gconst14)*(gconst3)*(gconst30)*(gconst35)))+(((gconst1)*(gconst18)*(gconst29)*(gconst34)))+(((-1.00000000000000)*(gconst13)*(gconst31)*(gconst33)*(gconst5)))+(((gconst14)*(gconst18)*(gconst23)*(gconst27)))+(((gconst14)*(gconst15)*(gconst18)*(gconst35)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst27)*(gconst9)))+(((gconst14)*(gconst16)*(gconst19)*(gconst33)))+(((gconst17)*(gconst24)*(gconst33)*(gconst8)))+(((gconst13)*(gconst29)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst30)*(gconst34)*(gconst4)))+(((gconst0)*(gconst21)*(gconst29)*(gconst32)))+(((-1.00000000000000)*(gconst17)*(gconst19)*(gconst21)*(gconst25)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst28)))+(((gconst10)*(gconst17)*(gconst25)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst22)*(gconst30)))+(((-1.00000000000000)*(gconst16)*(gconst26)*(gconst34)*(gconst6)))+(((gconst14)*(gconst31)*(gconst33)*(gconst4)))+(((gconst13)*(gconst18)*(gconst22)*(gconst29)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst33)*(gconst8)))+(((gconst17)*(gconst20)*(gconst21)*(gconst24)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst30)*(gconst35)))+(((gconst23)*(gconst26)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst29)*(gconst35)*(gconst6)))+(((gconst19)*(gconst26)*(gconst28)*(gconst9)))+(((gconst21)*(gconst26)*(gconst31)*(gconst4)))+(((gconst13)*(gconst17)*(gconst18)*(gconst34)))+(((gconst15)*(gconst2)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst3)*(gconst33)))+(((gconst16)*(gconst26)*(gconst31)*(gconst9)))+(((gconst21)*(gconst24)*(gconst29)*(gconst8)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst28)*(gconst34)))+(((gconst11)*(gconst18)*(gconst26)*(gconst27)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst21)*(gconst29)*(gconst31)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst27)*(gconst33)))+(((gconst13)*(gconst17)*(gconst22)*(gconst30)))+(((gconst1)*(gconst22)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst28)*(gconst30))));
+op[4]=((((gconst14)*(gconst15)*(gconst35)*(gconst6)))+(((gconst12)*(gconst20)*(gconst29)*(gconst9)))+(((gconst12)*(gconst21)*(gconst32)*(gconst5)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst21)*(gconst27)))+(((gconst10)*(gconst13)*(gconst18)*(gconst29)))+(((gconst0)*(gconst20)*(gconst21)*(gconst29)))+(((gconst14)*(gconst23)*(gconst27)*(gconst6)))+(((gconst14)*(gconst16)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst23)*(gconst30)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst35)*(gconst5)))+(((gconst10)*(gconst13)*(gconst17)*(gconst30)))+(((gconst15)*(gconst23)*(gconst26)*(gconst6)))+(((gconst13)*(gconst17)*(gconst18)*(gconst22)))+(((gconst26)*(gconst28)*(gconst7)*(gconst9)))+(((gconst1)*(gconst29)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst31)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst28)*(gconst30)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst17)*(gconst30)))+(((gconst2)*(gconst28)*(gconst31)*(gconst9)))+(((gconst0)*(gconst17)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst28)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst33)*(gconst7)))+(((gconst12)*(gconst21)*(gconst29)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst30)*(gconst35)*(gconst5)))+(((gconst16)*(gconst2)*(gconst21)*(gconst31)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst32)))+(((-1.00000000000000)*(gconst24)*(gconst35)*(gconst5)*(gconst6)))+(((gconst25)*(gconst34)*(gconst5)*(gconst6)))+(((gconst0)*(gconst17)*(gconst21)*(gconst32)))+(((gconst12)*(gconst17)*(gconst33)*(gconst8)))+(((gconst1)*(gconst17)*(gconst18)*(gconst34)))+(((-1.00000000000000)*(gconst18)*(gconst23)*(gconst24)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst22)*(gconst30)))+(((-1.00000000000000)*(gconst1)*(gconst31)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst19)*(gconst25)*(gconst9)))+(((gconst11)*(gconst2)*(gconst27)*(gconst30)))+(((gconst13)*(gconst17)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst16)*(gconst30)))+(((gconst0)*(gconst29)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst22)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst21)*(gconst29)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst3)*(gconst33)))+(((-1.00000000000000)*(gconst15)*(gconst21)*(gconst26)*(gconst8)))+(((gconst14)*(gconst16)*(gconst31)*(gconst9)))+(((gconst2)*(gconst27)*(gconst35)*(gconst6)))+(((gconst18)*(gconst23)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst15)*(gconst20)*(gconst26)*(gconst9)))+(((gconst14)*(gconst21)*(gconst31)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst29)*(gconst35)*(gconst6)))+(((gconst2)*(gconst3)*(gconst30)*(gconst35)))+(((-1.00000000000000)*(gconst19)*(gconst21)*(gconst25)*(gconst5)))+(((gconst19)*(gconst2)*(gconst21)*(gconst28)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst18)*(gconst28)))+(((gconst19)*(gconst21)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst26)*(gconst34)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst18)*(gconst23)))+(((gconst10)*(gconst17)*(gconst18)*(gconst25)))+(((gconst26)*(gconst3)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst19)*(gconst33)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst20)*(gconst21)))+(((gconst0)*(gconst32)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst17)*(gconst21)*(gconst25)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst29)*(gconst7)))+(((gconst11)*(gconst14)*(gconst18)*(gconst27)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst20)*(gconst33)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst29)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst3)*(gconst32)))+(((gconst11)*(gconst14)*(gconst15)*(gconst30)))+(((-1.00000000000000)*(gconst26)*(gconst27)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst25)*(gconst31)*(gconst5)*(gconst9)))+(((gconst12)*(gconst17)*(gconst20)*(gconst21)))+(((gconst12)*(gconst20)*(gconst33)*(gconst5)))+(((gconst18)*(gconst22)*(gconst25)*(gconst5)))+(((gconst2)*(gconst28)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst19)*(gconst21)))+(((gconst26)*(gconst33)*(gconst4)*(gconst7)))+(((gconst16)*(gconst21)*(gconst26)*(gconst7)))+(((gconst16)*(gconst19)*(gconst26)*(gconst9)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst16)*(gconst22)*(gconst26)*(gconst6)))+(((gconst17)*(gconst21)*(gconst24)*(gconst8)))+(((gconst14)*(gconst18)*(gconst3)*(gconst35)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst33)*(gconst8)))+(((gconst18)*(gconst2)*(gconst23)*(gconst27)))+(((gconst26)*(gconst31)*(gconst4)*(gconst9)))+(((gconst24)*(gconst32)*(gconst5)*(gconst9)))+(((gconst10)*(gconst25)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst20)*(gconst21)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst27)*(gconst9)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst17)*(gconst23)*(gconst24)*(gconst6)))+(((gconst13)*(gconst22)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst28)*(gconst34)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst30)*(gconst34)*(gconst4)))+(((gconst1)*(gconst30)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst31)*(gconst9)))+(((gconst14)*(gconst15)*(gconst18)*(gconst23)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst21)*(gconst31)))+(((gconst24)*(gconst33)*(gconst5)*(gconst8)))+(((gconst15)*(gconst2)*(gconst23)*(gconst30)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst18)*(gconst35)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst28)*(gconst6)))+(((gconst14)*(gconst19)*(gconst28)*(gconst9)))+(((gconst10)*(gconst25)*(gconst29)*(gconst6)))+(((gconst15)*(gconst18)*(gconst2)*(gconst35)))+(((gconst17)*(gconst22)*(gconst25)*(gconst6)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst29)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst2)*(gconst34)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst27)*(gconst8)))+(((gconst14)*(gconst23)*(gconst3)*(gconst30)))+(((gconst24)*(gconst29)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst18)*(gconst29)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst22)*(gconst28)))+(((-1.00000000000000)*(gconst25)*(gconst33)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst34)*(gconst6)))+(((gconst2)*(gconst31)*(gconst33)*(gconst4)))+(((gconst1)*(gconst18)*(gconst22)*(gconst29)))+(((gconst17)*(gconst20)*(gconst24)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst17)*(gconst18)*(gconst24)))+(((-1.00000000000000)*(gconst25)*(gconst29)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst18)*(gconst22)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst23)*(gconst29)))+(((gconst14)*(gconst19)*(gconst33)*(gconst4)))+(((gconst20)*(gconst21)*(gconst24)*(gconst5)))+(((gconst11)*(gconst26)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst33)*(gconst8)))+(((gconst11)*(gconst15)*(gconst18)*(gconst26)))+(((gconst1)*(gconst17)*(gconst22)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst34)*(gconst4)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst32)*(gconst33)))+(((gconst13)*(gconst18)*(gconst34)*(gconst5)))+(((gconst1)*(gconst10)*(gconst29)*(gconst30)))+(((gconst16)*(gconst19)*(gconst2)*(gconst33)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst18)*(gconst26)))+(((gconst14)*(gconst16)*(gconst19)*(gconst21)))+(((gconst12)*(gconst17)*(gconst32)*(gconst9)))+(((gconst0)*(gconst29)*(gconst32)*(gconst9)))+(((gconst13)*(gconst22)*(gconst30)*(gconst5)))+(((gconst11)*(gconst26)*(gconst3)*(gconst30)))+(((gconst14)*(gconst21)*(gconst28)*(gconst7))));
+op[5]=((((gconst13)*(gconst18)*(gconst22)*(gconst5)))+(((gconst14)*(gconst3)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst19)*(gconst25)*(gconst5)*(gconst9)))+(((gconst2)*(gconst23)*(gconst3)*(gconst30)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst26)*(gconst6)))+(((gconst12)*(gconst20)*(gconst21)*(gconst5)))+(((gconst16)*(gconst19)*(gconst2)*(gconst21)))+(((gconst14)*(gconst15)*(gconst23)*(gconst6)))+(((gconst1)*(gconst17)*(gconst18)*(gconst22)))+(((gconst2)*(gconst23)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst17)*(gconst23)*(gconst6)))+(((gconst0)*(gconst21)*(gconst32)*(gconst5)))+(((-1.00000000000000)*(gconst13)*(gconst33)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst33)*(gconst8)))+(((gconst14)*(gconst19)*(gconst21)*(gconst4)))+(((gconst21)*(gconst26)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst17)*(gconst24)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst26)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst34)*(gconst6)))+(((gconst10)*(gconst17)*(gconst25)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst19)*(gconst21)))+(((gconst2)*(gconst21)*(gconst28)*(gconst7)))+(((gconst14)*(gconst16)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst30)*(gconst5)))+(((gconst14)*(gconst28)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst27)*(gconst8)))+(((-1.00000000000000)*(gconst22)*(gconst26)*(gconst4)*(gconst6)))+(((gconst16)*(gconst2)*(gconst33)*(gconst7)))+(((gconst11)*(gconst14)*(gconst27)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst21)*(gconst8)))+(((gconst1)*(gconst18)*(gconst34)*(gconst5)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst27)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst29)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst26)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst34)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst17)*(gconst18)))+(((gconst17)*(gconst24)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst13)*(gconst31)*(gconst5)*(gconst9)))+(((gconst13)*(gconst17)*(gconst22)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst32)*(gconst9)))+(((gconst10)*(gconst13)*(gconst29)*(gconst6)))+(((gconst1)*(gconst22)*(gconst29)*(gconst6)))+(((gconst11)*(gconst14)*(gconst15)*(gconst18)))+(((gconst19)*(gconst2)*(gconst33)*(gconst4)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst16)*(gconst18)))+(((gconst0)*(gconst21)*(gconst29)*(gconst8)))+(((gconst15)*(gconst2)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst27)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst33)*(gconst7)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst21)*(gconst3)))+(((gconst16)*(gconst2)*(gconst31)*(gconst9)))+(((gconst10)*(gconst13)*(gconst30)*(gconst5)))+(((gconst0)*(gconst20)*(gconst33)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst29)*(gconst9)))+(((gconst19)*(gconst26)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst18)*(gconst29)))+(((gconst11)*(gconst14)*(gconst3)*(gconst30)))+(((gconst22)*(gconst25)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst18)*(gconst23)))+(((-1.00000000000000)*(gconst12)*(gconst18)*(gconst23)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst2)*(gconst30)))+(((gconst2)*(gconst21)*(gconst31)*(gconst4)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst3)*(gconst33)))+(((gconst0)*(gconst17)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst35)*(gconst5)))+(((-1.00000000000000)*(gconst21)*(gconst26)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst17)*(gconst30)))+(((gconst13)*(gconst34)*(gconst5)*(gconst6)))+(((gconst16)*(gconst26)*(gconst7)*(gconst9)))+(((gconst14)*(gconst31)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst19)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst21)*(gconst29)*(gconst7)))+(((gconst10)*(gconst18)*(gconst25)*(gconst5)))+(((-1.00000000000000)*(gconst12)*(gconst35)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst31)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst2)*(gconst28)))+(((gconst12)*(gconst17)*(gconst21)*(gconst8)))+(((gconst11)*(gconst15)*(gconst2)*(gconst30)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst34)*(gconst4)))+(((gconst11)*(gconst18)*(gconst2)*(gconst27)))+(((gconst0)*(gconst20)*(gconst29)*(gconst9)))+(((gconst0)*(gconst17)*(gconst33)*(gconst8)))+(((-1.00000000000000)*(gconst17)*(gconst25)*(gconst7)*(gconst9)))+(((gconst20)*(gconst24)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst20)*(gconst26)*(gconst3)*(gconst9)))+(((gconst23)*(gconst26)*(gconst3)*(gconst6)))+(((gconst12)*(gconst33)*(gconst5)*(gconst8)))+(((gconst12)*(gconst32)*(gconst5)*(gconst9)))+(((gconst11)*(gconst18)*(gconst26)*(gconst3)))+(((-1.00000000000000)*(gconst21)*(gconst25)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst14)*(gconst18)*(gconst22)*(gconst4)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst21)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst28)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst28)*(gconst6)))+(((gconst1)*(gconst22)*(gconst30)*(gconst5)))+(((gconst15)*(gconst18)*(gconst2)*(gconst23)))+(((gconst14)*(gconst16)*(gconst19)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst18)*(gconst24)*(gconst5)))+(((gconst1)*(gconst17)*(gconst34)*(gconst6)))+(((gconst10)*(gconst13)*(gconst17)*(gconst18)))+(((-1.00000000000000)*(gconst23)*(gconst24)*(gconst5)*(gconst6)))+(((gconst0)*(gconst17)*(gconst20)*(gconst21)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst3)*(gconst32)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst30)*(gconst4)))+(((gconst12)*(gconst17)*(gconst20)*(gconst9)))+(((gconst12)*(gconst29)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst35)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst16)*(gconst22)*(gconst6)))+(((-1.00000000000000)*(gconst16)*(gconst18)*(gconst2)*(gconst22)))+(((gconst19)*(gconst2)*(gconst28)*(gconst9)))+(((gconst18)*(gconst2)*(gconst3)*(gconst35)))+(((gconst14)*(gconst33)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst29)*(gconst6)))+(((gconst11)*(gconst15)*(gconst26)*(gconst6)))+(((gconst1)*(gconst10)*(gconst17)*(gconst30)))+(((-1.00000000000000)*(gconst1)*(gconst21)*(gconst31)*(gconst5)))+(((gconst21)*(gconst24)*(gconst5)*(gconst8)))+(((gconst1)*(gconst10)*(gconst18)*(gconst29)))+(((gconst14)*(gconst18)*(gconst23)*(gconst3))));
+op[6]=((((gconst14)*(gconst21)*(gconst4)*(gconst7)))+(((-1.00000000000000)*(gconst13)*(gconst17)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst21)*(gconst5)*(gconst7)))+(((gconst0)*(gconst33)*(gconst5)*(gconst8)))+(((gconst1)*(gconst10)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst33)*(gconst8)))+(((gconst14)*(gconst23)*(gconst3)*(gconst6)))+(((gconst1)*(gconst10)*(gconst17)*(gconst18)))+(((gconst13)*(gconst22)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst12)*(gconst23)*(gconst5)*(gconst6)))+(((gconst12)*(gconst20)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst17)*(gconst23)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst21)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst2)*(gconst34)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst33)*(gconst5)*(gconst7)))+(((-1.00000000000000)*(gconst0)*(gconst35)*(gconst5)*(gconst6)))+(((gconst24)*(gconst5)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst21)*(gconst5)))+(((gconst2)*(gconst3)*(gconst35)*(gconst6)))+(((gconst0)*(gconst20)*(gconst21)*(gconst5)))+(((-1.00000000000000)*(gconst0)*(gconst18)*(gconst23)*(gconst5)))+(((gconst15)*(gconst2)*(gconst23)*(gconst6)))+(((gconst1)*(gconst18)*(gconst22)*(gconst5)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst19)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst15)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst30)*(gconst4)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst21)*(gconst7)))+(((gconst10)*(gconst25)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst17)*(gconst6)))+(((gconst14)*(gconst16)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst27)*(gconst8)*(gconst9)))+(((gconst11)*(gconst2)*(gconst27)*(gconst6)))+(((gconst0)*(gconst32)*(gconst5)*(gconst9)))+(((gconst19)*(gconst2)*(gconst21)*(gconst4)))+(((gconst11)*(gconst26)*(gconst3)*(gconst6)))+(((gconst12)*(gconst21)*(gconst5)*(gconst8)))+(((gconst11)*(gconst15)*(gconst18)*(gconst2)))+(((gconst1)*(gconst17)*(gconst22)*(gconst6)))+(((gconst10)*(gconst13)*(gconst17)*(gconst6)))+(((-1.00000000000000)*(gconst10)*(gconst26)*(gconst4)*(gconst6)))+(((gconst16)*(gconst19)*(gconst2)*(gconst9)))+(((gconst18)*(gconst2)*(gconst23)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst18)*(gconst4)))+(((-1.00000000000000)*(gconst26)*(gconst3)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst32)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst21)*(gconst3)))+(((gconst16)*(gconst2)*(gconst21)*(gconst7)))+(((-1.00000000000000)*(gconst1)*(gconst31)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst1)*(gconst29)*(gconst7)*(gconst9)))+(((gconst1)*(gconst34)*(gconst5)*(gconst6)))+(((gconst10)*(gconst13)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst16)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst17)*(gconst18)))+(((-1.00000000000000)*(gconst14)*(gconst20)*(gconst3)*(gconst9)))+(((gconst26)*(gconst4)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst18)*(gconst2)*(gconst22)*(gconst4)))+(((gconst1)*(gconst10)*(gconst30)*(gconst5)))+(((gconst2)*(gconst28)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst29)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst24)*(gconst5)*(gconst6)))+(((gconst11)*(gconst14)*(gconst15)*(gconst6)))+(((gconst12)*(gconst17)*(gconst8)*(gconst9)))+(((gconst11)*(gconst2)*(gconst3)*(gconst30)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst18)*(gconst2)))+(((-1.00000000000000)*(gconst25)*(gconst5)*(gconst7)*(gconst9)))+(((gconst0)*(gconst29)*(gconst8)*(gconst9)))+(((gconst14)*(gconst19)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst16)*(gconst2)*(gconst22)*(gconst6)))+(((gconst11)*(gconst14)*(gconst18)*(gconst3)))+(((gconst0)*(gconst17)*(gconst20)*(gconst9)))+(((-1.00000000000000)*(gconst14)*(gconst22)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst30)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst28)*(gconst6)))+(((gconst2)*(gconst31)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst19)*(gconst5)*(gconst9)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst18)*(gconst5)))+(((gconst2)*(gconst33)*(gconst4)*(gconst7)))+(((gconst0)*(gconst17)*(gconst21)*(gconst8)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst21)*(gconst8))));
+op[7]=((((-1.00000000000000)*(gconst1)*(gconst21)*(gconst5)*(gconst7)))+(((gconst0)*(gconst20)*(gconst5)*(gconst9)))+(((gconst2)*(gconst23)*(gconst3)*(gconst6)))+(((-1.00000000000000)*(gconst15)*(gconst2)*(gconst8)*(gconst9)))+(((gconst1)*(gconst22)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst21)*(gconst3)*(gconst8)))+(((-1.00000000000000)*(gconst1)*(gconst17)*(gconst7)*(gconst9)))+(((gconst0)*(gconst17)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst13)*(gconst5)*(gconst7)*(gconst9)))+(((gconst14)*(gconst4)*(gconst7)*(gconst9)))+(((gconst11)*(gconst18)*(gconst2)*(gconst3)))+(((-1.00000000000000)*(gconst10)*(gconst14)*(gconst4)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst20)*(gconst3)*(gconst9)))+(((gconst2)*(gconst21)*(gconst4)*(gconst7)))+(((gconst1)*(gconst10)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst1)*(gconst19)*(gconst5)*(gconst9)))+(((gconst0)*(gconst21)*(gconst5)*(gconst8)))+(((-1.00000000000000)*(gconst10)*(gconst18)*(gconst2)*(gconst4)))+(((gconst10)*(gconst13)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst14)*(gconst3)*(gconst8)*(gconst9)))+(((gconst19)*(gconst2)*(gconst4)*(gconst9)))+(((-1.00000000000000)*(gconst2)*(gconst22)*(gconst4)*(gconst6)))+(((gconst1)*(gconst10)*(gconst17)*(gconst6)))+(((gconst11)*(gconst14)*(gconst3)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst17)*(gconst6)))+(((gconst11)*(gconst15)*(gconst2)*(gconst6)))+(((-1.00000000000000)*(gconst0)*(gconst23)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst11)*(gconst12)*(gconst5)*(gconst6)))+(((gconst12)*(gconst5)*(gconst8)*(gconst9)))+(((gconst16)*(gconst2)*(gconst7)*(gconst9)))+(((-1.00000000000000)*(gconst0)*(gconst11)*(gconst18)*(gconst5)))+(((-1.00000000000000)*(gconst10)*(gconst16)*(gconst2)*(gconst6))));
+op[8]=((((-1.00000000000000)*(gconst0)*(gconst11)*(gconst5)*(gconst6)))+(((-1.00000000000000)*(gconst2)*(gconst3)*(gconst8)*(gconst9)))+(((gconst0)*(gconst5)*(gconst8)*(gconst9)))+(((-1.00000000000000)*(gconst10)*(gconst2)*(gconst4)*(gconst6)))+(((gconst11)*(gconst2)*(gconst3)*(gconst6)))+(((-1.00000000000000)*(gconst1)*(gconst5)*(gconst7)*(gconst9)))+(((gconst2)*(gconst4)*(gconst7)*(gconst9)))+(((gconst1)*(gconst10)*(gconst5)*(gconst6))));
+polyroots8(op,zeror,numroots);
+IKReal j0array[8], cj0array[8], sj0array[8], tempj0array[1];
+int numsolutions = 0;
+for(int ij0 = 0; ij0 < numroots; ++ij0)
+{
+IKReal htj0 = zeror[ij0];
+tempj0array[0]=((2.00000000000000)*(atan(htj0)));
+for(int kj0 = 0; kj0 < 1; ++kj0)
+{
+j0array[numsolutions] = tempj0array[kj0];
+if( j0array[numsolutions] > IKPI )
+{
+    j0array[numsolutions]-=IK2PI;
+}
+else if( j0array[numsolutions] < -IKPI )
+{
+    j0array[numsolutions]+=IK2PI;
+}
+sj0array[numsolutions] = IKsin(j0array[numsolutions]);
+cj0array[numsolutions] = IKcos(j0array[numsolutions]);
+numsolutions++;
+}
+}
+bool j0valid[8]={true,true,true,true,true,true,true,true};
+_nj0 = 8;
+for(int ij0 = 0; ij0 < numsolutions; ++ij0)
+    {
+if( !j0valid[ij0] )
+{
+    continue;
+}
+    j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
+htj0 = IKtan(j0/2);
+
+_ij0[0] = ij0; _ij0[1] = -1;
+for(int iij0 = ij0+1; iij0 < numsolutions; ++iij0)
+{
+if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
+{
+    j0valid[iij0]=false; _ij0[1] = iij0; break; 
+}
+}
+{
+IKReal j5array[2], cj5array[2], sj5array[2];
+bool j5valid[2]={false};
+_nj5 = 2;
+IKReal x54=((r01)*(sj0));
+IKReal x55=((cj0)*(r11));
+IKReal x56=((x54)+(((-1.00000000000000)*(x55))));
+IKReal x57=((r00)*(sj0));
+IKReal x58=((cj0)*(r10));
+IKReal x59=((x57)+(((-1.00000000000000)*(x58))));
+if( IKabs(x56) < IKFAST_ATAN2_MAGTHRESH && IKabs(x59) < IKFAST_ATAN2_MAGTHRESH )
+    continue;
+IKReal x60=IKatan2(x56, x59);
+j5array[0]=((-1.00000000000000)*(x60));
+sj5array[0]=IKsin(j5array[0]);
+cj5array[0]=IKcos(j5array[0]);
+j5array[1]=((3.14159265358979)+(((-1.00000000000000)*(x60))));
+sj5array[1]=IKsin(j5array[1]);
+cj5array[1]=IKcos(j5array[1]);
+if( j5array[0] > IKPI )
+{
+    j5array[0]-=IK2PI;
+}
+else if( j5array[0] < -IKPI )
+{    j5array[0]+=IK2PI;
+}
+j5valid[0] = true;
+if( j5array[1] > IKPI )
+{
+    j5array[1]-=IK2PI;
+}
+else if( j5array[1] < -IKPI )
+{    j5array[1]+=IK2PI;
+}
+j5valid[1] = true;
+for(int ij5 = 0; ij5 < 2; ++ij5)
+{
+if( !j5valid[ij5] )
+{
+    continue;
+}
+_ij5[0] = ij5; _ij5[1] = -1;
+for(int iij5 = ij5+1; iij5 < 2; ++iij5)
+{
+if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
+{
+    j5valid[iij5]=false; _ij5[1] = iij5; break; 
+}
+}
+j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
+{
+IKReal evalcond[1];
+IKReal x61=IKsin(j5);
+IKReal x62=IKcos(j5);
+evalcond[0]=((-0.109150000000000)+(((0.0946500000000000)*(cj0)*(r11)*(x62)))+(((-1.00000000000000)*(px)*(sj0)))+(((-0.0946500000000000)*(r00)*(sj0)*(x61)))+(((0.0946500000000000)*(cj0)*(r10)*(x61)))+(((-0.0946500000000000)*(r01)*(sj0)*(x62)))+(((cj0)*(py))));
+if( IKabs(evalcond[0]) > 0.000001  )
+{
+continue;
+}
+}
+
+{
+IKReal dummyeval[1];
+IKReal gconst52;
+gconst52=IKsign(((((r01)*(r22)*(sj0)*(sj5)))+(((-1.00000000000000)*(r02)*(r21)*(sj0)*(sj5)))+(((cj5)*(r02)*(r20)*(sj0)))+(((-1.00000000000000)*(cj5)*(r00)*(r22)*(sj0)))+(((cj0)*(cj5)*(r10)*(r22)))+(((cj0)*(r12)*(r21)*(sj5)))+(((-1.00000000000000)*(cj0)*(r11)*(r22)*(sj5)))+(((-1.00000000000000)*(cj0)*(cj5)*(r12)*(r20)))));
+dummyeval[0]=((((r01)*(r22)*(sj0)*(sj5)))+(((-1.00000000000000)*(r02)*(r21)*(sj0)*(sj5)))+(((cj5)*(r02)*(r20)*(sj0)))+(((-1.00000000000000)*(cj5)*(r00)*(r22)*(sj0)))+(((cj0)*(cj5)*(r10)*(r22)))+(((cj0)*(r12)*(r21)*(sj5)))+(((-1.00000000000000)*(cj0)*(r11)*(r22)*(sj5)))+(((-1.00000000000000)*(cj0)*(cj5)*(r12)*(r20))));
+if( IKabs(dummyeval[0]) < 0.0000010000000000  )
+{
+continue;
+
+} else
+{
+{
+IKReal j4array[1], cj4array[1], sj4array[1];
+bool j4valid[1]={false};
+_nj4 = 1;
+if( IKabs(((gconst52)*(r22))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((r21)*(sj5)))+(((-1.00000000000000)*(cj5)*(r20))))))) < IKFAST_ATAN2_MAGTHRESH )
+    continue;
+j4array[0]=IKatan2(((gconst52)*(r22)), ((gconst52)*(((((r21)*(sj5)))+(((-1.00000000000000)*(cj5)*(r20)))))));
+sj4array[0]=IKsin(j4array[0]);
+cj4array[0]=IKcos(j4array[0]);
+if( j4array[0] > IKPI )
+{
+    j4array[0]-=IK2PI;
+}
+else if( j4array[0] < -IKPI )
+{    j4array[0]+=IK2PI;
+}
+j4valid[0] = true;
+for(int ij4 = 0; ij4 < 1; ++ij4)
+{
+if( !j4valid[ij4] )
+{
+    continue;
+}
+_ij4[0] = ij4; _ij4[1] = -1;
+for(int iij4 = ij4+1; iij4 < 1; ++iij4)
+{
+if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
+{
+    j4valid[iij4]=false; _ij4[1] = iij4; break; 
+}
+}
+j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
+{
+IKReal evalcond[4];
+IKReal x63=IKsin(j4);
+IKReal x64=IKcos(j4);
+evalcond[0]=((((r22)*(x64)))+(((-1.00000000000000)*(r21)*(sj5)*(x63)))+(((cj5)*(r20)*(x63))));
+evalcond[1]=((((-1.00000000000000)*(cj0)*(cj5)*(r10)*(x64)))+(((cj0)*(r11)*(sj5)*(x64)))+(((cj0)*(r12)*(x63)))+(((-1.00000000000000)*(r02)*(sj0)*(x63)))+(((-1.00000000000000)*(r01)*(sj0)*(sj5)*(x64)))+(((cj5)*(r00)*(sj0)*(x64))));
+evalcond[2]=((((r11)*(sj0)*(sj5)*(x63)))+(((-1.00000000000000)*(cj0)*(cj5)*(r00)*(x63)))+(((-1.00000000000000)*(cj5)*(r10)*(sj0)*(x63)))+(((-1.00000000000000)*(cj0)*(r02)*(x64)))+(((-1.00000000000000)*(r12)*(sj0)*(x64)))+(((cj0)*(r01)*(sj5)*(x63))));
+evalcond[3]=((1.00000000000000)+(((-1.00000000000000)*(cj0)*(cj5)*(r10)*(x63)))+(((r02)*(sj0)*(x64)))+(((cj0)*(r11)*(sj5)*(x63)))+(((-1.00000000000000)*(r01)*(sj0)*(sj5)*(x63)))+(((cj5)*(r00)*(sj0)*(x63)))+(((-1.00000000000000)*(cj0)*(r12)*(x64))));
+if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
+{
+continue;
+}
+}
+
+{
+IKReal dummyeval[1];
+IKReal gconst53;
+gconst53=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst54;
+gconst54=0.784500000000000;
+IKReal gconst55;
+gconst55=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst56;
+gconst56=((0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst57;
+gconst57=((0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst58;
+gconst58=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst59;
+gconst59=0.784500000000000;
+IKReal gconst60;
+gconst60=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst61;
+gconst61=((0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst62;
+gconst62=((0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst63;
+gconst63=1.63450000000000;
+IKReal gconst64;
+gconst64=0.0655000000000000;
+IKReal gconst65;
+gconst65=-1.56900000000000;
+IKReal gconst66;
+gconst66=1.63450000000000;
+IKReal gconst67;
+gconst67=0.0655000000000000;
+IKReal gconst68;
+gconst68=-1.56900000000000;
+IKReal gconst69;
+gconst69=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst70;
+gconst70=-0.784500000000000;
+IKReal gconst71;
+gconst71=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst72;
+gconst72=((-0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst73;
+gconst73=((-0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst74;
+gconst74=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst75;
+gconst75=-0.784500000000000;
+IKReal gconst76;
+gconst76=((((0.0946500000000000)*(cj5)*(r21)))+(((0.0946500000000000)*(r20)*(sj5)))+(pz));
+IKReal gconst77;
+gconst77=((-0.817250000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+IKReal gconst78;
+gconst78=((-0.0327500000000000)+(((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+dummyeval[0]=((((gconst69)*(gconst73)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst69)*(gconst73)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst70)*(gconst73)*(gconst75)*(gconst77)))+(((gconst71)*(gconst72)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst71)*(gconst72)*(gconst76)*(gconst77))));
+if( IKabs(dummyeval[0]) < 0.0000001000000000  )
+{
+continue;
+
+} else
+{
+IKReal op[8+1], zeror[8];
+int numroots;
+op[0]=((((gconst69)*(gconst73)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst69)*(gconst73)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst70)*(gconst73)*(gconst75)*(gconst77)))+(((gconst71)*(gconst72)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst71)*(gconst72)*(gconst76)*(gconst77))));
+op[1]=((((gconst65)*(gconst71)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst66)*(gconst69)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst64)*(gconst72)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst63)*(gconst73)*(gconst74)*(gconst78)))+(((gconst67)*(gconst69)*(gconst73)*(gconst77)))+(((-1.00000000000000)*(gconst67)*(gconst71)*(gconst72)*(gconst77)))+(((gconst64)*(gconst72)*(gconst74)*(gconst78)))+(((gconst66)*(gconst71)*(gconst72)*(gconst78)))+(((gconst63)*(gconst73)*(gconst76)*(gconst77)))+(((gconst68)*(gconst70)*(gconst73)*(gconst74))));
+op[2]=((((gconst58)*(gconst71)*(gconst72)*(gconst78)))+(((-1.00000000000000)*(gconst64)*(gconst67)*(gconst72)*(gconst77)))+(((gconst62)*(gconst71)*(gconst72)*(gconst74)))+(((-1.00000000000000)*(gconst57)*(gconst69)*(gconst74)*(gconst78)))+(((gconst60)*(gconst69)*(gconst73)*(gconst77)))+(((gconst53)*(gconst73)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst70)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst62)*(gconst69)*(gconst73)*(gconst74)))+(((gconst63)*(gconst67)*(gconst73)*(gconst77)))+(((gconst64)*(gconst65)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst60)*(gconst71)*(gconst72)*(gconst77)))+(((gconst64)*(gconst66)*(gconst72)*(gconst78)))+(((gconst66)*(gconst68)*(gconst70)*(gconst73)))+(((-1.00000000000000)*(gconst65)*(gconst68)*(gconst71)*(gconst74)))+(((-1.00000000000000)*(gconst58)*(gconst69)*(gconst73)*(gconst78)))+(((gconst55)*(gconst72)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst53)*(gconst73)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst56)*(gconst71)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst54)*(gconst73)*(gconst75)*(gconst77)))+(((gconst61)*(gconst69)*(gconst73)*(gconst76)))+(((-1.00000000000000)*(gconst61)*(gconst71)*(gconst72)*(gconst76)))+(((-1.00000000000000)*(gconst61)*(gconst70)*(gconst73)*(gconst75)))+(((-1.00000000000000)*(gconst55)*(gconst72)*(gconst76)*(gconst77)))+(((gconst57)*(gconst69)*(gconst76)*(gconst77)))+(((gconst56)*(gconst71)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst63)*(gconst66)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst59)*(gconst70)*(gconst73)*(gconst77))));
+op[3]=((((gconst62)*(gconst64)*(gconst72)*(gconst74)))+(((gconst61)*(gconst67)*(gconst69)*(gconst73)))+(((gconst58)*(gconst64)*(gconst72)*(gconst78)))+(((-1.00000000000000)*(gconst61)*(gconst64)*(gconst72)*(gconst76)))+(((gconst53)*(gconst67)*(gconst73)*(gconst77)))+(((-1.00000000000000)*(gconst62)*(gconst66)*(gconst69)*(gconst73)))+(((gconst57)*(gconst67)*(gconst69)*(gconst77)))+(((gconst55)*(gconst65)*(gconst75)*(gconst77)))+(((gconst57)*(gconst68)*(gconst70)*(gconst74)))+(((-1.00000000000000)*(gconst60)*(gconst64)*(gconst72)*(gconst77)))+(((-1.00000000000000)*(gconst65)*(gconst66)*(gconst68)*(gconst71)))+(((-1.00000000000000)*(gconst57)*(gconst63)*(gconst74)*(gconst78)))+(((gconst56)*(gconst64)*(gconst74)*(gconst78)))+(((gconst60)*(gconst63)*(gconst73)*(gconst77)))+(((gconst55)*(gconst66)*(gconst72)*(gconst78)))+(((-1.00000000000000)*(gconst53)*(gconst66)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst58)*(gconst63)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst56)*(gconst67)*(gconst71)*(gconst77)))+(((-1.00000000000000)*(gconst64)*(gconst65)*(gconst68)*(gconst74)))+(((gconst61)*(gconst63)*(gconst73)*(gconst76)))+(((-1.00000000000000)*(gconst55)*(gconst67)*(gconst72)*(gconst77)))+(((-1.00000000000000)*(gconst62)*(gconst63)*(gconst73)*(gconst74)))+(((gconst62)*(gconst66)*(gconst71)*(gconst72)))+(((gconst54)*(gconst68)*(gconst73)*(gconst74)))+(((-1.00000000000000)*(gconst61)*(gconst67)*(gconst71)*(gconst72)))+(((gconst61)*(gconst65)*(gconst71)*(gconst75)))+(((gconst56)*(gconst66)*(gconst71)*(gconst78)))+(((gconst59)*(gconst65)*(gconst71)*(gconst77)))+(((gconst58)*(gconst68)*(gconst70)*(gconst73)))+(((gconst57)*(gconst63)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst56)*(gconst64)*(gconst76)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst66)*(gconst69)*(gconst78))));
+op[4]=((((-1.00000000000000)*(gconst62)*(gconst63)*(gconst66)*(gconst73)))+(((gconst53)*(gconst57)*(gconst76)*(gconst77)))+(((gconst53)*(gconst60)*(gconst73)*(gconst77)))+(((gconst56)*(gconst58)*(gconst71)*(gconst78)))+(((gconst58)*(gconst62)*(gconst71)*(gconst72)))+(((gconst57)*(gconst61)*(gconst69)*(gconst76)))+(((gconst54)*(gconst66)*(gconst68)*(gconst73)))+(((gconst61)*(gconst64)*(gconst65)*(gconst75)))+(((-1.00000000000000)*(gconst58)*(gconst65)*(gconst68)*(gconst71)))+(((-1.00000000000000)*(gconst54)*(gconst59)*(gconst73)*(gconst77)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst74)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst59)*(gconst70)*(gconst77)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst75)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst63)*(gconst66)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst61)*(gconst70)*(gconst75)))+(((gconst55)*(gconst56)*(gconst74)*(gconst78)))+(((gconst57)*(gconst66)*(gconst68)*(gconst70)))+(((gconst62)*(gconst64)*(gconst66)*(gconst72)))+(((gconst55)*(gconst62)*(gconst72)*(gconst74)))+(((gconst61)*(gconst63)*(gconst67)*(gconst73)))+(((-1.00000000000000)*(gconst58)*(gconst62)*(gconst69)*(gconst73)))+(((-1.00000000000000)*(gconst53)*(gconst58)*(gconst73)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst69)*(gconst78)))+(((gconst59)*(gconst64)*(gconst65)*(gconst77)))+(((-1.00000000000000)*(gconst55)*(gconst61)*(gconst72)*(gconst76)))+(((gconst60)*(gconst61)*(gconst69)*(gconst73)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst71)*(gconst77)))+(((-1.00000000000000)*(gconst56)*(gconst61)*(gconst71)*(gconst76)))+(((-1.00000000000000)*(gconst54)*(gconst61)*(gconst73)*(gconst75)))+(((-1.00000000000000)*(gconst55)*(gconst60)*(gconst72)*(gconst77)))+(((-1.00000000000000)*(gconst61)*(gconst64)*(gconst67)*(gconst72)))+(((-1.00000000000000)*(gconst53)*(gconst62)*(gconst73)*(gconst74)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst69)*(gconst74)))+(((-1.00000000000000)*(gconst56)*(gconst64)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst76)*(gconst77)))+(((gconst56)*(gconst62)*(gconst71)*(gconst74)))+(((-1.00000000000000)*(gconst60)*(gconst61)*(gconst71)*(gconst72)))+(((-1.00000000000000)*(gconst55)*(gconst65)*(gconst68)*(gconst74)))+(((-1.00000000000000)*(gconst64)*(gconst65)*(gconst66)*(gconst68)))+(((gconst57)*(gconst63)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst59)*(gconst61)*(gconst70)*(gconst73)))+(((gconst53)*(gconst61)*(gconst73)*(gconst76)))+(((gconst57)*(gconst60)*(gconst69)*(gconst77)))+(((gconst56)*(gconst64)*(gconst66)*(gconst78)))+(((gconst55)*(gconst58)*(gconst72)*(gconst78))));
+op[5]=((((gconst55)*(gconst62)*(gconst66)*(gconst72)))+(((-1.00000000000000)*(gconst56)*(gconst61)*(gconst64)*(gconst76)))+(((gconst53)*(gconst57)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst66)*(gconst78)))+(((gconst54)*(gconst57)*(gconst68)*(gconst74)))+(((gconst56)*(gconst58)*(gconst64)*(gconst78)))+(((gconst57)*(gconst60)*(gconst63)*(gconst77)))+(((gconst57)*(gconst61)*(gconst67)*(gconst69)))+(((gconst60)*(gconst61)*(gconst63)*(gconst73)))+(((gconst57)*(gconst58)*(gconst68)*(gconst70)))+(((gconst54)*(gconst58)*(gconst68)*(gconst73)))+(((gconst55)*(gconst61)*(gconst65)*(gconst75)))+(((gconst59)*(gconst61)*(gconst65)*(gconst71)))+(((gconst53)*(gconst61)*(gconst67)*(gconst73)))+(((-1.00000000000000)*(gconst53)*(gconst62)*(gconst66)*(gconst73)))+(((-1.00000000000000)*(gconst58)*(gconst64)*(gconst65)*(gconst68)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst64)*(gconst77)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst67)*(gconst77)))+(((-1.00000000000000)*(gconst58)*(gconst62)*(gconst63)*(gconst73)))+(((-1.00000000000000)*(gconst55)*(gconst61)*(gconst67)*(gconst72)))+(((gconst55)*(gconst59)*(gconst65)*(gconst77)))+(((gconst56)*(gconst62)*(gconst64)*(gconst74)))+(((-1.00000000000000)*(gconst55)*(gconst65)*(gconst66)*(gconst68)))+(((gconst56)*(gconst62)*(gconst66)*(gconst71)))+(((-1.00000000000000)*(gconst60)*(gconst61)*(gconst64)*(gconst72)))+(((gconst57)*(gconst61)*(gconst63)*(gconst76)))+(((-1.00000000000000)*(gconst56)*(gconst61)*(gconst67)*(gconst71)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst63)*(gconst78)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst63)*(gconst74)))+(((gconst58)*(gconst62)*(gconst64)*(gconst72)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst66)*(gconst69)))+(((gconst55)*(gconst56)*(gconst66)*(gconst78))));
+op[6]=((((-1.00000000000000)*(gconst56)*(gconst61)*(gconst64)*(gconst67)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst62)*(gconst69)))+(((-1.00000000000000)*(gconst55)*(gconst58)*(gconst65)*(gconst68)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst61)*(gconst75)))+(((gconst54)*(gconst57)*(gconst66)*(gconst68)))+(((gconst56)*(gconst58)*(gconst62)*(gconst71)))+(((gconst59)*(gconst61)*(gconst64)*(gconst65)))+(((gconst55)*(gconst56)*(gconst58)*(gconst78)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst61)*(gconst76)))+(((-1.00000000000000)*(gconst55)*(gconst60)*(gconst61)*(gconst72)))+(((gconst53)*(gconst60)*(gconst61)*(gconst73)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst58)*(gconst78)))+(((gconst57)*(gconst60)*(gconst61)*(gconst69)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst62)*(gconst74)))+(((gconst55)*(gconst56)*(gconst62)*(gconst74)))+(((gconst55)*(gconst58)*(gconst62)*(gconst72)))+(((-1.00000000000000)*(gconst57)*(gconst59)*(gconst61)*(gconst70)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst59)*(gconst77)))+(((gconst56)*(gconst62)*(gconst64)*(gconst66)))+(((gconst53)*(gconst57)*(gconst61)*(gconst76)))+(((-1.00000000000000)*(gconst54)*(gconst59)*(gconst61)*(gconst73)))+(((gconst57)*(gconst61)*(gconst63)*(gconst67)))+(((gconst53)*(gconst57)*(gconst60)*(gconst77)))+(((-1.00000000000000)*(gconst57)*(gconst62)*(gconst63)*(gconst66)))+(((-1.00000000000000)*(gconst53)*(gconst58)*(gconst62)*(gconst73)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst61)*(gconst71)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst60)*(gconst77))));
+op[7]=((((gconst55)*(gconst59)*(gconst61)*(gconst65)))+(((-1.00000000000000)*(gconst57)*(gconst58)*(gconst62)*(gconst63)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst61)*(gconst67)))+(((gconst56)*(gconst58)*(gconst62)*(gconst64)))+(((gconst57)*(gconst60)*(gconst61)*(gconst63)))+(((gconst55)*(gconst56)*(gconst62)*(gconst66)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst62)*(gconst66)))+(((gconst53)*(gconst57)*(gconst61)*(gconst67)))+(((gconst54)*(gconst57)*(gconst58)*(gconst68)))+(((-1.00000000000000)*(gconst56)*(gconst60)*(gconst61)*(gconst64))));
+op[8]=((((gconst55)*(gconst56)*(gconst58)*(gconst62)))+(((-1.00000000000000)*(gconst53)*(gconst57)*(gconst58)*(gconst62)))+(((-1.00000000000000)*(gconst54)*(gconst57)*(gconst59)*(gconst61)))+(((gconst53)*(gconst57)*(gconst60)*(gconst61)))+(((-1.00000000000000)*(gconst55)*(gconst56)*(gconst60)*(gconst61))));
+polyroots8(op,zeror,numroots);
+IKReal j1array[8], cj1array[8], sj1array[8], tempj1array[1];
+int numsolutions = 0;
+for(int ij1 = 0; ij1 < numroots; ++ij1)
+{
+IKReal htj1 = zeror[ij1];
+tempj1array[0]=((2.00000000000000)*(atan(htj1)));
+for(int kj1 = 0; kj1 < 1; ++kj1)
+{
+j1array[numsolutions] = tempj1array[kj1];
+if( j1array[numsolutions] > IKPI )
+{
+    j1array[numsolutions]-=IK2PI;
+}
+else if( j1array[numsolutions] < -IKPI )
+{
+    j1array[numsolutions]+=IK2PI;
+}
+sj1array[numsolutions] = IKsin(j1array[numsolutions]);
+cj1array[numsolutions] = IKcos(j1array[numsolutions]);
+numsolutions++;
+}
+}
+bool j1valid[8]={true,true,true,true,true,true,true,true};
+_nj1 = 8;
+for(int ij1 = 0; ij1 < numsolutions; ++ij1)
+    {
+if( !j1valid[ij1] )
+{
+    continue;
+}
+    j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
+htj1 = IKtan(j1/2);
+
+_ij1[0] = ij1; _ij1[1] = -1;
+for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
+{
+if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
+{
+    j1valid[iij1]=false; _ij1[1] = iij1; break; 
+}
+}
+{
+IKReal dummyeval[1];
+IKReal gconst95;
+gconst95=IKsign(((((-1569.00000000000)*((cj1)*(cj1))))+(((-1569.00000000000)*((sj1)*(sj1))))));
+dummyeval[0]=((((-1.00000000000000)*((sj1)*(sj1))))+(((-1.00000000000000)*((cj1)*(cj1)))));
+if( IKabs(dummyeval[0]) < 0.0000010000000000  )
+{
+continue;
+
+} else
+{
+{
+IKReal j2array[1], cj2array[1], sj2array[1];
+bool j2valid[1]={false};
+_nj2 = 1;
+if( IKabs(((gconst95)*(((((4000.00000000000)*(cj0)*(px)*(sj1)))+(((4000.00000000000)*(py)*(sj0)*(sj1)))+(((378.600000000000)*(r10)*(sj0)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(r00)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(cj5)*(r01)*(sj1)))+(((378.600000000000)*(cj1)*(cj5)*(r21)))+(((378.600000000000)*(cj5)*(r11)*(sj0)*(sj1)))+(((4000.00000000000)*(cj1)*(pz)))+(((378.600000000000)*(cj1)*(r20)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst95)*(((((-378.600000000000)*(cj1)*(r10)*(sj0)*(sj5)))+(((1700.00000000000)*((cj1)*(cj1))))+(((378.600000000000)*(r20)*(sj1)*(sj5)))+(((-4000.00000000000)*(cj0)*(cj1)*(px)))+(((4000.00000000000)*(pz)*(sj1)))+(((-378.600000000000)*(cj0)*(cj1)*(cj5)*(r01)))+(((-378.600000000000)*(cj1)*(cj5)*(r11)*(sj0)))+(((1700.00000000000)*((sj1)*(sj1))))+(((-4000.00000000000)*(cj1)*(py)*(sj0)))+(((-378.600000000000)*(cj0)*(cj1)*(r00)*(sj5)))+(((378.600000000000)*(cj5)*(r21)*(sj1))))))) < IKFAST_ATAN2_MAGTHRESH )
+    continue;
+j2array[0]=IKatan2(((gconst95)*(((((4000.00000000000)*(cj0)*(px)*(sj1)))+(((4000.00000000000)*(py)*(sj0)*(sj1)))+(((378.600000000000)*(r10)*(sj0)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(r00)*(sj1)*(sj5)))+(((378.600000000000)*(cj0)*(cj5)*(r01)*(sj1)))+(((378.600000000000)*(cj1)*(cj5)*(r21)))+(((378.600000000000)*(cj5)*(r11)*(sj0)*(sj1)))+(((4000.00000000000)*(cj1)*(pz)))+(((378.600000000000)*(cj1)*(r20)*(sj5)))))), ((gconst95)*(((((-378.600000000000)*(cj1)*(r10)*(sj0)*(sj5)))+(((1700.00000000000)*((cj1)*(cj1))))+(((378.600000000000)*(r20)*(sj1)*(sj5)))+(((-4000.00000000000)*(cj0)*(cj1)*(px)))+(((4000.00000000000)*(pz)*(sj1)))+(((-378.600000000000)*(cj0)*(cj1)*(cj5)*(r01)))+(((-378.600000000000)*(cj1)*(cj5)*(r11)*(sj0)))+(((1700.00000000000)*((sj1)*(sj1))))+(((-4000.00000000000)*(cj1)*(py)*(sj0)))+(((-378.600000000000)*(cj0)*(cj1)*(r00)*(sj5)))+(((378.600000000000)*(cj5)*(r21)*(sj1)))))));
+sj2array[0]=IKsin(j2array[0]);
+cj2array[0]=IKcos(j2array[0]);
+if( j2array[0] > IKPI )
+{
+    j2array[0]-=IK2PI;
+}
+else if( j2array[0] < -IKPI )
+{    j2array[0]+=IK2PI;
+}
+j2valid[0] = true;
+for(int ij2 = 0; ij2 < 1; ++ij2)
+{
+if( !j2valid[ij2] )
+{
+    continue;
+}
+_ij2[0] = ij2; _ij2[1] = -1;
+for(int iij2 = ij2+1; iij2 < 1; ++iij2)
+{
+if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
+{
+    j2valid[iij2]=false; _ij2[1] = iij2; break; 
+}
+}
+j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
+{
+IKReal evalcond[2];
+IKReal x65=IKcos(j2);
+IKReal x66=IKsin(j2);
+evalcond[0]=((((0.0946500000000000)*(cj5)*(r21)))+(((0.425000000000000)*(sj1)))+(((0.392250000000000)*(cj1)*(x66)))+(((0.0946500000000000)*(r20)*(sj5)))+(((0.392250000000000)*(sj1)*(x65)))+(pz));
+evalcond[1]=((((-0.0946500000000000)*(r10)*(sj0)*(sj5)))+(((-0.392250000000000)*(sj1)*(x66)))+(((0.392250000000000)*(cj1)*(x65)))+(((0.425000000000000)*(cj1)))+(((-1.00000000000000)*(py)*(sj0)))+(((-1.00000000000000)*(cj0)*(px)))+(((-0.0946500000000000)*(cj0)*(cj5)*(r01)))+(((-0.0946500000000000)*(cj5)*(r11)*(sj0)))+(((-0.0946500000000000)*(cj0)*(r00)*(sj5))));
+if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  )
+{
+continue;
+}
+}
+
+{
+IKReal dummyeval[1];
+IKReal gconst96;
+IKReal x67=(sj1)*(sj1);
+IKReal x68=(cj2)*(cj2);
+IKReal x69=(cj1)*(cj1);
+IKReal x70=(sj2)*(sj2);
+gconst96=IKsign(((((x69)*(x70)))+(((x67)*(x70)))+(((x68)*(x69)))+(((x67)*(x68)))));
+IKReal x71=(sj1)*(sj1);
+IKReal x72=(cj2)*(cj2);
+IKReal x73=(cj1)*(cj1);
+IKReal x74=(sj2)*(sj2);
+dummyeval[0]=((((x72)*(x73)))+(((x73)*(x74)))+(((x71)*(x72)))+(((x71)*(x74))));
+if( IKabs(dummyeval[0]) < 0.0000010000000000  )
+{
+continue;
+
+} else
+{
+{
+IKReal j3array[1], cj3array[1], sj3array[1];
+bool j3valid[1]={false};
+_nj3 = 1;
+if( IKabs(((gconst96)*(((((-1.00000000000000)*(cj1)*(cj5)*(r21)*(sj2)))+(((-1.00000000000000)*(cj2)*(r20)*(sj1)*(sj5)))+(((cj4)*(r21)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(cj4)*(cj5)*(r20)))+(((-1.00000000000000)*(cj4)*(cj5)*(r20)*(sj1)*(sj2)))+(((r22)*(sj1)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj1)*(cj2)*(r22)*(sj4)))+(((-1.00000000000000)*(cj2)*(cj5)*(r21)*(sj1)))+(((-1.00000000000000)*(cj1)*(cj2)*(cj4)*(r21)*(sj5)))+(((-1.00000000000000)*(cj1)*(r20)*(sj2)*(sj5))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst96)*(((((-1.00000000000000)*(r20)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(r20)*(sj5)))+(((-1.00000000000000)*(cj1)*(cj4)*(r21)*(sj2)*(sj5)))+(((-1.00000000000000)*(cj1)*(r22)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj2)*(r22)*(sj1)*(sj4)))+(((cj2)*(cj4)*(cj5)*(r20)*(sj1)))+(((cj1)*(cj2)*(cj5)*(r21)))+(((cj1)*(cj4)*(cj5)*(r20)*(sj2)))+(((-1.00000000000000)*(cj2)*(cj4)*(r21)*(sj1)*(sj5)))+(((-1.00000000000000)*(cj5)*(r21)*(sj1)*(sj2))))))) < IKFAST_ATAN2_MAGTHRESH )
+    continue;
+j3array[0]=IKatan2(((gconst96)*(((((-1.00000000000000)*(cj1)*(cj5)*(r21)*(sj2)))+(((-1.00000000000000)*(cj2)*(r20)*(sj1)*(sj5)))+(((cj4)*(r21)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(cj4)*(cj5)*(r20)))+(((-1.00000000000000)*(cj4)*(cj5)*(r20)*(sj1)*(sj2)))+(((r22)*(sj1)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj1)*(cj2)*(r22)*(sj4)))+(((-1.00000000000000)*(cj2)*(cj5)*(r21)*(sj1)))+(((-1.00000000000000)*(cj1)*(cj2)*(cj4)*(r21)*(sj5)))+(((-1.00000000000000)*(cj1)*(r20)*(sj2)*(sj5)))))), ((gconst96)*(((((-1.00000000000000)*(r20)*(sj1)*(sj2)*(sj5)))+(((cj1)*(cj2)*(r20)*(sj5)))+(((-1.00000000000000)*(cj1)*(cj4)*(r21)*(sj2)*(sj5)))+(((-1.00000000000000)*(cj1)*(r22)*(sj2)*(sj4)))+(((-1.00000000000000)*(cj2)*(r22)*(sj1)*(sj4)))+(((cj2)*(cj4)*(cj5)*(r20)*(sj1)))+(((cj1)*(cj2)*(cj5)*(r21)))+(((cj1)*(cj4)*(cj5)*(r20)*(sj2)))+(((-1.00000000000000)*(cj2)*(cj4)*(r21)*(sj1)*(sj5)))+(((-1.00000000000000)*(cj5)*(r21)*(sj1)*(sj2)))))));
+sj3array[0]=IKsin(j3array[0]);
+cj3array[0]=IKcos(j3array[0]);
+if( j3array[0] > IKPI )
+{
+    j3array[0]-=IK2PI;
+}
+else if( j3array[0] < -IKPI )
+{    j3array[0]+=IK2PI;
+}
+j3valid[0] = true;
+for(int ij3 = 0; ij3 < 1; ++ij3)
+{
+if( !j3valid[ij3] )
+{
+    continue;
+}
+_ij3[0] = ij3; _ij3[1] = -1;
+for(int iij3 = ij3+1; iij3 < 1; ++iij3)
+{
+if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
+{
+    j3valid[iij3]=false; _ij3[1] = iij3; break; 
+}
+}
+j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
+{
+IKReal evalcond[4];
+IKReal x75=IKsin(j3);
+IKReal x76=IKcos(j3);
+IKReal x77=((sj1)*(sj2)*(x75));
+IKReal x78=((cj2)*(sj1)*(x76));
+IKReal x79=((cj1)*(sj2)*(x76));
+IKReal x80=((cj1)*(cj2)*(x75));
+IKReal x81=((cj1)*(sj2)*(x75));
+IKReal x82=((cj2)*(sj1)*(x75));
+IKReal x83=((sj1)*(sj2)*(x76));
+IKReal x84=((cj1)*(cj2)*(x76));
+evalcond[0]=((((-1.00000000000000)*(r20)*(sj5)))+(x84)+(((-1.00000000000000)*(cj5)*(r21)))+(((-1.00000000000000)*(x81)))+(((-1.00000000000000)*(x83)))+(((-1.00000000000000)*(x82))));
+evalcond[1]=((((-1.00000000000000)*(x78)))+(((-1.00000000000000)*(x79)))+(((-1.00000000000000)*(r22)*(sj4)))+(((-1.00000000000000)*(cj4)*(r21)*(sj5)))+(x77)+(((cj4)*(cj5)*(r20)))+(((-1.00000000000000)*(x80))));
+evalcond[2]=((((cj0)*(cj5)*(r01)))+(((-1.00000000000000)*(x78)))+(((-1.00000000000000)*(x79)))+(((cj5)*(r11)*(sj0)))+(x77)+(((cj0)*(r00)*(sj5)))+(((r10)*(sj0)*(sj5)))+(((-1.00000000000000)*(x80))));
+evalcond[3]=((((-1.00000000000000)*(cj4)*(cj5)*(r10)*(sj0)))+(((-1.00000000000000)*(cj0)*(cj4)*(cj5)*(r00)))+(((r12)*(sj0)*(sj4)))+(((cj4)*(r11)*(sj0)*(sj5)))+(x82)+(x83)+(x81)+(((-1.00000000000000)*(x84)))+(((cj0)*(cj4)*(r01)*(sj5)))+(((cj0)*(r02)*(sj4))));
+if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
+{
+continue;
+}
+}
+
+{
+vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
+solution.basesol.resize(6);
+solution.basesol[0].foffset = j0;
+solution.basesol[0].indices[0] = _ij0[0];
+solution.basesol[0].indices[1] = _ij0[1];
+solution.basesol[0].maxsolutions = _nj0;
+solution.basesol[1].foffset = j1;
+solution.basesol[1].indices[0] = _ij1[0];
+solution.basesol[1].indices[1] = _ij1[1];
+solution.basesol[1].maxsolutions = _nj1;
+solution.basesol[2].foffset = j2;
+solution.basesol[2].indices[0] = _ij2[0];
+solution.basesol[2].indices[1] = _ij2[1];
+solution.basesol[2].maxsolutions = _nj2;
+solution.basesol[3].foffset = j3;
+solution.basesol[3].indices[0] = _ij3[0];
+solution.basesol[3].indices[1] = _ij3[1];
+solution.basesol[3].maxsolutions = _nj3;
+solution.basesol[4].foffset = j4;
+solution.basesol[4].indices[0] = _ij4[0];
+solution.basesol[4].indices[1] = _ij4[1];
+solution.basesol[4].maxsolutions = _nj4;
+solution.basesol[5].foffset = j5;
+solution.basesol[5].indices[0] = _ij5[0];
+solution.basesol[5].indices[1] = _ij5[1];
+solution.basesol[5].maxsolutions = _nj5;
+solution.vfree.resize(0);
+}
+}
+}
+
+}
+
+}
+}
+}
+
+}
+
+}
+    }
+
+}
+
+}
+}
+}
+
+}
+
+}
+}
+}
+    }
+}
+return vsolutions.size()>0;
+}
+static inline void polyroots8(IKReal rawcoeffs[8+1], IKReal rawroots[8], int& numroots)
+{
+    using std::complex;
+    //IKFAST_ASSERT(rawcoeffs[0] != 0);
+    const IKReal tol = 128.0*std::numeric_limits<IKReal>::epsilon();
+    const IKReal tolsqrt = 8*sqrt(std::numeric_limits<IKReal>::epsilon());
+    complex<IKReal> coeffs[8];
+    const int maxsteps = 110;
+    for(int i = 0; i < 8; ++i) {
+        coeffs[i] = complex<IKReal>(rawcoeffs[i+1]/rawcoeffs[0]);
+    }
+    complex<IKReal> roots[8];
+    IKReal err[8];
+    roots[0] = complex<IKReal>(1,0);
+    roots[1] = complex<IKReal>(0.4,0.9); // any complex number not a root of unity works
+    err[0] = 1.0;
+    err[1] = 1.0;
+    for(int i = 2; i < 8; ++i) {
+        roots[i] = roots[i-1]*roots[1];
+        err[i] = 1.0;
+    }
+    for(int step = 0; step < maxsteps; ++step) {
+        bool changed = false;
+        for(int i = 0; i < 8; ++i) {
+            if ( err[i] >= tol ) {
+                changed = true;
+                // evaluate
+                complex<IKReal> x = roots[i] + coeffs[0];
+                for(int j = 1; j < 8; ++j) {
+                    x = roots[i] * x + coeffs[j];
+                }
+                for(int j = 0; j < 8; ++j) {
+                    if( i != j ) {
+                        if( roots[i] != roots[j] ) {
+                            x /= (roots[i] - roots[j]);
+                        }
+                    }
+                }
+                roots[i] -= x;
+                err[i] = abs(x);
+            }
+        }
+        if( !changed ) {
+            break;
+        }
+    }
+
+    numroots = 0;
+    bool visited[8] = {false};
+    for(int i = 0; i < 8; ++i) {
+        if( !visited[i] ) {
+            // might be a multiple root, in which case it will have more error than the other roots
+            // find any neighboring roots, and take the average
+            complex<IKReal> newroot=roots[i];
+            int n = 1;
+            for(int j = i+1; j < 8; ++j) {
+                if( abs(roots[i]-roots[j]) < tolsqrt ) {
+                    newroot += roots[j];
+                    n += 1;
+                    visited[j] = true;
+                }
+            }
+            if( n > 1 ) {
+                newroot /= n;
+            }
+            // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
+            if( IKabs(imag(newroot)) < sqrt(std::numeric_limits<IKReal>::epsilon()) ) {
+                rawroots[numroots++] = real(newroot);
+            }
+        }
+    }
+}
+};
+
+
+/// solves the inverse kinematics equations.
+/// \param pfree is an array specifying the free joints of the chain.
+IKFAST_API bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
+IKSolver solver;
+return solver.ik(eetrans,eerot,pfree,vsolutions);
+}
+
+IKFAST_API const char* getKinematicsHash() { return "b61b6309e37433286886547930660887"; }
+
+IKFAST_API const char* getIKFastVersion() { return "54"; }
+
+#ifdef IKFAST_NAMESPACE
+} // end namespace
+#endif
+
+#ifndef IKFAST_NO_MAIN
+#include <stdio.h>
+#include <stdlib.h>
+#ifdef IKFAST_NAMESPACE
+using namespace IKFAST_NAMESPACE;
+#endif
+int main(int argc, char** argv)
+{
+    if( argc != 12+getNumFreeParameters()+1 ) {
+        printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
+               "Returns the ik solutions given the transformation of the end effector specified by\n"
+               "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
+               "There are %d free parameters that have to be specified.\n\n",getNumFreeParameters());
+        return 1;
+    }
+
+    std::vector<IKSolution> vsolutions;
+    std::vector<IKReal> vfree(getNumFreeParameters());
+    IKReal eerot[9],eetrans[3];
+    eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
+    eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
+    eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
+    for(std::size_t i = 0; i < vfree.size(); ++i)
+        vfree[i] = atof(argv[13+i]);
+    bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
+
+    if( !bSuccess ) {
+        fprintf(stderr,"Failed to get ik solution\n");
+        return -1;
+    }
+
+    printf("Found %d ik solutions:\n", (int)vsolutions.size());
+    std::vector<IKReal> sol(getNumJoints());
+    for(std::size_t i = 0; i < vsolutions.size(); ++i) {
+        printf("sol%d (free=%d): ", (int)i, (int)vsolutions[i].GetFree().size());
+        std::vector<IKReal> vsolfree(vsolutions[i].GetFree().size());
+        vsolutions[i].GetSolution(&sol[0],vsolfree.size()>0?&vsolfree[0]:NULL);
+        for( std::size_t j = 0; j < sol.size(); ++j)
+            printf("%.15f, ", sol[j]);
+        printf("\n");
+    }
+    return 0;
+}
+
+#endif
diff --git a/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp b/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d18972d245013657c4d8ee3c78860b25a6a96b7f
--- /dev/null
+++ b/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp
@@ -0,0 +1,663 @@
+#include <ros/ros.h>
+#include <kinematics_base/kinematics_base.h>
+#include <urdf/model.h>
+#include <moveit_configuration_tools/ik_fast_solver.h>
+#include <planning_models/transforms.h>
+
+namespace universal_robot_arm_kinematics
+{
+
+//autogenerated file
+#include "universal_robot_arm_ikfast_output.cpp"
+
+class IKFastKinematicsPlugin : public kinematics::KinematicsBase
+{
+  std::vector<std::string> joint_names_;
+  std::vector<double> joint_min_vector_;
+  std::vector<double> joint_max_vector_;
+  std::vector<bool> joint_has_limits_vector_;
+  std::vector<std::string> link_names_;
+  moveit_configuration_tools::ik_solver_base* ik_solver_;
+  size_t num_joints_;
+  std::vector<int> free_params_;
+  void (*fk)(const IKReal* j, IKReal* eetrans, IKReal* eerot);
+  
+public:
+
+  IKFastKinematicsPlugin():ik_solver_(0), num_joints_(0) {}
+  ~IKFastKinematicsPlugin(){ if(ik_solver_) delete ik_solver_;}
+
+  void fillFreeParams(int count, int *array) { free_params_.clear(); for(int i=0; i<count;++i) free_params_.push_back(array[i]); }
+  
+  bool initialize(const std::string& group_name,
+                  const std::string& base_name,
+                  const std::string& tip_name,
+                  double search_discretization) {
+
+    if(num_joints_ != 0) {
+      ROS_WARN_STREAM("Already initialized, not reinitializing");
+      return true;
+    }
+
+    setValues(group_name, base_name, tip_name,search_discretization);
+
+    ros::NodeHandle node_handle("~/"+group_name);
+
+    std::string robot;
+    node_handle.param("robot",robot,std::string());
+      
+    fillFreeParams(getNumFreeParameters(),getFreeParameters());
+    num_joints_ = getNumJoints();
+    ik_solver_ = new moveit_configuration_tools::ikfast_solver<IKSolution>(ik, num_joints_);
+    fk=fk;
+      
+    if(free_params_.size()>1){
+      ROS_FATAL("Only one free joint paramter supported!");
+      return false;
+    }
+      
+    urdf::Model robot_model;
+    std::string xml_string;
+
+    std::string urdf_xml,full_urdf_xml;
+    node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
+    node_handle.searchParam(urdf_xml,full_urdf_xml);
+
+    ROS_DEBUG("Reading xml file from parameter server\n");
+    if (!node_handle.getParam(full_urdf_xml, xml_string))
+    {
+      ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
+      return false;
+    }
+
+    node_handle.param(full_urdf_xml,xml_string,std::string());
+    robot_model.initString(xml_string);
+
+    boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(tip_frame_));
+    while(link->name != base_frame_ && joint_names_.size() <= num_joints_){
+      //	ROS_INFO("link %s",link->name.c_str());
+      link_names_.push_back(link->name);
+      boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
+      if(joint){
+        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
+          ROS_INFO_STREAM("Pushing back " << joint->name);
+	  joint_names_.push_back(joint->name);
+          float lower, upper;
+          int hasLimits;
+          if ( joint->type != urdf::Joint::CONTINUOUS ) {
+            if(joint->safety) {
+              lower = joint->safety->soft_lower_limit; 
+              upper = joint->safety->soft_upper_limit;
+            } else {
+              lower = joint->limits->lower;
+              upper = joint->limits->upper;
+            }
+            hasLimits = 1;
+          } else {
+            lower = -M_PI;
+            upper = M_PI;
+            hasLimits = 0;
+          }
+          if(hasLimits) {
+            joint_has_limits_vector_.push_back(true);
+            joint_min_vector_.push_back(lower);
+            joint_max_vector_.push_back(upper);
+          } else {
+            joint_has_limits_vector_.push_back(false);
+            joint_min_vector_.push_back(-M_PI);
+            joint_max_vector_.push_back(M_PI);
+          }
+        }
+      } else{
+        ROS_WARN("no joint corresponding to %s",link->name.c_str());
+      }
+      link = link->getParent();
+    }
+    
+    if(joint_names_.size() != num_joints_){
+      ROS_FATAL_STREAM("Joints number mismatch. Num joints " << num_joints_ << " joint names size is " << joint_names_.size());
+      return false;
+    }
+      
+    std::reverse(link_names_.begin(),link_names_.end());
+    std::reverse(joint_names_.begin(),joint_names_.end());
+    std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
+    std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
+    std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
+
+    for(size_t i=0; i <num_joints_; ++i)
+      ROS_INFO_STREAM(joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
+
+    return true;
+  }
+
+  bool getCount(int &count, 
+                const int &max_count, 
+                const int &min_count) const
+  {
+    if(count > 0)
+    {
+      if(-count >= min_count)
+      {   
+        count = -count;
+        return true;
+      }
+      else if(count+1 <= max_count)
+      {
+        count = count+1;
+        return true;
+      }
+      else
+      {
+        return false;
+      }
+    }
+    else
+    {
+      if(1-count <= max_count)
+      {
+        count = 1-count;
+        return true;
+      }
+      else if(count-1 >= min_count)
+      {
+        count = count -1;
+        return true;
+      }
+      else
+        return false;
+    }
+  }
+
+  bool getPositionIK(const geometry_msgs::Pose &ik_pose,
+                     const std::vector<double> &ik_seed_state,
+                     std::vector<double> &solution,
+                     moveit_msgs::MoveItErrorCodes &error_code) const
+  {
+    std::vector<double> vfree(free_params_.size());
+    for(std::size_t i = 0; i < free_params_.size(); ++i){
+      int p = free_params_[i];
+      //	    ROS_ERROR("%u is %f",p,ik_seed_state[p]);
+      vfree[i] = ik_seed_state[p];
+    }
+
+    Eigen::Affine3d frame;
+    planning_models::poseFromMsg(ik_pose, frame);
+
+    int numsol = ik_solver_->solve(frame,vfree);
+		
+    if(numsol){
+      for(int s = 0; s < numsol; ++s){
+        std::vector<double> sol;
+        ik_solver_->getSolution(s,sol);
+        bool obeys_limits = true;
+        ROS_INFO_STREAM("Got " << numsol << " solutions");
+        for(unsigned int i = 0; i < sol.size(); i++) {
+          if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
+            obeys_limits = false;
+            break;
+          }
+          //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
+        }
+        if(obeys_limits) {
+          ik_solver_->getSolution(s,solution);
+          error_code.val = error_code.SUCCESS;
+          return true;
+        }
+      }
+    } else {
+      ROS_INFO_STREAM("No ik solution");
+    }
+	
+    error_code.val = error_code.NO_IK_SOLUTION; 
+    return false;
+  }
+  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                        const std::vector<double> &ik_seed_state,
+                        double timeout,
+                        std::vector<double> &solution,
+                        moveit_msgs::MoveItErrorCodes &error_code) const
+  {
+    if(free_params_.size()==0){
+      return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
+    }
+
+    Eigen::Affine3d frame;
+    planning_models::poseFromMsg(ik_pose, frame);
+
+    std::vector<double> vfree(free_params_.size());
+
+    ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
+    int counter = 0;
+
+    double initial_guess = ik_seed_state[free_params_[0]];
+    vfree[0] = initial_guess;
+
+    int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
+    int num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
+
+    ROS_INFO_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
+
+    while(1) {
+      int numsol = ik_solver_->solve(frame,vfree);
+      
+      //ROS_INFO_STREAM("Solutions number is " << numsol);
+
+      //ROS_INFO("%f",vfree[0]);
+	    
+      if(numsol > 0){
+        for(int s = 0; s < numsol; ++s){
+          std::vector<double> sol;
+          ik_solver_->getSolution(s,sol);
+          bool obeys_limits = true;
+          for(unsigned int i = 0; i < sol.size(); i++) {
+            if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
+              obeys_limits = false;
+              break;
+            }
+            //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
+          }
+          if(obeys_limits) {
+            ik_solver_->getSolution(s,solution);
+            error_code.val = error_code.SUCCESS;
+            return true;
+          }
+        }
+      }
+      
+      // if(numsol > 0){
+      //   for(unsigned int i = 0; i < sol.size(); i++) {
+      //     if(i == 0) {
+      //       ik_solver_->getClosestSolution(ik_seed_state,solution);
+      //     } else {
+      //       ik_solver_->getSolution(s,sol);            
+      //     }
+      //   }
+      //   bool obeys_limits = true;
+      //   for(unsigned int i = 0; i < solution.size(); i++) {
+      //     if(joint_has_limits_vector_[i] && (solution[i] < joint_min_vector_[i] || solution[i] > joint_max_vector_[i])) {
+      //       obeys_limits = false;
+      //       break;
+      //     }
+      //     //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
+      //   }
+      //   if(obeys_limits) {
+      //     error_code.val = kinematics::SUCCESS;
+      //     return true;
+      //   }
+      // }
+      if(!getCount(counter, num_positive_increments, num_negative_increments)) {
+        error_code.val = error_code.NO_IK_SOLUTION; 
+        return false;
+      }
+      
+      vfree[0] = initial_guess+search_discretization_*counter;
+      ROS_DEBUG_STREAM(counter << " " << vfree[0]);
+    }
+    //shouldn't ever get here
+    error_code.val = error_code.NO_IK_SOLUTION; 
+    return false;
+  }      
+
+  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                        const std::vector<double> &ik_seed_state,
+                        double timeout,
+                        unsigned int redundancy,
+                        double consistency_limit,
+                        std::vector<double> &solution,
+                        moveit_msgs::MoveItErrorCodes &error_code) const
+  {
+    if(free_params_.size()==0){
+      //TODO - how to check consistency when there are no free params?
+      return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
+      ROS_WARN_STREAM("No free parameters, so can't search");
+    }
+
+    if(redundancy != (unsigned int)free_params_[0]) {
+      ROS_WARN_STREAM("Calling consistency search with wrong free param");
+      return false;
+    }
+
+    Eigen::Affine3d frame;
+    planning_models::poseFromMsg(ik_pose, frame);
+
+    std::vector<double> vfree(free_params_.size());
+
+    ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
+    int counter = 0;
+
+    double initial_guess = ik_seed_state[free_params_[0]];
+    vfree[0] = initial_guess;
+
+    double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
+    double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
+
+    int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
+    int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
+
+    ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
+
+    while(1) {
+      int numsol = ik_solver_->solve(frame,vfree);
+      
+      //ROS_INFO_STREAM("Solutions number is " << numsol);
+
+      //ROS_INFO("%f",vfree[0]);
+	    
+      if(numsol > 0){
+        for(int s = 0; s < numsol; ++s){
+          std::vector<double> sol;
+          ik_solver_->getSolution(s,sol);
+          bool obeys_limits = true;
+          for(unsigned int i = 0; i < sol.size(); i++) {
+            if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
+              obeys_limits = false;
+              break;
+            }
+            //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
+          }
+          if(obeys_limits) {
+            ik_solver_->getSolution(s,solution);
+            error_code.val = error_code.SUCCESS;
+            return true;
+          }
+        }
+      }
+      
+      // if(numsol > 0){
+      //   for(unsigned int i = 0; i < sol.size(); i++) {
+      //     if(i == 0) {
+      //       ik_solver_->getClosestSolution(ik_seed_state,solution);
+      //     } else {
+      //       ik_solver_->getSolution(s,sol);            
+      //     }
+      //   }
+      //   bool obeys_limits = true;
+      //   for(unsigned int i = 0; i < solution.size(); i++) {
+      //     if(joint_has_limits_vector_[i] && (solution[i] < joint_min_vector_[i] || solution[i] > joint_max_vector_[i])) {
+      //       obeys_limits = false;
+      //       break;
+      //     }
+      //     //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
+      //   }
+      //   if(obeys_limits) {
+      //     error_code.val = kinematics::SUCCESS;
+      //     return true;
+      //   }
+      // }
+      if(!getCount(counter, num_positive_increments, num_negative_increments)) {
+        error_code.val = error_code.NO_IK_SOLUTION; 
+        return false;
+      }
+      
+      vfree[0] = initial_guess+search_discretization_*counter;
+      ROS_DEBUG_STREAM(counter << " " << vfree[0]);
+    }
+    //shouldn't ever get here
+    error_code.val = error_code.NO_IK_SOLUTION; 
+    return false;
+  }      
+
+  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                        const std::vector<double> &ik_seed_state,
+                        double timeout,
+                        std::vector<double> &solution,
+                        const kinematics::KinematicsBase::IKCallbackFn &desired_pose_callback,
+                        const kinematics::KinematicsBase::IKCallbackFn &solution_callback,
+                        moveit_msgs::MoveItErrorCodes &error_code) const
+  {
+    if(free_params_.size()==0){
+      if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
+        ROS_DEBUG_STREAM("No solution whatsoever");
+        error_code.val = error_code.NO_IK_SOLUTION; 
+        return false;
+      } 
+      solution_callback(ik_pose,solution,error_code);
+      if(error_code.val == error_code.SUCCESS) {
+        ROS_DEBUG_STREAM("Solution passes");
+        return true;
+      } else {
+        ROS_DEBUG_STREAM("Solution has error code " << error_code);
+        return false;
+      }
+    }
+
+    if(!desired_pose_callback.empty())
+      desired_pose_callback(ik_pose,ik_seed_state,error_code);
+    if(error_code.val != error_code.SUCCESS)
+    {
+      ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
+      return false;
+    }
+
+    Eigen::Affine3d frame;
+    planning_models::poseFromMsg(ik_pose, frame);
+
+    std::vector<double> vfree(free_params_.size());
+
+    ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
+    int counter = 0;
+
+    double initial_guess = ik_seed_state[free_params_[0]];
+    vfree[0] = initial_guess;
+
+    int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
+    int num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])/search_discretization_;
+
+    ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
+
+    unsigned int solvecount = 0;
+    unsigned int countsol = 0;
+
+    ros::WallTime start = ros::WallTime::now();
+
+    std::vector<double> sol;
+    while(1) {
+      int numsol = ik_solver_->solve(frame,vfree);
+      if(solvecount == 0) {
+        if(numsol == 0) {
+          ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
+        } else {
+          ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
+        }
+      }
+      solvecount++;
+      if(numsol > 0){
+        if(solution_callback.empty()){
+          ik_solver_->getClosestSolution(ik_seed_state,solution);
+          error_code.val = error_code.SUCCESS;
+          return true;
+        }
+        
+        for(int s = 0; s < numsol; ++s){
+          ik_solver_->getSolution(s,sol);
+          countsol++;
+          bool obeys_limits = true;
+          for(unsigned int i = 0; i < sol.size(); i++) {
+            if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
+              obeys_limits = false;
+              break;
+            }
+          }
+          if(obeys_limits) {
+            solution_callback(ik_pose,sol,error_code);
+            if(error_code.val == error_code.SUCCESS){
+              solution = sol;
+              ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
+              return true;
+            }
+          }
+        }
+      }
+      if(!getCount(counter, num_positive_increments, num_negative_increments)) {
+        error_code.val = error_code.NO_IK_SOLUTION; 
+        ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
+        return false;
+      }
+      vfree[0] = initial_guess+search_discretization_*counter;
+      ROS_DEBUG_STREAM(counter << " " << vfree[0]);
+    }
+    error_code.val = error_code.NO_IK_SOLUTION; 
+    return false;
+  }      
+
+  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                        const std::vector<double> &ik_seed_state,
+                        double timeout,
+                        unsigned int redundancy,
+                        double consistency_limit,
+                        std::vector<double> &solution,
+                        const kinematics::KinematicsBase::IKCallbackFn &desired_pose_callback,
+                        const kinematics::KinematicsBase::IKCallbackFn &solution_callback,
+                        moveit_msgs::MoveItErrorCodes &error_code) const
+  {
+    if(free_params_.size()==0){
+      if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
+        ROS_DEBUG_STREAM("No solution whatsoever");
+        error_code.val = error_code.NO_IK_SOLUTION; 
+        return false;
+      } 
+      solution_callback(ik_pose,solution,error_code);
+      if(error_code.val == error_code.SUCCESS) {
+        ROS_DEBUG_STREAM("Solution passes");
+        return true;
+      } else {
+        ROS_DEBUG_STREAM("Solution has error code " << error_code);
+        return false;
+      }
+    }
+
+    if(redundancy != (unsigned int) free_params_[0]) {
+      ROS_WARN_STREAM("Calling consistency search with wrong free param");
+      return false;
+    }
+
+    if(!desired_pose_callback.empty())
+      desired_pose_callback(ik_pose,ik_seed_state,error_code);
+    if(error_code.val != error_code.SUCCESS)
+    {
+      ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
+      return false;
+    }
+
+    Eigen::Affine3d frame;
+    planning_models::poseFromMsg(ik_pose, frame);
+
+    std::vector<double> vfree(free_params_.size());
+
+    ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
+    int counter = 0;
+
+    double initial_guess = ik_seed_state[free_params_[0]];
+    vfree[0] = initial_guess;
+
+    double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
+    double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
+
+    int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
+    int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
+
+    ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
+
+    unsigned int solvecount = 0;
+    unsigned int countsol = 0;
+
+    ros::WallTime start = ros::WallTime::now();
+
+    std::vector<double> sol;
+    while(1) {
+      int numsol = ik_solver_->solve(frame,vfree);
+      if(solvecount == 0) {
+        if(numsol == 0) {
+          ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
+        } else {
+          ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
+        }
+      }
+      solvecount++;
+      if(numsol > 0){
+        if(solution_callback.empty()){
+          ik_solver_->getClosestSolution(ik_seed_state,solution);
+          error_code.val = error_code.SUCCESS;
+          return true;
+        }
+        
+        for(int s = 0; s < numsol; ++s){
+          ik_solver_->getSolution(s,sol);
+          countsol++;
+          bool obeys_limits = true;
+          for(unsigned int i = 0; i < sol.size(); i++) {
+            if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
+              obeys_limits = false;
+              break;
+            }
+          }
+          if(obeys_limits) {
+            solution_callback(ik_pose,sol,error_code);
+            if(error_code.val == error_code.SUCCESS){
+              solution = sol;
+              ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
+              return true;
+            }
+          }
+        }
+      }
+      if(!getCount(counter, num_positive_increments, num_negative_increments)) {
+        error_code.val = error_code.NO_IK_SOLUTION; 
+        ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
+        return false;
+      }
+      vfree[0] = initial_guess+search_discretization_*counter;
+      ROS_DEBUG_STREAM(counter << " " << vfree[0]);
+    }
+    error_code.val = error_code.NO_IK_SOLUTION; 
+    return false;
+  }      
+
+  bool getPositionFK(const std::vector<std::string> &link_names,
+                     const std::vector<double> &joint_angles, 
+                     std::vector<geometry_msgs::Pose> &poses) const
+  {
+    if(link_names.size() == 0) {
+      ROS_WARN_STREAM("Link names with nothing");
+      return false;
+    }
+
+    if(link_names.size()!=1 || link_names[0]!=tip_frame_){
+      ROS_ERROR("Can compute FK for %s only",tip_frame_.c_str());
+      return false;
+    }
+	
+    bool valid = true;
+	
+    double eerot[9], eetrans[3];
+    fk(&joint_angles[0],eetrans,eerot);
+    Eigen::Affine3d out_mat;
+    out_mat.translation().x() = eetrans[0];
+    out_mat.translation().y() = eetrans[1];
+    out_mat.translation().z() = eetrans[2];
+    out_mat(0,0) = eerot[0];
+    out_mat(0,1) = eerot[1];
+    out_mat(0,2) = eerot[2];
+
+    out_mat(1,0) = eerot[3];
+    out_mat(1,1) = eerot[4];
+    out_mat(1,2) = eerot[5];
+
+    out_mat(2,0) = eerot[6];
+    out_mat(2,1) = eerot[7];
+    out_mat(2,2) = eerot[8];
+
+    poses.resize(1);
+    planning_models::msgFromPose(out_mat, poses[0]);
+    
+    return valid;
+  }      
+  const std::vector<std::string>& getJointNames() const { return joint_names_; }
+  const std::vector<std::string>& getLinkNames() const { return link_names_; }
+};
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_DECLARE_CLASS(universal_robot_arm_kinematics, IKFastKinematicsPlugin, universal_robot_arm_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);
+