From 15010b3ce0be8fad62b0ec275246881245188589 Mon Sep 17 00:00:00 2001 From: corot Date: Sat, 19 May 2018 00:47:30 +0200 Subject: [PATCH] Issue #12: Regenerate turtlebot_arm_ikfast_plugin --- turtlebot_arm_ikfast_plugin/CMakeLists.txt | 58 +- turtlebot_arm_ikfast_plugin/include/ikfast.h | 384 +- turtlebot_arm_ikfast_plugin/package.xml | 6 +- ...turtlebot_arm_arm_ikfast_moveit_plugin.cpp | 1153 ++-- .../src/turtlebot_arm_arm_ikfast_solver.cpp | 4772 ++++------------- ...t_arm_moveit_ikfast_plugin_description.xml | 4 +- 6 files changed, 2131 insertions(+), 4246 deletions(-) diff --git a/turtlebot_arm_ikfast_plugin/CMakeLists.txt b/turtlebot_arm_ikfast_plugin/CMakeLists.txt index 7eff21a..711c495 100644 --- a/turtlebot_arm_ikfast_plugin/CMakeLists.txt +++ b/turtlebot_arm_ikfast_plugin/CMakeLists.txt @@ -1,56 +1,40 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 2.8.12) project(turtlebot_arm_ikfast_plugin) -add_definitions(-std=c++11) +add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS - cmake_modules moveit_core pluginlib roscpp tf_conversions - ) -find_package(Boost REQUIRED) -find_package(Eigen REQUIRED) +) + +include_directories(${catkin_INCLUDE_DIRS}) catkin_package( LIBRARIES - DEPENDS - moveit_core - pluginlib - roscpp - tf_conversions - ) + CATKIN_DEPENDS + moveit_core + pluginlib + roscpp + tf_conversions +) -include_directories(${catkin_INCLUDE_DIRS}) -include_directories(${Eigen_INCLUDE_DIRS}) include_directories(include) -link_directories(${catkin_LIBRARY_DIRS}) +set(IKFAST_LIBRARY_NAME turtlebot_arm_arm_moveit_ikfast_plugin) -set(MOVEIT_LIB_NAME turtlebot_arm_moveit_ikfast_kinematics_plugin) +find_package(LAPACK REQUIRED) -add_library(${MOVEIT_LIB_NAME} src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp) -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_library(${IKFAST_LIBRARY_NAME} src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp) +target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(DIRECTORY include/ DESTINATION include) +install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(FILES turtlebot_arm_moveit_ikfast_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + FILES + turtlebot_arm_moveit_ikfast_plugin_description.xml + DESTINATION + ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - - - - - - - - - - - - - - - diff --git a/turtlebot_arm_ikfast_plugin/include/ikfast.h b/turtlebot_arm_ikfast_plugin/include/ikfast.h index 9a2a2f1..c948de3 100644 --- a/turtlebot_arm_ikfast_plugin/include/ikfast.h +++ b/turtlebot_arm_ikfast_plugin/include/ikfast.h @@ -17,7 +17,8 @@ The file is divided into two sections: - Common - the abstract classes section that all ikfast share regardless of their settings - - Library Specific - the library-specific definitions, which depends on the precision/settings that the library was compiled with + - Library Specific - the library-specific definitions, which depends on the precision/settings that the + library was compiled with The defines are as follows, they are also used for the ikfast C++ class: @@ -25,7 +26,8 @@ - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility. - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY - - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected. + - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid + conditions are detected. - IKFAST_REAL - Use to force a custom real number type for IkReal. - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded. @@ -40,52 +42,57 @@ /// should be the same as ikfast.__version__ #define IKFAST_VERSION 61 -namespace ikfast { - +namespace ikfast +{ /// \brief holds the solution for a single dof template class IkSingleDOFSolutionBase { public: - IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) { - indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; - } - T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset - signed char freeind; ///< if >= 0, mimics another joint - unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider - unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself - unsigned char indices[5]; ///< 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 + IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) + { + indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; + } + T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset + signed char freeind; ///< if >= 0, mimics another joint + unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider + unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself + unsigned char indices[5]; ///< 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 }; /// \brief The discrete solutions are returned in this structure. /// /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. -/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is: +/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its +/// prototype is: template class IkSolutionBase { public: - virtual ~IkSolutionBase() { - } - /// \brief gets a concrete solution - /// - /// \param[out] solution the result - /// \param[in] freevalues values for the free parameters \se GetFree - virtual void GetSolution(T* solution, const T* freevalues) const = 0; - - /// \brief std::vector version of \ref GetSolution - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } - - /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned - /// - /// \return vector of indices indicating the free parameters - virtual const std::vector& GetFree() const = 0; - - /// \brief the dof of the solution - virtual const int GetDOF() const = 0; + virtual ~IkSolutionBase() + { + } + /// \brief gets a concrete solution + /// + /// \param[out] solution the result + /// \param[in] freevalues values for the free parameters \se GetFree + virtual void GetSolution(T* solution, const T* freevalues) const = 0; + + /// \brief std::vector version of \ref GetSolution + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + } + + /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned + /// + /// \return vector of indices indicating the free parameters + virtual const std::vector& GetFree() const = 0; + + /// \brief the dof of the solution + virtual const int GetDOF() const = 0; }; /// \brief manages all the solutions @@ -93,23 +100,26 @@ template class IkSolutionListBase { public: - virtual ~IkSolutionListBase() { - } - - /// \brief add one solution and return its index for later retrieval - /// - /// \param vinfos Solution data for each degree of freedom of the manipulator - /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; - - /// \brief returns the solution pointer - virtual const IkSolutionBase& GetSolution(size_t index) const = 0; - - /// \brief returns the number of solutions stored - virtual size_t GetNumSolutions() const = 0; - - /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated. - virtual void Clear() = 0; + virtual ~IkSolutionListBase() + { + } + + /// \brief add one solution and return its index for later retrieval + /// + /// \param vinfos Solution data for each degree of freedom of the manipulator + /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can + /// freely set. + virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; + + /// \brief returns the solution pointer + virtual const IkSolutionBase& GetSolution(size_t index) const = 0; + + /// \brief returns the number of solutions stored + virtual size_t GetNumSolutions() const = 0; + + /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be + /// invalidated. + virtual void Clear() = 0; }; /// \brief holds function pointers for all the exported functions of ikfast @@ -117,28 +127,39 @@ template class IkFastFunctions { public: - IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) { - } - virtual ~IkFastFunctions() { - } - typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); - ComputeIkFn _ComputeIk; - typedef void (*ComputeFkFn)(const T*, T*, T*); - ComputeFkFn _ComputeFk; - typedef int (*GetNumFreeParametersFn)(); - GetNumFreeParametersFn _GetNumFreeParameters; - typedef int* (*GetFreeParametersFn)(); - GetFreeParametersFn _GetFreeParameters; - typedef int (*GetNumJointsFn)(); - GetNumJointsFn _GetNumJoints; - typedef int (*GetIkRealSizeFn)(); - GetIkRealSizeFn _GetIkRealSize; - typedef const char* (*GetIkFastVersionFn)(); - GetIkFastVersionFn _GetIkFastVersion; - typedef int (*GetIkTypeFn)(); - GetIkTypeFn _GetIkType; - typedef const char* (*GetKinematicsHashFn)(); - GetKinematicsHashFn _GetKinematicsHash; + IkFastFunctions() + : _ComputeIk(NULL) + , _ComputeFk(NULL) + , _GetNumFreeParameters(NULL) + , _GetFreeParameters(NULL) + , _GetNumJoints(NULL) + , _GetIkRealSize(NULL) + , _GetIkFastVersion(NULL) + , _GetIkType(NULL) + , _GetKinematicsHash(NULL) + { + } + virtual ~IkFastFunctions() + { + } + typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); + ComputeIkFn _ComputeIk; + typedef void (*ComputeFkFn)(const T*, T*, T*); + ComputeFkFn _ComputeFk; + typedef int (*GetNumFreeParametersFn)(); + GetNumFreeParametersFn _GetNumFreeParameters; + typedef int* (*GetFreeParametersFn)(); + GetFreeParametersFn _GetFreeParameters; + typedef int (*GetNumJointsFn)(); + GetNumJointsFn _GetNumJoints; + typedef int (*GetIkRealSizeFn)(); + GetIkRealSizeFn _GetIkRealSize; + typedef const char* (*GetIkFastVersionFn)(); + GetIkFastVersionFn _GetIkFastVersion; + typedef int (*GetIkTypeFn)(); + GetIkTypeFn _GetIkType; + typedef const char* (*GetKinematicsHashFn)(); + GetKinematicsHashFn _GetKinematicsHash; }; // Implementations of the abstract classes, user doesn't need to use them @@ -148,80 +169,103 @@ template class IkSolution : public IkSolutionBase { public: - IkSolution(const std::vector >& vinfos, const std::vector& vfree) { - _vbasesol = vinfos; - _vfree = vfree; - } - - virtual void GetSolution(T* solution, const T* freevalues) const { - for(std::size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].freeind < 0 ) - solution[i] = _vbasesol[i].foffset; - else { - solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset; - if( solution[i] > T(3.14159265358979) ) { - solution[i] -= T(6.28318530717959); - } - else if( solution[i] < T(-3.14159265358979) ) { - solution[i] += T(6.28318530717959); - } - } + IkSolution(const std::vector >& vinfos, const std::vector& vfree) + { + _vbasesol = vinfos; + _vfree = vfree; + } + + virtual void GetSolution(T* solution, const T* freevalues) const + { + for (std::size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].freeind < 0) + solution[i] = _vbasesol[i].foffset; + else + { + solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset; + if (solution[i] > T(3.14159265358979)) + { + solution[i] -= T(6.28318530717959); } + else if (solution[i] < T(-3.14159265358979)) + { + solution[i] += T(6.28318530717959); + } + } } - - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } - - virtual const std::vector& GetFree() const { - return _vfree; - } - virtual const int GetDOF() const { - return static_cast(_vbasesol.size()); - } - - virtual void Validate() const { - for(size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].maxsolutions == (unsigned char)-1) { - throw std::runtime_error("max solutions for joint not initialized"); - } - if( _vbasesol[i].maxsolutions > 0 ) { - if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("index >= max solutions for joint"); - } - if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("2nd index >= max solutions for joint"); - } - } + } + + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + } + + virtual const std::vector& GetFree() const + { + return _vfree; + } + virtual const int GetDOF() const + { + return static_cast(_vbasesol.size()); + } + + virtual void Validate() const + { + for (size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].maxsolutions == (unsigned char)-1) + { + throw std::runtime_error("max solutions for joint not initialized"); + } + if (_vbasesol[i].maxsolutions > 0) + { + if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("index >= max solutions for joint"); + } + if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("2nd index >= max solutions for joint"); } + } } + } - virtual void GetSolutionIndices(std::vector& v) const { - v.resize(0); - v.push_back(0); - for(int i = (int)_vbasesol.size()-1; i >= 0; --i) { - if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) { - for(size_t j = 0; j < v.size(); ++j) { - v[j] *= _vbasesol[i].maxsolutions; - } - size_t orgsize=v.size(); - if( _vbasesol[i].indices[1] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v.push_back(v[j]+_vbasesol[i].indices[1]); - } - } - if( _vbasesol[i].indices[0] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v[j] += _vbasesol[i].indices[0]; - } - } - } + virtual void GetSolutionIndices(std::vector& v) const + { + v.resize(0); + v.push_back(0); + for (int i = (int)_vbasesol.size() - 1; i >= 0; --i) + { + if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1) + { + for (size_t j = 0; j < v.size(); ++j) + { + v[j] *= _vbasesol[i].maxsolutions; + } + size_t orgsize = v.size(); + if (_vbasesol[i].indices[1] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v.push_back(v[j] + _vbasesol[i].indices[1]); + } } + if (_vbasesol[i].indices[0] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v[j] += _vbasesol[i].indices[0]; + } + } + } } + } - std::vector< IkSingleDOFSolutionBase > _vbasesol; ///< solution and their offsets if joints are mimiced - std::vector _vfree; + std::vector > _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector _vfree; }; /// \brief Default implementation of \ref IkSolutionListBase @@ -229,38 +273,40 @@ template class IkSolutionList : public IkSolutionListBase { public: - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) + virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) + { + size_t index = _listsolutions.size(); + _listsolutions.push_back(IkSolution(vinfos, vfree)); + return index; + } + + virtual const IkSolutionBase& GetSolution(size_t index) const + { + if (index >= _listsolutions.size()) { - size_t index = _listsolutions.size(); - _listsolutions.push_back(IkSolution(vinfos,vfree)); - return index; + throw std::runtime_error("GetSolution index is invalid"); } + typename std::list >::const_iterator it = _listsolutions.begin(); + std::advance(it, index); + return *it; + } - virtual const IkSolutionBase& GetSolution(size_t index) const - { - if( index >= _listsolutions.size() ) { - throw std::runtime_error("GetSolution index is invalid"); - } - typename std::list< IkSolution >::const_iterator it = _listsolutions.begin(); - std::advance(it,index); - return *it; - } + virtual size_t GetNumSolutions() const + { + return _listsolutions.size(); + } - virtual size_t GetNumSolutions() const { - return _listsolutions.size(); - } - - virtual void Clear() { - _listsolutions.clear(); - } + virtual void Clear() + { + _listsolutions.clear(); + } protected: - std::list< IkSolution > _listsolutions; + std::list > _listsolutions; }; - } -#endif // OPENRAVE_IKFAST_HEADER +#endif // OPENRAVE_IKFAST_HEADER // The following code is dependent on the C++ library linking with. #ifdef IKFAST_HAS_LIBRARY @@ -277,7 +323,8 @@ class IkSolutionList : public IkSolutionListBase #endif #ifdef IKFAST_NAMESPACE -namespace IKFAST_NAMESPACE { +namespace IKFAST_NAMESPACE +{ #endif #ifdef IKFAST_REAL @@ -292,10 +339,13 @@ typedef double IkReal; - ``eerot`` - For **Transform6D** it is 9 values for the 3x3 rotation matrix. - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. - - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle. - - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. + - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value + represents the angle. + - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end + effector coordinate system. */ -IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase& solutions); +IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, + ikfast::IkSolutionListBase& solutions); /// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); @@ -325,4 +375,4 @@ IKFAST_API const char* GetKinematicsHash(); } #endif -#endif // IKFAST_HAS_LIBRARY +#endif // IKFAST_HAS_LIBRARY diff --git a/turtlebot_arm_ikfast_plugin/package.xml b/turtlebot_arm_ikfast_plugin/package.xml index a90d949..7edaaf3 100644 --- a/turtlebot_arm_ikfast_plugin/package.xml +++ b/turtlebot_arm_ikfast_plugin/package.xml @@ -12,17 +12,19 @@ catkin - cmake_modules moveit_core + liblapack-dev pluginlib roscpp tf_conversions + pluginlib - cmake_modules + liblapack-dev moveit_core pluginlib roscpp tf_conversions + pluginlib diff --git a/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp b/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp index 570e508..95411ba 100644 --- a/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp +++ b/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp @@ -2,7 +2,8 @@ * * Software License Agreement (BSD License) * - * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer IPA + * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer + *IPA * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -52,11 +53,100 @@ // Need a floating point tolerance when checking joint limits, in case the joint starts at limit const double LIMIT_TOLERANCE = .0000001; +/// \brief Search modes for searchPositionIK(), see there +enum SEARCH_MODE +{ + OPTIMIZE_FREE_JOINT = 1, + OPTIMIZE_MAX_JOINT = 2 +}; namespace ikfast_kinematics_plugin { +#define IKFAST_NO_MAIN // Don't include main() from IKFast + +/// \brief The types of inverse kinematics parameterizations supported. +/// +/// The minimum degree of freedoms required is set in the upper 4 bits of each type. +/// The number of values used to represent the parameterization ( >= dof ) is the next 4 bits. +/// The lower bits contain a unique id of the type. +enum IkParameterizationType +{ + IKP_None = 0, + IKP_Transform6D = 0x67000001, ///< end effector reaches desired 6D transformation + IKP_Rotation3D = 0x34000002, ///< end effector reaches desired 3D rotation + IKP_Translation3D = 0x33000003, ///< end effector origin reaches desired 3D translation + IKP_Direction3D = 0x23000004, ///< direction on end effector coordinate system reaches desired direction + IKP_Ray4D = 0x46000005, ///< ray on end effector coordinate system reaches desired global ray + IKP_Lookat3D = 0x23000006, ///< direction on end effector coordinate system points to desired 3D position + IKP_TranslationDirection5D = 0x56000007, ///< end effector origin and direction reaches desired 3D translation and + /// direction. Can be thought of as Ray IK where the origin of the ray must + /// coincide. + IKP_TranslationXY2D = 0x22000008, ///< 2D translation along XY plane + IKP_TranslationXYOrientation3D = 0x33000009, ///< 2D translation along XY plane and 1D rotation around Z axis. The + /// offset of the rotation is measured starting at +X, so at +X is it 0, + /// at +Y it is pi/2. + IKP_TranslationLocalGlobal6D = 0x3600000a, ///< local point on end effector origin reaches desired 3D global point + + IKP_TranslationXAxisAngle4D = 0x4400000b, ///< end effector origin reaches desired 3D translation, manipulator + /// direction makes a specific angle with x-axis like a cone, angle is from + /// 0-pi. Axes defined in the manipulator base link's coordinate system) + IKP_TranslationYAxisAngle4D = 0x4400000c, ///< end effector origin reaches desired 3D translation, manipulator + /// direction makes a specific angle with y-axis like a cone, angle is from + /// 0-pi. Axes defined in the manipulator base link's coordinate system) + IKP_TranslationZAxisAngle4D = 0x4400000d, ///< end effector origin reaches desired 3D translation, manipulator + /// direction makes a specific angle with z-axis like a cone, angle is from + /// 0-pi. Axes are defined in the manipulator base link's coordinate system. + + IKP_TranslationXAxisAngleZNorm4D = 0x4400000e, ///< end effector origin reaches desired 3D translation, manipulator + /// direction needs to be orthogonal to z-axis and be rotated at a + /// certain angle starting from the x-axis (defined in the manipulator + /// base link's coordinate system) + IKP_TranslationYAxisAngleXNorm4D = 0x4400000f, ///< end effector origin reaches desired 3D translation, manipulator + /// direction needs to be orthogonal to x-axis and be rotated at a + /// certain angle starting from the y-axis (defined in the manipulator + /// base link's coordinate system) + IKP_TranslationZAxisAngleYNorm4D = 0x44000010, ///< end effector origin reaches desired 3D translation, manipulator + /// direction needs to be orthogonal to y-axis and be rotated at a + /// certain angle starting from the z-axis (defined in the manipulator + /// base link's coordinate system) + + IKP_NumberOfParameterizations = 16, ///< number of parameterizations (does not count IKP_None) + + IKP_VelocityDataBit = + 0x00008000, ///< bit is set if the data represents the time-derivate velocity of an IkParameterization + IKP_Transform6DVelocity = IKP_Transform6D | IKP_VelocityDataBit, + IKP_Rotation3DVelocity = IKP_Rotation3D | IKP_VelocityDataBit, + IKP_Translation3DVelocity = IKP_Translation3D | IKP_VelocityDataBit, + IKP_Direction3DVelocity = IKP_Direction3D | IKP_VelocityDataBit, + IKP_Ray4DVelocity = IKP_Ray4D | IKP_VelocityDataBit, + IKP_Lookat3DVelocity = IKP_Lookat3D | IKP_VelocityDataBit, + IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D | IKP_VelocityDataBit, + IKP_TranslationXY2DVelocity = IKP_TranslationXY2D | IKP_VelocityDataBit, + IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D | IKP_VelocityDataBit, + IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D | IKP_VelocityDataBit, + IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D | IKP_VelocityDataBit, + IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D | IKP_VelocityDataBit, + IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D | IKP_VelocityDataBit, + IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D | IKP_VelocityDataBit, + IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D | IKP_VelocityDataBit, + IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D | IKP_VelocityDataBit, + + IKP_UniqueIdMask = 0x0000ffff, ///< the mask for the unique ids + IKP_CustomDataBit = 0x00010000, ///< bit is set if the ikparameterization contains custom data, this is only used + /// when serializing the ik parameterizations +}; + +// struct for storing and sorting solutions +struct LimitObeyingSol +{ + std::vector value; + double dist_from_seed; -#define IKFAST_NO_MAIN // Don't include main() from IKFast + bool operator<(const LimitObeyingSol& a) const + { + return dist_from_seed < a.dist_from_seed; + } +}; // Code generated by IKFast56/61 #include "turtlebot_arm_arm_ikfast_solver.cpp" @@ -68,19 +158,31 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase std::vector joint_max_vector_; std::vector joint_has_limits_vector_; std::vector link_names_; - size_t num_joints_; + const size_t num_joints_; std::vector free_params_; - bool active_; // Internal variable that indicates whether solvers are configured and ready + bool active_; // Internal variable that indicates whether solvers are configured and ready + const std::string name_{ "ikfast" }; - const std::vector& getJointNames() const { return joint_names_; } - const std::vector& getLinkNames() const { return link_names_; } + const std::vector& getJointNames() const + { + return joint_names_; + } + const std::vector& getLinkNames() const + { + return link_names_; + } public: - /** @class * @brief Interface for an IKFast kinematics plugin */ - IKFastKinematicsPlugin():active_(false){} + IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), active_(false) + { + srand(time(NULL)); + supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION); + supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED); + supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED); + } /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it @@ -91,12 +193,29 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ - // Returns the first IK solution that is within joint limits, this is called by get_ik() service - bool getPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + // Returns the IK solution that is within joint limits closest to ik_seed_state + bool getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; + + /** + * @brief Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. + * + * This is a default implementation that returns only one solution and so its result is equivalent to calling + * 'getPositionIK(...)' with a zero initialized seed. + * + * @param ik_poses The desired pose of each tip link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solutions A vector of vectors where each entry is a valid joint solution + * @param result A struct that reports the results of the query + * @param options An option struct which contains the type of redundancy discretization used. This default + * implementation only supports the KinmaticSearches::NO_DISCRETIZATION method; requesting any + * other will result in failure. + * @return True if a valid set of solutions was found, false otherwise. + */ + bool getPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + std::vector>& solutions, kinematics::KinematicsResult& result, + const kinematics::KinematicsQueryOptions& options) const; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -106,12 +225,9 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @param ik_seed_state an initial guess solution for the inverse kinematics * @return True if a valid solution was found, false otherwise */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -122,13 +238,10 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @param the distance that the redundancy can be from the current position * @return True if a valid solution was found, false otherwise */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -138,13 +251,10 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @param ik_seed_state an initial guess solution for the inverse kinematics * @return True if a valid solution was found, false otherwise */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -156,122 +266,144 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @param consistency_limit the distance that the redundancy can be from the current position * @return True if a valid solution was found, false otherwise */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; /** * @brief Given a set of joint angles and a set of links, compute their pose * - * This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, - * otherwise ROS TF is used to calculate the forward kinematics - * * @param link_names A set of links for which FK needs to be computed * @param joint_angles The state for which FK is being computed * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) * @return True if a valid solution was found, false otherwise */ - bool getPositionFK(const std::vector &link_names, - const std::vector &joint_angles, - std::vector &poses) const; + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const; -private: + /** + * @brief Sets the discretization value for the redundant joint. + * + * Since this ikfast implementation allows for one redundant joint then only the first entry will be in the + *discretization map will be used. + * Calling this method replaces previous discretization settings. + * + * @param discretization a map of joint indices and discretization value pairs. + */ + void setSearchDiscretization(const std::map& discretization); - bool initialize(const std::string &robot_description, - const std::string& group_name, - const std::string& base_name, - const std::string& tip_name, - double search_discretization); + /** + * @brief Overrides the default method to prevent changing the redundant joints + */ + bool setRedundantJoints(const std::vector& redundant_joint_indices); + +private: + bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_name, + const std::string& tip_name, double search_discretization); /** * @brief Calls the IK solver from IKFast * @return The number of solutions found */ - int solve(KDL::Frame &pose_frame, const std::vector &vfree, IkSolutionList &solutions) const; + int solve(KDL::Frame& pose_frame, const std::vector& vfree, IkSolutionList& solutions) const; /** * @brief Gets a specific solution from the set */ - void getSolution(const IkSolutionList &solutions, int i, std::vector& solution) const; + void getSolution(const IkSolutionList& solutions, int i, std::vector& solution) const; - double harmonize(const std::vector &ik_seed_state, std::vector &solution) const; - //void getOrderedSolutions(const std::vector &ik_seed_state, std::vector >& solslist); - void getClosestSolution(const IkSolutionList &solutions, const std::vector &ik_seed_state, std::vector &solution) const; - void fillFreeParams(int count, int *array); - bool getCount(int &count, const int &max_count, const int &min_count) const; + /** + * @brief Gets a specific solution from the set with joints rotated 360° to be near seed state where possible + */ + void getSolution(const IkSolutionList& solutions, const std::vector& ik_seed_state, int i, + std::vector& solution) const; -}; // end class + double harmonize(const std::vector& ik_seed_state, std::vector& solution) const; + // void getOrderedSolutions(const std::vector &ik_seed_state, std::vector >& solslist); + void getClosestSolution(const IkSolutionList& solutions, const std::vector& ik_seed_state, + std::vector& solution) const; + void fillFreeParams(int count, int* array); + bool getCount(int& count, const int& max_count, const int& min_count) const; + + /** + * @brief samples the designated redundant joint using the chosen discretization method + * @param method An enumeration flag indicating the discretization method to be used + * @param sampled_joint_vals Sampled joint values for the redundant joint + * @return True if sampling succeeded. + */ + bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector& sampled_joint_vals) const; -bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, - const std::string& group_name, - const std::string& base_name, - const std::string& tip_name, +}; // end class + +bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name, + const std::string& base_name, const std::string& tip_name, double search_discretization) { setValues(robot_description, group_name, base_name, tip_name, search_discretization); - base_frame_ = "arm_base_link"; - ros::NodeHandle node_handle("~/"+group_name); + ros::NodeHandle node_handle("~/" + group_name); std::string robot; - node_handle.param("robot",robot,std::string()); + lookupParam("robot", robot, std::string()); // IKFast56/61 - fillFreeParams( GetNumFreeParameters(), GetFreeParameters() ); - num_joints_ = GetNumJoints(); + fillFreeParams(GetNumFreeParameters(), GetFreeParameters()); - if(free_params_.size() > 1) + if (free_params_.size() > 1) { - ROS_FATAL("Only one free joint paramter supported!"); + ROS_FATAL("Only one free joint parameter supported!"); return false; } + else if (free_params_.size() == 1) + { + redundant_joint_indices_.clear(); + redundant_joint_indices_.push_back(free_params_[0]); + KinematicsBase::setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION); + } urdf::Model robot_model; std::string xml_string; - std::string urdf_xml,full_urdf_xml; - node_handle.param("urdf_xml",urdf_xml,robot_description); - node_handle.searchParam(urdf_xml,full_urdf_xml); + std::string urdf_xml, full_urdf_xml; + lookupParam("urdf_xml", urdf_xml, robot_description); + node_handle.searchParam(urdf_xml, full_urdf_xml); - ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server"); + ROS_DEBUG_NAMED(name_, "Reading xml file from parameter server"); if (!node_handle.getParam(full_urdf_xml, xml_string)) { - ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str()); + ROS_FATAL_NAMED(name_, "Could not load the xml from parameter server: %s", urdf_xml.c_str()); return false; } - node_handle.param(full_urdf_xml,xml_string,std::string()); robot_model.initString(xml_string); - ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF"); + ROS_DEBUG_STREAM_NAMED(name_, "Reading joints and links from URDF"); - boost::shared_ptr link = boost::const_pointer_cast(robot_model.getLink(tip_frame_)); - while(link->name != base_frame_ && joint_names_.size() <= num_joints_) + urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame()); + while (link->name != base_frame_ && joint_names_.size() <= num_joints_) { - ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str()); + ROS_DEBUG_NAMED(name_, "Link %s", link->name.c_str()); link_names_.push_back(link->name); - boost::shared_ptr joint = link->parent_joint; - if(joint) + urdf::JointSharedPtr joint = link->parent_joint; + if (joint) { if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { - ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name ); + ROS_DEBUG_STREAM_NAMED(name_, "Adding joint " << joint->name); joint_names_.push_back(joint->name); float lower, upper; int hasLimits; - if ( joint->type != urdf::Joint::CONTINUOUS ) + if (joint->type != urdf::Joint::CONTINUOUS) { - if(joint->safety) + if (joint->safety) { lower = joint->safety->soft_lower_limit; upper = joint->safety->soft_upper_limit; - } else { + } + else + { lower = joint->limits->lower; upper = joint->limits->upper; } @@ -283,7 +415,7 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, upper = M_PI; hasLimits = 0; } - if(hasLimits) + if (hasLimits) { joint_has_limits_vector_.push_back(true); joint_min_vector_.push_back(lower); @@ -296,99 +428,224 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, joint_max_vector_.push_back(M_PI); } } - } else + } + else { - ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str()); + ROS_WARN_NAMED(name_, "no joint corresponding to %s", link->name.c_str()); } link = link->getParent(); } - if(joint_names_.size() != num_joints_) + if (joint_names_.size() != num_joints_) { - ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_); + ROS_FATAL_STREAM_NAMED(name_, "Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " + << num_joints_); 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(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 &vfree, IkSolutionList &solutions) const +void IKFastKinematicsPlugin::setSearchDiscretization(const std::map& discretization) +{ + if (discretization.empty()) + { + ROS_ERROR("The 'discretization' map is empty"); + return; + } + + if (redundant_joint_indices_.empty()) + { + ROS_ERROR_STREAM("This group's solver doesn't support redundant joints"); + return; + } + + if (discretization.begin()->first != redundant_joint_indices_[0]) + { + std::string redundant_joint = joint_names_[free_params_[0]]; + ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint " + << discretization.begin()->first << ", only joint '" << redundant_joint << "' with index " + << redundant_joint_indices_[0] << " is redundant."); + return; + } + + if (discretization.begin()->second <= 0.0) + { + ROS_ERROR_STREAM("Discretization can not takes values that are <= 0"); + return; + } + + redundant_joint_discretization_.clear(); + redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second; +} + +bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector& redundant_joint_indices) +{ + ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver "); + return false; +} + +int IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector& vfree, + IkSolutionList& solutions) const { // IKFast56/61 solutions.Clear(); - //KDL::Rotation rot = KDL::Rotation::RotY(M_PI/2); - KDL::Rotation orig = pose_frame.M; - KDL::Rotation mult = orig;//*rot; - - /*double vals[9]; - vals[0] = mult(0,0); - vals[1] = mult(0,1); - vals[2] = mult(0,2); - vals[3] = mult(1,0); - vals[4] = mult(1,1); - vals[5] = mult(1,2); - vals[6] = mult(2,0); - vals[7] = mult(2,1); - vals[8] = mult(2,2);*/ - KDL::Vector direction = mult * KDL::Vector(0,0,1); - double trans[3]; - trans[0] = pose_frame.p[0];//-.18; + trans[0] = pose_frame.p[0]; //-.18; trans[1] = pose_frame.p[1]; trans[2] = pose_frame.p[2]; - // IKFast56/61 - ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions); - //ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions); - return solutions.GetNumSolutions(); + KDL::Rotation mult; + KDL::Vector direction; + + switch (GetIkType()) + { + case IKP_Transform6D: + case IKP_Translation3D: + // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored. + + mult = pose_frame.M; + + double vals[9]; + vals[0] = mult(0, 0); + vals[1] = mult(0, 1); + vals[2] = mult(0, 2); + vals[3] = mult(1, 0); + vals[4] = mult(1, 1); + vals[5] = mult(1, 2); + vals[6] = mult(2, 0); + vals[7] = mult(2, 1); + vals[8] = mult(2, 2); + + // IKFast56/61 + ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + return solutions.GetNumSolutions(); + + case IKP_Direction3D: + case IKP_Ray4D: + case IKP_TranslationDirection5D: + // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target + // direction. + + direction = pose_frame.M * KDL::Vector(0, 0, 1); + ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationXAxisAngle4D: + case IKP_TranslationYAxisAngle4D: + case IKP_TranslationZAxisAngle4D: + // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value + // represents the angle. + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_TranslationLocalGlobal6D: + // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end + // effector coordinate system. + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_Rotation3D: + case IKP_Lookat3D: + case IKP_TranslationXY2D: + case IKP_TranslationXYOrientation3D: + case IKP_TranslationXAxisAngleZNorm4D: + case IKP_TranslationYAxisAngleXNorm4D: + case IKP_TranslationZAxisAngleYNorm4D: + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + default: + ROS_ERROR_NAMED(name_, "Unknown IkParameterizationType! Was the solver generated with an incompatible version " + "of Openrave?"); + return 0; + } } -void IKFastKinematicsPlugin::getSolution(const IkSolutionList &solutions, int i, std::vector& solution) const +void IKFastKinematicsPlugin::getSolution(const IkSolutionList& solutions, int i, + std::vector& solution) const { solution.clear(); solution.resize(num_joints_); // IKFast56/61 const IkSolutionBase& sol = solutions.GetSolution(i); - std::vector vsolfree( sol.GetFree().size() ); - sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); // std::cout << "solution " << i << ":" ; // for(int j=0;j& solutions, + const std::vector& ik_seed_state, int i, + std::vector& solution) const +{ + solution.clear(); + solution.resize(num_joints_); + + // IKFast56/61 + const IkSolutionBase& sol = solutions.GetSolution(i); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); + + // rotate joints by +/-360° where it is possible and useful + for (std::size_t i = 0; i < num_joints_; ++i) + { + if (joint_has_limits_vector_[i]) + { + double signed_distance = solution[i] - ik_seed_state[i]; + while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE)) + { + signed_distance -= 2 * M_PI; + solution[i] -= 2 * M_PI; + } + while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE)) + { + signed_distance += 2 * M_PI; + solution[i] += 2 * M_PI; + } + } + } } -double IKFastKinematicsPlugin::harmonize(const std::vector &ik_seed_state, std::vector &solution) const +double IKFastKinematicsPlugin::harmonize(const std::vector& ik_seed_state, std::vector& solution) const { double dist_sqr = 0; std::vector ss = ik_seed_state; - for(size_t i=0; i< ik_seed_state.size(); ++i) + for (size_t i = 0; i < ik_seed_state.size(); ++i) { - while(ss[i] > 2*M_PI) { - ss[i] -= 2*M_PI; + while (ss[i] > 2 * M_PI) + { + ss[i] -= 2 * M_PI; } - while(ss[i] < 2*M_PI) { - ss[i] += 2*M_PI; + while (ss[i] < 2 * M_PI) + { + ss[i] += 2 * M_PI; } - while(solution[i] > 2*M_PI) { - solution[i] -= 2*M_PI; + while (solution[i] > 2 * M_PI) + { + solution[i] -= 2 * M_PI; } - while(solution[i] < 2*M_PI) { - solution[i] += 2*M_PI; + while (solution[i] < 2 * M_PI) + { + solution[i] += 2 * M_PI; } dist_sqr += fabs(ik_seed_state[i] - solution[i]); } @@ -418,48 +675,53 @@ double IKFastKinematicsPlugin::harmonize(const std::vector &ik_seed_stat // } // } -void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList &solutions, const std::vector &ik_seed_state, std::vector &solution) const +void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList& solutions, + const std::vector& ik_seed_state, + std::vector& solution) const { double mindist = DBL_MAX; int minindex = -1; std::vector sol; // IKFast56/61 - for(size_t i=0; i < solutions.GetNumSolutions(); ++i) + for (size_t i = 0; i < solutions.GetNumSolutions(); ++i) { - getSolution(solutions, i,sol); + getSolution(solutions, i, sol); double dist = harmonize(ik_seed_state, sol); - ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist); - //std::cout << "dist[" << i << "]= " << dist << std::endl; - if(minindex == -1 || dist &ik_seed_state, - double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const { - const IKCallbackFn solution_callback = 0; + const IKCallbackFn solution_callback = 0; std::vector consistency_limits; - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - consistency_limits, - solution, - solution_callback, - error_code, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } - -bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const + +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const { - const IKCallbackFn solution_callback = 0; - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - consistency_limits, - solution, - solution_callback, - error_code, + const IKCallbackFn solution_callback = 0; + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } -bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - consistency_limits, - solution, - solution_callback, - error_code, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } -bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const { - ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK"); + ROS_DEBUG_STREAM_NAMED(name_, "searchPositionIK"); + + /// search_mode is currently fixed during code generation + SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT; // Check if there are no redundant joints - if(free_params_.size()==0) + if (free_params_.size() == 0) { - ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints"); + ROS_DEBUG_STREAM_NAMED(name_, "No need to search since no free params/redundant joints"); - // Find first IK solution, within joint limits - if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code)) + std::vector ik_poses(1, ik_pose); + std::vector> solutions; + kinematics::KinematicsResult kinematic_result; + // Find all IK solution within joint limits + if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options)) { - ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever"); + ROS_DEBUG_STREAM_NAMED(name_, "No solution whatsoever"); error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } - // check for collisions if a callback is provided - if( !solution_callback.empty() ) + // sort solutions by their distance to the seed + std::vector solutions_obey_limits; + for (std::size_t i = 0; i < solutions.size(); ++i) { - solution_callback(ik_pose, solution, error_code); - if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) + double dist_from_seed = 0.0; + for (std::size_t j = 0; j < ik_seed_state.size(); ++j) { - ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback"); - return true; + dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]); } - else + + solutions_obey_limits.push_back({ solutions[i], dist_from_seed }); + } + std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end()); + + // check for collisions if a callback is provided + if (!solution_callback.empty()) + { + for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i) { - ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code); - return false; + solution_callback(ik_pose, solutions_obey_limits[i].value, error_code); + if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) + { + solution = solutions_obey_limits[i].value; + ROS_DEBUG_STREAM_NAMED(name_, "Solution passes callback"); + return true; + } } + + ROS_DEBUG_STREAM_NAMED(name_, "Solution has error code " << error_code); + return false; } else { - return true; // no collision check callback provided + solution = solutions_obey_limits[0].value; + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; // no collision check callback provided } } // ------------------------------------------------------------------------------------------------- // Error Checking - if(!active_) + if (!active_) { - ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active"); + ROS_ERROR_STREAM_NAMED(name_, "Kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } - if(ik_seed_state.size() != num_joints_) + if (ik_seed_state.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED(name_, "Seed state must have size " << num_joints_ << " instead of size " + << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } - if(!consistency_limits.empty() && consistency_limits.size() != num_joints_) + if (!consistency_limits.empty() && consistency_limits.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size()); + ROS_ERROR_STREAM_NAMED(name_, "Consistency limits be empty or must have size " << num_joints_ << " instead of size " + << consistency_limits.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } - // ------------------------------------------------------------------------------------------------- // Initialize KDL::Frame frame; - tf::poseMsgToKDL(ik_pose,frame); + tf::poseMsgToKDL(ik_pose, frame); std::vector vfree(free_params_.size()); @@ -667,59 +947,69 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose int num_positive_increments; int num_negative_increments; - if(!consistency_limits.empty()) + if (!consistency_limits.empty()) { // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector) // Assume [0]th free_params element for now. Probably wrong. - double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]); - double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]); + double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]); + double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]); - num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_); - num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_); + num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_); + num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_); } - else // no consitency limits provided + else // no consitency limits provided { - num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_; - num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_; + num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_; + num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_; } // ------------------------------------------------------------------------------------------------- // Begin searching - ROS_DEBUG_STREAM_NAMED("ikfast","Free param is " << free_params_[0] << " initial guess is " << initial_guess << ", # positive increments: " << num_positive_increments << ", # negative increments: " << num_negative_increments); + ROS_DEBUG_STREAM_NAMED(name_, "Free param is " << free_params_[0] << " initial guess is " << initial_guess + << ", # positive increments: " << num_positive_increments + << ", # negative increments: " << num_negative_increments); + if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000) + ROS_WARN_STREAM_ONCE_NAMED(name_, "Large search space, consider increasing the search discretization"); + + double best_costs = -1.0; + std::vector best_solution; + int nattempts = 0, nvalid = 0; - while(true) + while (true) { IkSolutionList solutions; - int numsol = solve(frame,vfree, solutions); + int numsol = solve(frame, vfree, solutions); - ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast"); - //ROS_INFO("%f",vfree[0]); + // ROS_INFO("%f",vfree[0]); - if( numsol > 0 ) + if (numsol > 0) { - for(int s = 0; s < numsol; ++s) + for (int s = 0; s < numsol; ++s) { + nattempts++; std::vector sol; - getSolution(solutions,s,sol); + getSolution(solutions, ik_seed_state, s, sol); bool obeys_limits = true; - for(unsigned int i = 0; i < sol.size(); i++) + 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])) + 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_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); + // ROS_INFO_STREAM_NAMED(name_,"Num " << i << " value " << sol[i] << " has limits " << + // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); } - if(obeys_limits) + if (obeys_limits) { - getSolution(solutions,s,solution); + getSolution(solutions, ik_seed_state, s, solution); // This solution is within joint limits, now check if in collision (if callback provided) - if(!solution_callback.empty()) + if (!solution_callback.empty()) { solution_callback(ik_pose, solution, error_code); } @@ -728,103 +1018,360 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose error_code.val = error_code.SUCCESS; } - if(error_code.val == error_code.SUCCESS) + if (error_code.val == error_code.SUCCESS) { - return true; + nvalid++; + if (search_mode & OPTIMIZE_MAX_JOINT) + { + // Costs for solution: Largest joint motion + double costs = 0.0; + for (unsigned int i = 0; i < solution.size(); i++) + { + double d = fabs(ik_seed_state[i] - solution[i]); + if (d > costs) + costs = d; + } + if (costs < best_costs || best_costs == -1.0) + { + best_costs = costs; + best_solution = solution; + } + } + else + // Return first feasible solution + return true; } } } } - if(!getCount(counter, num_positive_increments, num_negative_increments)) + if (!getCount(counter, num_positive_increments, -num_negative_increments)) { + // Everything searched error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; - return false; + break; } - vfree[0] = initial_guess+search_discretization_*counter; - ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]); + vfree[0] = initial_guess + search_discretization_ * counter; + // ROS_DEBUG_STREAM_NAMED(name_,"Attempt " << counter << " with 0th free joint having value " << vfree[0]); + } + + ROS_DEBUG_STREAM_NAMED(name_, "Valid solutions: " << nvalid << "/" << nattempts); + + if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0) + { + solution = best_solution; + error_code.val = error_code.SUCCESS; + return true; } - // not really needed b/c shouldn't ever get here + // No solution found error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } // Used when there are no redundant joints - aka no free params -bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const +bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const { - ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK"); + ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK"); - if(!active_) + if (!active_) { - ROS_ERROR("kinematics not active"); + ROS_ERROR("kinematics not active"); return false; } + if (ik_seed_state.size() < num_joints_) + { + ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires " + << num_joints_); + return false; + } + + // Check if seed is in bound + for (std::size_t i = 0; i < ik_seed_state.size(); i++) + { + // Add tolerance to limit check + if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || + (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) + { + ROS_DEBUG_STREAM_NAMED("ikseed", "Not in limits! " << (int)i << " value " << ik_seed_state[i] + << " has limit: " << joint_has_limits_vector_[i] << " being " + << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + return false; + } + } + std::vector vfree(free_params_.size()); - for(std::size_t i = 0; i < free_params_.size(); ++i) + 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]); // DTC + ROS_ERROR("%u is %f", p, ik_seed_state[p]); // DTC vfree[i] = ik_seed_state[p]; } KDL::Frame frame; - tf::poseMsgToKDL(ik_pose,frame); + tf::poseMsgToKDL(ik_pose, frame); IkSolutionList solutions; - int numsol = solve(frame,vfree,solutions); + int numsol = solve(frame, vfree, solutions); + ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast"); - ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); + std::vector solutions_obey_limits; - if(numsol) + if (numsol) { - for(int s = 0; s < numsol; ++s) + std::vector solution_obey_limits; + for (std::size_t s = 0; s < numsol; ++s) { std::vector sol; - getSolution(solutions,s,sol); - ROS_DEBUG_NAMED("ikfast","Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]); + getSolution(solutions, ik_seed_state, s, sol); + ROS_DEBUG_NAMED(name_, "Sol %d: %e %e %e %e %e %e", (int)s, sol[0], sol[1], sol[2], sol[3], sol[4], + sol[5]); bool obeys_limits = true; - for(unsigned int i = 0; i < sol.size(); i++) + for (std::size_t i = 0; i < sol.size(); i++) { // Add tolerance to limit check - if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) || - (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) ) + if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || + (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) { // One element of solution is not within limits obeys_limits = false; - ROS_DEBUG_STREAM_NAMED("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << (int)i << " value " << sol[i] << " has limit: " + << joint_has_limits_vector_[i] << " being " + << joint_min_vector_[i] << " to " << joint_max_vector_[i]); break; } } - if(obeys_limits) + if (obeys_limits) { - // All elements of solution obey limits - getSolution(solutions,s,solution); - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - return true; + // All elements of this solution obey limits + getSolution(solutions, ik_seed_state, s, solution_obey_limits); + double dist_from_seed = 0.0; + for (std::size_t i = 0; i < ik_seed_state.size(); ++i) + { + dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]); + } + + solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed }); } } } else { - ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution"); + ROS_DEBUG_STREAM_NAMED(name_, "No IK solution"); + } + + // Sort the solutions under limits and find the one that is closest to ik_seed_state + if (!solutions_obey_limits.empty()) + { + std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end()); + solution = solutions_obey_limits[0].value; + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; } error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } +bool IKFastKinematicsPlugin::getPositionIK(const std::vector& ik_poses, + const std::vector& ik_seed_state, + std::vector>& solutions, + kinematics::KinematicsResult& result, + const kinematics::KinematicsQueryOptions& options) const +{ + ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK with multiple solutions"); + + if (!active_) + { + ROS_ERROR("kinematics not active"); + result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE; + return false; + } + + if (ik_poses.empty()) + { + ROS_ERROR("ik_poses is empty"); + result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES; + return false; + } + + if (ik_poses.size() > 1) + { + ROS_ERROR("ik_poses contains multiple entries, only one is allowed"); + result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED; + return false; + } + + if (ik_seed_state.size() < num_joints_) + { + ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires " + << num_joints_); + return false; + } + + KDL::Frame frame; + tf::poseMsgToKDL(ik_poses[0], frame); + + // solving ik + std::vector> solution_set; + IkSolutionList ik_solutions; + std::vector vfree; + int numsol = 0; + std::vector sampled_joint_vals; + if (!redundant_joint_indices_.empty()) + { + // initializing from seed + sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]); + + // checking joint limits when using no discretization + if (options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION && + joint_has_limits_vector_[redundant_joint_indices_.front()]) + { + double joint_min = joint_min_vector_[redundant_joint_indices_.front()]; + double joint_max = joint_max_vector_[redundant_joint_indices_.front()]; + + double jv = sampled_joint_vals[0]; + if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE)))) + { + result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS; + ROS_ERROR_STREAM("ik seed is out of bounds"); + return false; + } + } + + // computing all solutions sets for each sampled value of the redundant joint + if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals)) + { + result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED; + return false; + } + + for (unsigned int i = 0; i < sampled_joint_vals.size(); i++) + { + vfree.clear(); + vfree.push_back(sampled_joint_vals[i]); + numsol += solve(frame, vfree, ik_solutions); + solution_set.push_back(ik_solutions); + } + } + else + { + // computing for single solution set + numsol = solve(frame, vfree, ik_solutions); + solution_set.push_back(ik_solutions); + } + + ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast"); + bool solutions_found = false; + if (numsol > 0) + { + /* + Iterating through all solution sets and storing those that do not exceed joint limits. + */ + for (unsigned int r = 0; r < solution_set.size(); r++) + { + ik_solutions = solution_set[r]; + numsol = ik_solutions.GetNumSolutions(); + for (int s = 0; s < numsol; ++s) + { + std::vector sol; + getSolution(ik_solutions, ik_seed_state, s, sol); + + bool obeys_limits = true; + for (unsigned int i = 0; i < sol.size(); i++) + { + // Add tolerance to limit check + if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || + (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) + { + // One element of solution is not within limits + obeys_limits = false; + ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << i << " value " << sol[i] << " has limit: " + << joint_has_limits_vector_[i] << " being " + << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + break; + } + } + if (obeys_limits) + { + // All elements of solution obey limits + solutions_found = true; + solutions.push_back(sol); + } + } + } + + if (solutions_found) + { + result.kinematic_error = kinematics::KinematicErrors::OK; + return true; + } + } + else + { + ROS_DEBUG_STREAM_NAMED(name_, "No IK solution"); + } + + result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION; + return false; +} + +bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method, + std::vector& sampled_joint_vals) const +{ + double joint_min = -M_PI; + double joint_max = M_PI; + int index = redundant_joint_indices_.front(); + double joint_dscrt = redundant_joint_discretization_.at(index); + + if (joint_has_limits_vector_[redundant_joint_indices_.front()]) + { + joint_min = joint_min_vector_[index]; + joint_max = joint_max_vector_[index]; + } + switch (method) + { + case kinematics::DiscretizationMethods::ALL_DISCRETIZED: + { + int steps = std::ceil((joint_max - joint_min) / joint_dscrt); + for (unsigned int i = 0; i < steps; i++) + { + sampled_joint_vals.push_back(joint_min + joint_dscrt * i); + } + sampled_joint_vals.push_back(joint_max); + } + break; + case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED: + { + int steps = std::ceil((joint_max - joint_min) / joint_dscrt); + steps = steps > 0 ? steps : 1; + double diff = joint_max - joint_min; + for (int i = 0; i < steps; i++) + { + sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast(RAND_MAX))) + joint_min); + } + } + + break; + case kinematics::DiscretizationMethods::NO_DISCRETIZATION: + + break; + default: + ROS_ERROR_STREAM("Discretization method " << method << " is not supported"); + return false; + } + + return true; +} -} // end namespace +} // end namespace -//register IKFastKinematicsPlugin as a KinematicsBase implementation +// register IKFastKinematicsPlugin as a KinematicsBase implementation #include PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase); diff --git a/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_solver.cpp b/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_solver.cpp index fedab15..b50018e 100644 --- a/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_solver.cpp +++ b/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_solver.cpp @@ -12,7 +12,8 @@ /// See the License for the specific language governing permissions and /// limitations under the License. /// -/// ikfast version 61 generated on 2013-05-31 01:16:42.037862 +/// ikfast version 0x1000004a generated on 2018-05-19 00:23:16.992561 +/// Generated using solver translationdirection5d /// To compile with gcc: /// gcc -lstdc++ ik.cpp /// To compile without any main function as a shared object (might need -llapack): @@ -23,7 +24,7 @@ using namespace ikfast; // check if the included ikfast version matches what this file was compiled with #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] -IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); +IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a); #include #include @@ -31,9 +32,6 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #include #include -#define IKFAST_STRINGIZE2(s) #s -#define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s) - #ifndef IKFAST_ASSERT #include #include @@ -67,6 +65,12 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #ifndef isnan #define isnan _isnan #endif +#ifndef isinf +#define isinf _isinf +#endif +//#ifndef isfinite +//#define isfinite _isfinite +//#endif #endif // _MSC_VER // lapack routines @@ -94,14 +98,14 @@ 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 +// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation #ifndef IKFAST_SINCOS_THRESH -#define IKFAST_SINCOS_THRESH ((IkReal)0.000001) +#define IKFAST_SINCOS_THRESH ((IkReal)1e-7) #endif -// used to check input to atan2 for degenerate cases +// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation #ifndef IKFAST_ATAN2_MAGTHRESH -#define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6) +#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7) #endif // minimum distance of separate solutions @@ -109,6 +113,12 @@ inline double IKlog(double f) { return log(f); } #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) #endif +// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate +#ifndef IKFAST_EVALCOND_THRESH +#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001) +#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 @@ -164,27 +174,55 @@ 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 IKatan2Simple(float fy, float fx) { + return atan2f(fy,fx); +} inline float IKatan2(float fy, float fx) { - if( std::isnan(fy) ) { - IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned + if( isnan(fy) ) { + IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned return float(IKPI_2); } - else if( std::isnan(fx) ) { + else if( isnan(fx) ) { return 0; } return atan2f(fy,fx); } +inline double IKatan2Simple(double fy, double fx) { + return atan2(fy,fx); +} inline double IKatan2(double fy, double fx) { - if( std::isnan(fy) ) { - IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned + if( isnan(fy) ) { + IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned return IKPI_2; } - else if( std::isnan(fx) ) { + else if( isnan(fx) ) { return 0; } return atan2(fy,fx); } +template +struct CheckValue +{ + T value; + bool valid; +}; + +template +inline CheckValue IKatan2WithCheck(T fy, T fx, T epsilon) +{ + CheckValue ret; + ret.valid = false; + ret.value = 0; + if( !isnan(fy) && !isnan(fx) ) { + if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) { + ret.value = IKatan2Simple(fy,fx); + ret.valid = true; + } + } + return ret; +} + inline float IKsign(float f) { if( f > 0 ) { return float(1); @@ -205,58 +243,96 @@ inline double IKsign(double f) { return 0; } +template +inline CheckValue IKPowWithIntegerCheck(T f, int n) +{ + CheckValue ret; + ret.valid = true; + if( n == 0 ) { + ret.value = 1.0; + return ret; + } + else if( n == 1 ) + { + ret.value = f; + return ret; + } + else if( n < 0 ) + { + if( f == 0 ) + { + ret.valid = false; + ret.value = (T)1.0e30; + return ret; + } + if( n == -1 ) { + ret.value = T(1.0)/f; + return ret; + } + } + + int num = n > 0 ? n : -n; + if( num == 2 ) { + ret.value = f*f; + } + else if( num == 3 ) { + ret.value = f*f*f; + } + else { + ret.value = 1.0; + while(num>0) { + if( num & 1 ) { + ret.value *= f; + } + num >>= 1; + f *= f; + } + } + + if( n < 0 ) { + ret.value = T(1.0)/ret.value; + } + return ret; +} + /// solves the forward kinematics equations. /// \param pfree is an array specifying the free joints of the chain. IKFAST_API void ComputeFk(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; -x0=IKcos(j[3]); -x1=IKcos(j[2]); -x2=IKsin(j[1]); -x3=IKcos(j[1]); -x4=IKsin(j[2]); -x5=IKsin(j[3]); -x6=IKsin(j[4]); -x7=IKcos(j[0]); -x8=IKsin(j[0]); -x9=IKcos(j[4]); -x10=((IkReal(0.999999985550000))*(x8)); -x11=((IkReal(0.000169999997543500))*(x7)); -x12=((IkReal(1.00000000000000))*(x4)); -x13=((x1)*(x3)); -x14=((x2)*(x4)); -x15=((x1)*(x2)); -x16=((x3)*(x4)); -x17=((((IkReal(-1.00000000000000))*(x10)))+(x11)); -x18=((((IkReal(-1.00000000000000))*(x11)))+(x10)); -x19=((((IkReal(0.000169999997543500))*(x8)))+(((IkReal(0.999999985550000))*(x7)))); -x20=((IkReal(-1.00000000000000))*(x19)); -x21=((x17)*(x6)); -x22=((x17)*(x2)); -x23=((x17)*(x4)); -x24=((((IkReal(-1.00000000000000))*(x13)))+(x14)); -x25=((x19)*(x2)); -x26=((x20)*(x6)); -x27=((x14)*(x17)); -x28=((((IkReal(1.00000000000000))*(x15)))+(((x12)*(x3)))); -x29=((IkReal(-1.00000000000000))*(x28)); -x30=((x24)*(x5)); -x31=((x0)*(x24)); -x32=((x0)*(x29)); -x33=((x28)*(x5)); -x34=((((x16)*(x17)))+(((x15)*(x17)))); -x35=((((x16)*(x19)))+(((x15)*(x19)))); -IkReal x40=((x13)*(x18)); -x36=((((x5)*(((((IkReal(0.000799999744000123))*(x40)))+(((IkReal(-0.999999680000154))*(x34)))+(((IkReal(0.000799999744000123))*(x27)))))))+(((x0)*(((((IkReal(-0.999999680000154))*(x40)))+(((IkReal(-0.000799999744000123))*(x34)))+(((IkReal(-0.999999680000154))*(x27)))))))); -x37=((x36)*(x9)); -IkReal x41=((x13)*(x20)); -x38=((((x0)*(((((IkReal(-0.999999680000154))*(x41)))+(((IkReal(-0.999999680000154))*(x14)*(x19)))+(((IkReal(-0.000799999744000123))*(x35)))))))+(((x5)*(((((IkReal(0.000799999744000123))*(x41)))+(((IkReal(-0.999999680000154))*(x35)))+(((IkReal(0.000799999744000123))*(x12)*(x25)))))))); -x39=((x38)*(x9)); -eetrans[0]=((IkReal(-6.12051650867111e-6))+(((((((IkReal(-0.999999990999352))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-0.999999990999352))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(-0.999999990999352))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-0.999999990999352))*(((((IkReal(1.21855920251392e-11))*(IKsin(j[0]))))+(((IkReal(7.16799530890540e-8))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((IkReal(1.27211445824509e-5))*(IKsin(j[1]))*(IKsin(j[2]))))+(((((((IkReal(-8.95999418495792e-5))*(((((IkReal(-0.000169999997543500))*(IKsin(j[0]))))+(((IkReal(-0.999999985550000))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-8.95999418495792e-5))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((IkReal(-1.27211445824509e-5))*(IKcos(j[1]))))+(((((((IkReal(1.34412007677683e-5))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(1.34412007677683e-5))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-5.99891970842010e-5))*(((((IkReal(-8.95999413613176e-5))*(IKsin(j[0]))))+(((IkReal(1.52319900314240e-8))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-5.99891970842010e-5))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-0.999999990999352))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-0.999999990999352))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-0.999999990999352))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-0.999999990999352))*(((((IkReal(8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(6.71878577340761e-6))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(6.71878577340761e-6))*(((((IkReal(0.999999985550000))*(IKsin(j[0]))))+(((IkReal(-0.000169999997543500))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(1.80199995774193e-5))*(IKsin(j[0]))))+(((IkReal(0.105999997514231))*(IKcos(j[0]))))))*(IKsin(j[1]))))+(((((((IkReal(-5.99891970842010e-5))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-5.99891970842010e-5))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(0.111999927311974))*(((((IkReal(-0.000169999997543500))*(IKsin(j[0]))))+(((IkReal(-0.999999985550000))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(0.111999927311974))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-5.99891970842010e-5))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-5.99891970842010e-5))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(0.111999927311974))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(0.111999927311974))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(6.71878577340761e-6))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(6.71878577340761e-6))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-8.95999418495792e-5))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-8.95999418495792e-5))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((IkReal(-1.27211445824509e-5))*(IKcos(j[1]))*(IKcos(j[2]))))+(((((((IkReal(-8.60236849137173e-12))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(8.60236849137173e-12))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-5.99891970842010e-5))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-5.99891970842010e-5))*(((((IkReal(1.21855920251392e-11))*(IKcos(j[0]))))+(((IkReal(-7.16799530890540e-8))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(1.80199995774193e-5))*(IKsin(j[0]))))+(((IkReal(0.105999997514231))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((((((IkReal(-6.35885479903985e-6))*(IKsin(j[0]))))+(((IkReal(1.08100531583677e-9))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((((((IkReal(-5.37502861872609e-9))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-5.37502861872609e-9))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(8.60236849137173e-12))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(8.60236849137173e-12))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-1.07529606142147e-8))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-1.07529606142147e-8))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(1.07529606142147e-8))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-1.07529606142147e-8))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-6.35885479903985e-6))*(IKsin(j[0]))))+(((IkReal(1.08100531583677e-9))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((((((IkReal(1.07529606142147e-8))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(1.07529606142147e-8))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(-6.35885479903985e-6))*(IKsin(j[0]))))+(((IkReal(1.08100531583677e-9))*(IKcos(j[0]))))))*(IKsin(j[1]))))+(((((((IkReal(-5.37502861872609e-9))*(((((IkReal(0.999999985550000))*(IKsin(j[0]))))+(((IkReal(-0.000169999997543500))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-5.37502861872609e-9))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(1.34412007677683e-5))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-1.34412007677683e-5))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(1.80199995774193e-5))*(IKsin(j[0]))))+(((IkReal(0.105999997514231))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((((((IkReal(1.07529606142147e-8))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-1.07529606142147e-8))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))); -eetrans[1]=((IkReal(-2.27985057461787e-6))+(((((((IkReal(3.22579151213407e-13))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-3.22579151213407e-13))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-5.99891979482657e-5))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-5.99891979482657e-5))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-4.03223939016759e-10))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(4.03223939016759e-10))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-0.111999928118519))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-0.111999928118519))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-1.80199997071867e-5))*(IKcos(j[0]))))+(((IkReal(0.105999998277569))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((((((IkReal(1.08100533140722e-9))*(IKsin(j[0]))))+(((IkReal(6.35885489063071e-6))*(IKcos(j[0]))))))*(IKsin(j[1]))))+(((((((IkReal(-5.99891979482657e-5))*(((((IkReal(8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-5.99891979482657e-5))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-5.99891979482657e-5))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-5.99891979482657e-5))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(6.71878587018280e-6))*(((((IkReal(-0.000169999997543500))*(IKsin(j[0]))))+(((IkReal(-0.999999985550000))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(6.71878587018280e-6))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(3.22579151213407e-13))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(3.22579151213407e-13))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(-3.22579151213407e-13))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(3.22579151213407e-13))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(0.999999998200648))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(0.999999998200648))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(0.999999998200648))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(0.999999998200648))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-3.22579151213407e-13))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-3.22579151213407e-13))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(8.95999424948149e-5))*(((((IkReal(0.999999985550000))*(IKsin(j[0]))))+(((IkReal(-0.000169999997543500))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(8.95999424948149e-5))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(1.08100533140722e-9))*(IKsin(j[0]))))+(((IkReal(6.35885489063071e-6))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((((((IkReal(-4.03223939016759e-10))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-4.03223939016759e-10))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(6.71878587018280e-6))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(6.71878587018280e-6))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(-5.99891979482657e-5))*(((((IkReal(1.21855920251392e-11))*(IKsin(j[0]))))+(((IkReal(7.16799530890540e-8))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-5.99891979482657e-5))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((IkReal(3.81622900807933e-10))*(IKcos(j[1]))))+(((((((IkReal(8.95999424948149e-5))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(8.95999424948149e-5))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(0.999999998200648))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(0.999999998200648))*(((((IkReal(-8.95999413613176e-5))*(IKsin(j[0]))))+(((IkReal(1.52319900314240e-8))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((IkReal(-3.81622900807933e-10))*(IKsin(j[1]))*(IKsin(j[2]))))+(((((((IkReal(-5.37502869614624e-9))*(((((IkReal(-0.000169999997543500))*(IKsin(j[0]))))+(((IkReal(-0.999999985550000))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-5.37502869614624e-9))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-5.37502869614624e-9))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-5.37502869614624e-9))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(-2.58063320970726e-16))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-2.58063320970726e-16))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(0.999999998200648))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(0.999999998200648))*(((((IkReal(1.21855920251392e-11))*(IKcos(j[0]))))+(((IkReal(-7.16799530890540e-8))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-1.80199997071867e-5))*(IKcos(j[0]))))+(((IkReal(0.105999998277569))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((((((IkReal(-2.58063320970726e-16))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(2.58063320970726e-16))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-1.80199997071867e-5))*(IKcos(j[0]))))+(((IkReal(0.105999998277569))*(IKsin(j[0]))))))*(IKsin(j[1]))))+(((((((IkReal(-0.111999928118519))*(((((IkReal(0.999999985550000))*(IKsin(j[0]))))+(((IkReal(-0.000169999997543500))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-0.111999928118519))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((IkReal(3.81622900807933e-10))*(IKcos(j[1]))*(IKcos(j[2]))))+(((((((IkReal(1.08100533140722e-9))*(IKsin(j[0]))))+(((IkReal(6.35885489063071e-6))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))); -eetrans[2]=((IkReal(0.0604999995301421))+(((((((IkReal(8.95999420108010e-5))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(8.95999420108010e-5))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(-1.14475414131654e-9))*(IKsin(j[0]))))+(((IkReal(1.94608204023812e-13))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(0.105999999236663))*(IKcos(j[1]))*(IKcos(j[2]))))+(((((((IkReal(-0.111999927513501))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(0.111999927513501))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-1.14475414131654e-9))*(IKsin(j[0]))))+(((IkReal(1.94608204023812e-13))*(IKcos(j[0]))))))*(IKsin(j[1]))))+(((((((IkReal(2.16259453998404e-9))*(IKsin(j[0]))))+(((IkReal(1.27211443528473e-5))*(IKcos(j[0]))))))*(IKsin(j[1]))))+(((((((IkReal(-8.95999420108010e-5))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(8.95999420108010e-5))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-7.16799536086408e-8))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(7.16799536086408e-8))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-0.000120010797515734))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-0.000120010797515734))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(2.16259453998404e-9))*(IKsin(j[0]))))+(((IkReal(1.27211443528473e-5))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((((((IkReal(-0.000120010797515734))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-0.000120010797515734))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(1.34412007193938e-5))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(1.34412007193938e-5))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-1.07995675269645e-8))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-1.07995675269645e-8))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(1.34412007193938e-5))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(1.34412007193938e-5))*(((((IkReal(-0.000169999997543500))*(IKsin(j[0]))))+(((IkReal(-0.999999985550000))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-0.111999927513501))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-0.111999927513501))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-1.07529605755150e-8))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-1.07529605755150e-8))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(-9.67640631126014e-13))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-9.67640631126014e-13))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-7.16799536086408e-8))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-7.16799536086408e-8))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKsin(j[3]))))+(((((((IkReal(-1.07529605755150e-8))*(((((IkReal(-0.000169999997543500))*(IKsin(j[0]))))+(((IkReal(-0.999999985550000))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-1.07529605755150e-8))*(((((IkReal(0.999999985550000))*(IKcos(j[0]))))+(((IkReal(0.000169999997543500))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-0.000120010797515734))*(((((IkReal(-7.16799530890540e-8))*(IKcos(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(-0.000120010797515734))*(((((IkReal(1.21855920251392e-11))*(IKsin(j[0]))))+(((IkReal(7.16799530890540e-8))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-1.07995675269645e-8))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-1.07995675269645e-8))*(((((IkReal(-8.95999413613176e-5))*(IKsin(j[0]))))+(((IkReal(1.52319900314240e-8))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-9.67640631126014e-13))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-9.67640631126014e-13))*(((((IkReal(0.999999985550000))*(IKsin(j[0]))))+(((IkReal(-0.000169999997543500))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(-1.14475414131654e-9))*(IKsin(j[0]))))+(((IkReal(1.94608204023812e-13))*(IKcos(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((((((IkReal(2.16259453998404e-9))*(IKsin(j[0]))))+(((IkReal(1.27211443528473e-5))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((((((IkReal(-1.07995675269645e-8))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))+(((IkReal(-1.07995675269645e-8))*(((((IkReal(-1.52319900314240e-8))*(IKcos(j[0]))))+(((IkReal(8.95999413613176e-5))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(1.20955078890752e-9))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))+(((IkReal(1.20955078890752e-9))*(((((IkReal(0.999999985550000))*(IKsin(j[0]))))+(((IkReal(-0.000169999997543500))*(IKcos(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(1.20955078890752e-9))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(1.20955078890752e-9))*(((((IkReal(0.000169999997543500))*(IKcos(j[0]))))+(((IkReal(-0.999999985550000))*(IKsin(j[0]))))))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((IkReal(0.105999999236663))*(IKcos(j[1]))))+(((IkReal(-0.105999999236663))*(IKsin(j[1]))*(IKsin(j[2]))))+(((((((IkReal(-1.07995675269645e-8))*(((((IkReal(1.21855920251392e-11))*(IKcos(j[0]))))+(((IkReal(-7.16799530890540e-8))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-1.07995675269645e-8))*(((((IkReal(7.16799530890540e-8))*(IKsin(j[0]))))+(((IkReal(-1.21855920251392e-11))*(IKcos(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))+(((((((IkReal(-8.95999420108010e-5))*(IKcos(j[1]))*(IKsin(j[2]))))+(((IkReal(-8.95999420108010e-5))*(IKcos(j[2]))*(IKsin(j[1]))))))*(IKcos(j[3]))))+(((((((IkReal(-0.000120010797515734))*(((((IkReal(-8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(-1.52319900314240e-8))*(IKsin(j[0]))))))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-0.000120010797515734))*(((((IkReal(8.95999413613176e-5))*(IKcos(j[0]))))+(((IkReal(1.52319900314240e-8))*(IKsin(j[0]))))))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKcos(j[3]))))+(((((((IkReal(8.95999420108010e-5))*(IKsin(j[1]))*(IKsin(j[2]))))+(((IkReal(-8.95999420108010e-5))*(IKcos(j[1]))*(IKcos(j[2]))))))*(IKsin(j[3]))))); -eerot[0]=((((IkReal(-5.99891970842010e-5))*(x26)))+(((IkReal(-0.999999990999352))*(x39)))+(((x9)*(((((IkReal(0.000120010759544213))*(x32)))+(((IkReal(0.000120010759544213))*(x30)))+(((IkReal(9.60086076353706e-8))*(x33)))+(((IkReal(9.60086076353706e-8))*(x31)))))))+(((IkReal(-5.99891970842010e-5))*(x37)))+(((IkReal(-0.999999990999352))*(x21)))); -eerot[1]=((((IkReal(-5.99891979482657e-5))*(x21)))+(((IkReal(-5.99891979482657e-5))*(x39)))+(((IkReal(0.999999998200648))*(x26)))+(((IkReal(0.999999998200648))*(x37)))+(((x9)*(((((IkReal(-2.88017191463142e-12))*(x33)))+(((IkReal(-2.88017191463142e-12))*(x31)))+(((IkReal(-3.60021489328928e-9))*(x32)))+(((IkReal(-3.60021489328928e-9))*(x30)))))))); -eerot[2]=((((IkReal(-0.000120010797515734))*(x39)))+(((x9)*(((((IkReal(-0.999999672798860))*(x30)))+(((IkReal(-0.999999672798860))*(x32)))+(((IkReal(-0.000799999738239088))*(x31)))+(((IkReal(-0.000799999738239088))*(x33)))))))+(((IkReal(-1.07995675269645e-8))*(x37)))+(((IkReal(-0.000120010797515734))*(x21)))+(((IkReal(-1.07995675269645e-8))*(x26)))); +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; +x0=IKcos(j[0]); +x1=IKsin(j[2]); +x2=IKcos(j[1]); +x3=IKsin(j[1]); +x4=IKcos(j[2]); +x5=IKsin(j[0]); +x6=IKcos(j[3]); +x7=IKsin(j[3]); +x8=IKcos(j[4]); +x9=IKsin(j[4]); +x10=((0.112)*x1); +x11=((0.112)*x4); +x12=((0.106)*x4); +x13=((1.0)*x6); +x14=((1.0)*x4); +x15=((1.0)*x7); +x16=((1.1e-7)*x4); +x17=((1.1e-7)*x1); +x18=(x0*x3); +x19=(x3*x5); +x20=(x1*x2); +x21=(x0*x2); +x22=(x2*x5); +x23=(x1*x3); +x24=((1.1e-7)*x21); +eetrans[0]=((-3.0e-8)+((x7*((((x11*x21))+(((-1.0)*x10*x18))))))+(((0.106)*x18))+(((-1.0)*x24))+(((-1.0)*x16*x21))+((x17*x18))+((x12*x18))+(((0.106)*x0*x20))+((x6*((((x11*x18))+((x10*x21))))))); +eetrans[1]=(((x6*((((x11*x19))+((x10*x22))))))+(((0.106)*x20*x5))+(((0.106)*x19))+(((-1.0)*x16*x22))+(((-1.1e-7)*x22))+((x17*x19))+((x12*x19))+((x7*((((x11*x22))+(((-1.0)*x10*x19))))))); +IkReal x25=((1.0)*x3); +eetrans[2]=((0.0605)+(((0.106)*x2))+(((1.1e-7)*x3))+(((-0.106)*x23))+((x17*x2))+((x16*x3))+((x7*(((((-1.0)*x10*x2))+(((-1.0)*x11*x25))))))+((x6*(((((-1.0)*x10*x25))+((x11*x2))))))+((x12*x2))); +eerot[0]=(((x5*x9))+((x8*((((x7*((((x18*x4))+((x0*x20))))))+((x6*((((x1*x18))+(((-1.0)*x14*x21))))))))))); +IkReal x26=((1.0)*x19); +eerot[1]=((((-1.0)*x8*((((x13*(((((-1.0)*x1*x26))+((x22*x4))))))+((x15*(((((-1.0)*x14*x26))+(((-1.0)*x20*x5))))))))))+(((-1.0)*x0*x9))); +IkReal x27=((1.0)*x14); +eerot[2]=((-1.0)*x8*((((x13*(((((-1.0)*x20))+(((-1.0)*x27*x3))))))+((x15*((x23+(((-1.0)*x2*x27))))))))); } IKFAST_API int GetNumFreeParameters() { return 0; } @@ -269,9 +345,11 @@ IKFAST_API int GetIkType() { return 0x56000007; } class IKSolver { public: -IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j5,cj5,sj5,htj5,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp; +IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_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,_ij5[2], _nj5; +IkReal j100, cj100, sj100; +unsigned char _ij100[2], _nj100; bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions) { j0=numeric_limits::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j5=numeric_limits::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; for(int dummyiter = 0; dummyiter < 1; ++dummyiter) { @@ -282,26 +360,37 @@ r00 = eerot[0]; r01 = eerot[1]; r02 = eerot[2]; px = eetrans[0]; py = eetrans[1]; pz = eetrans[2]; -new_r00=((((IkReal(-1.07995675269559e-8))*(r02)))+(((IkReal(0.999999998200648))*(r01)))+(((IkReal(-5.99891970842010e-5))*(r00)))); -new_px=((IkReal(2.28013677947483e-6))+(((IkReal(-5.99891970842010e-5))*(px)))+(((IkReal(-1.07995675269559e-8))*(pz)))+(((IkReal(0.999999998200648))*(py)))); -new_r01=((((IkReal(-5.99891979482657e-5))*(r01)))+(((IkReal(-0.000120010797515734))*(r02)))+(((IkReal(-0.999999990999352))*(r00)))); -new_py=((IkReal(1.13999997332400e-6))+(((IkReal(-0.000120010797515734))*(pz)))+(((IkReal(-5.99891979482657e-5))*(py)))+(((IkReal(-0.999999990999352))*(px)))); -new_r02=((((IkReal(3.60021604536983e-9))*(r01)))+(((IkReal(0.999999992798704))*(r02)))+(((IkReal(-0.000120010797947650))*(r00)))); -new_pz=((IkReal(-0.0604999998289836))+(((IkReal(3.60021604536983e-9))*(py)))+(((IkReal(-0.000120010797947650))*(px)))+(((IkReal(0.999999992798704))*(pz)))); +new_r00=r01; +new_px=py; +new_r01=((-1.0)*r00); +new_py=((-3.0e-8)+(((-1.0)*px))); +new_r02=r02; +new_pz=((-0.0605)+pz); r00 = new_r00; r01 = new_r01; r02 = new_r02; px = new_px; py = new_py; pz = new_pz; -pp=(((px)*(px))+((py)*(py))+((pz)*(pz))); +pp=((px*px)+(py*py)+(pz*pz)); +{ +IkReal j0eval[1]; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +continue; // no branches [j0] + +} else +{ { IkReal j0array[2], cj0array[2], sj0array[2]; bool j0valid[2]={false}; _nj0 = 2; -if( IKabs(((((IkReal(-0.000169999997543500))*(py)))+(((IkReal(0.999999985550000))*(px))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(0.000169999997543500))*(px)))+(((IkReal(0.999999985550000))*(py))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x42=IKatan2(((((IkReal(-0.000169999997543500))*(py)))+(((IkReal(0.999999985550000))*(px)))), ((((IkReal(0.000169999997543500))*(px)))+(((IkReal(0.999999985550000))*(py))))); -j0array[0]=((IkReal(-1.00000000000000))*(x42)); +CheckValue x29 = IKatan2WithCheck(IkReal(((-1.0)*px)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH); +if(!x29.valid){ +continue; +} +IkReal x28=x29.value; +j0array[0]=((-1.0)*x28); sj0array[0]=IKsin(j0array[0]); cj0array[0]=IKcos(j0array[0]); -j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x42)))); +j0array[1]=((3.14159265358979)+(((-1.0)*x28))); sj0array[1]=IKsin(j0array[1]); cj0array[1]=IKcos(j0array[1]); if( j0array[0] > IKPI ) @@ -338,18 +427,18 @@ j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; IkReal op[4+1], zeror[4]; int numroots; -op[0]=((((IkReal(-0.000812850431382459))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(0.00531864888470676))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-0.000679999980348001))*(pz)*(r00)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(1.17456890729270e-11))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.848000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.000679999980348001))*(pz)*(r01)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(px)*((py)*(py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(5.73838480187078e-10))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.00203999994104400))*(py)*(pz)*(r00)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(-9.77980785828634e-12))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(-8.02815474597405e-5))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(-8.31283667954339e-16))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(7.20799979168881e-5))*(r01)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.0397120066969557))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.000339999990174000))*(cj0)*(sj0)*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-1.29363115338308e-8))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-1.99999994220000))*((py)*(py))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-1.38184577328552e-7))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.000144159995833776))*(cj0)*(pz)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.0449439712358584))*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.423999987746400))*(r01)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-9.77980785828634e-12))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(1.22535996458710e-8))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.53708957210214e-10))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.00203999994104400))*(px)*(pz)*(r01)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-5.73838480187078e-10))*((cj0)*(cj0))*((py)*(py))*((r00)*(r00))))+(((IkReal(5.73838480187078e-10))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.000144159995833776))*(cj0)*(sj0)*((pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(0.423999987746400))*(pz)*((cj0)*(cj0))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-7.20799979168881e-5))*(r01)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(8.02815474597405e-5))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(-8.02815474597405e-5))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.000144159995833776))*(cj0)*(pz)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.00135999996069600))*(cj0)*(pz)*(r01)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-0.999999971100001))*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(3.99999976880001))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.000216239993750664))*(px)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(-0.0106372977694135))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(1.99999982660000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-5.73838480187078e-10))*((px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000144159995833776))*(px)*(py)*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.000679999980348001))*(px)*((cj0)*(cj0))*((py)*(py)*(py))*((r00)*(r00))))+(((IkReal(0.0198560027746394))*((cj0)*(cj0))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.848000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.848000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.0198560027746394))*((px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-3.99999988440000))*(pz)*(r00)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-0.000432479987501328))*(cj0)*(px)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(1.44667348522452e-9))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(8.50984403073250e-6))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(11.9999993064000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(-4.18559726073583e-6))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(2.89334697044905e-9))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.424000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(0.00531864903841572))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-2.88999991647900e-8))*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00135999996069600))*(px)*(py)*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.423999987746400))*(pz)*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.0250879966515221))*(pp)*((r02)*(r02))))+(((IkReal(1.53347389433800e-10))*(cj0)*(r00)*(r02)))+(((IkReal(1.99999994220000))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.29363115338308e-8))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-1.15599996659160e-7))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.423999975492801))*(cj0)*(r01)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(6.75104094337739e-6))*(px)*(py)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r00)*(r00))))+(((IkReal(2.89334697044905e-9))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.0397120619298817))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0397120044016018))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.000144159995833776))*(px)*(py)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(3.67607989376129e-8))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-1.99999988440000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px)*(px)*(px))))+(((IkReal(4.00000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-0.0198560027746394))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(cj0)*(px)*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-7.99999965320001))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(-0.999999971100001))*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(6.75104094337739e-6))*(cj0)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(1.53347389433800e-10))*(r01)*(r02)*(sj0)))+(((IkReal(1.14767862294149e-9))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-8.50984403073250e-6))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-2.87641407596657e-8))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.00531864919212468))*(px)*(r00)*(r02)))+(((IkReal(-6.75104094337739e-6))*(cj0)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.00531864903841572))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(1.69599990197120))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.000339999990174000))*(cj0)*(sj0)*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(-6.75104094337739e-6))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(6.75104094337739e-6))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-8.02815474597405e-5))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(-2.88999991647900e-8))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-6.75105072318525e-6))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(4.18559726073583e-6))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(-2.88999991647900e-8))*((px)*(px)*(px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000288319991667552))*(px)*(py)*(pz)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.38184577328552e-7))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-8.31283667954339e-16))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(0.0397120630775603))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.00407999988208800))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.140063935774761))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-0.0397120044016018))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.423999987746400))*((pz)*(pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(6.75105072318525e-6))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-1.69600000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(0.000144159995833776))*(cj0)*(pz)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(1.99999988440000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz)*(pz)*(pz))))+(((IkReal(-6.75104094337739e-6))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-7.11551534325092e-10))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.000432479987501328))*(cj0)*(py)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.00203999994104400))*(cj0)*(sj0)*((px)*(px))*((py)*(py))*((r00)*(r00))))+(((IkReal(-0.847999950985601))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-1.36478630681559e-8))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(0.424000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(6.75105072318525e-6))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.000679999980348001))*(py)*((cj0)*(cj0))*((px)*(px)*(px))*((r01)*(r01))))+(((IkReal(9.02043467257645e-7))*(cj0)*(r01)*(r02)))+(((IkReal(8.44671447204764e-5))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(7.20799979168881e-5))*(r00)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-0.0198560315387801))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(3.99999988440000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-1.15599996659160e-7))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(px)*((cj0)*(cj0))*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.00203999994104400))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))*((py)*(py))))+(((IkReal(0.000679999980348001))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-0.000144159995833776))*(cj0)*(r01)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-5.73839311470746e-10))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.000339999990174000))*(r00)*(r01)*((cj0)*(cj0))*((py)*(py)*(py)*(py))))+(((IkReal(1.35021014463705e-5))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.000432479987501328))*(cj0)*(py)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-2.87641407596657e-8))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(7.60959501990047e-5))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.000144159995833776))*(cj0)*(r00)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(-3.99999982660001))*((px)*(px))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(8.50984403073250e-6))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(1.80834067306134e-6))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.00135999996069600))*(cj0)*(py)*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(1.99999994220000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.000679999980348001))*(py)*((px)*(px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.27199992647840))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(1.15599996659160e-7))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(9.04170336530672e-7))*(py)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.847999950985601))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-1.36478630681559e-8))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-1.15599996659160e-7))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-7.20799979168881e-5))*(r00)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(1.44667348522452e-9))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.00203999994104400))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-1.99999988440000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py)*(py)*(py))))+(((IkReal(9.04170336530672e-7))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.000406425227436919))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-2.88999991647900e-8))*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.0951199645389027))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.27199996323920))*(py)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(0.00203999994104400))*(cj0)*(sj0)*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0397120630775603))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.999999971100001))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r01)*(r01))))+(((IkReal(1.44667348522452e-9))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-0.999999971100001))*((py)*(py)*(py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.0397120619298817))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.000679999980348001))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-3.99999976880001))*(px)*(r00)*(r01)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-0.000679999980348001))*(py)*((px)*(px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000144159995833776))*(r00)*(r01)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-7.99999965320001))*(py)*(pz)*(r01)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(-1.80834067306134e-6))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(6.75105072318525e-6))*(px)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(0.000216239993750664))*(px)*(r01)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.000216239993750664))*(px)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.423999987746400))*(r00)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(1.53708957210214e-10))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.000339999990174000))*(cj0)*(sj0)*((py)*(py)*(py)*(py))*((r01)*(r01))))+(((IkReal(1.36478630681559e-8))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(0.000339999990174000))*(cj0)*(sj0)*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.000679999980348001))*(px)*((py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.000339999990174000))*(r00)*(r01)*((px)*(px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-5.77999983295801e-8))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.36478630681559e-8))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.000288319991667552))*(px)*(py)*(pz)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(-0.847999963239201))*(py)*(r01)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(-1.35021014463705e-5))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(1.44667348522452e-9))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-8.44671447204764e-5))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.848000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.000339999990174000))*(cj0)*(sj0)*((px)*(px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-1.69600000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-3.99999976880001))*(cj0)*(pz)*(r00)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-1.80834067306134e-6))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(1.27199996323920))*(px)*(r00)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.000144159995833776))*(px)*(py)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000339999990174000))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px)*(px))))+(((IkReal(-0.00531864919212468))*(pz)*((r02)*(r02))))+(((IkReal(11.9999993064000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-9.04170336530672e-7))*(px)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(3.99999965320001))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-1.69600000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-1.22535996458710e-8))*(pz)*((cj0)*(cj0))*((py)*(py))*((r00)*(r00))))+(((IkReal(-0.848000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(1.14767862294149e-9))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.77999983295801e-8))*((py)*(py))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.847999963239201))*(px)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(6.75104094337739e-6))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.000406425227436919))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.847999950985601))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-3.99999982660001))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r00)*(r00))))+(((IkReal(0.00135999996069600))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(7.99999953760001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.00271999992139200))*(cj0)*(px)*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(-3.99999976880001))*(py)*(r00)*(r01)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-0.00531864903841572))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.00531864888470676))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(0.00135999996069600))*(cj0)*(pz)*(r00)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(-1.22535996458710e-8))*((cj0)*(cj0))*((pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(4.18559726073583e-6))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(1.99999982660000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-7.11551534325092e-10))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(3.99999976880001))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.000144159995833776))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-3.99999976880001))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(5.75282798567641e-8))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.140063935774761))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.847999975492801))*(pz)*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(1.15599996659160e-7))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-1.15599996659160e-7))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-2.88999991647900e-8))*((cj0)*(cj0))*((py)*(py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-0.000216239993750664))*(py)*(r00)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.423999963239201))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.000157351575182608))*((r02)*(r02))))+(((IkReal(1.27199992647840))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.000432479987501328))*(cj0)*(px)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.0397120066969557))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(11.9999993064000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-1.99999994220000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.00203999994104400))*(r00)*(r01)*((px)*(px))*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(3.99999988440000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.22535996458710e-8))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-1.44667348522452e-9))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-0.423999987746400))*(pz)*((cj0)*(cj0))*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.00531864919212468))*(py)*(r01)*(r02)))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(9.77980785828634e-12))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(-3.99999976880001))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-1.27199992647840))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-1.38184577328552e-7))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(-3.99999988440000))*((px)*(px))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(8.02815474597405e-5))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(1.36478630681559e-8))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(0.424000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-0.000144159995833776))*(px)*(py)*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(7.11551534325092e-10))*(cj0)*(r00)*(r02)*((py)*(py))))+(((IkReal(-1.22535996458710e-8))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-3.99999976880001))*(cj0)*(pz)*(r01)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(-0.999999971100001))*((px)*(px)*(px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-1.27199992647840))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.000144159995833776))*(cj0)*(sj0)*((pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(0.000679999980348001))*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(1.80834067306134e-6))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0898879424717168))*(px)*(py)*(r00)*(r01)))+(((IkReal(0.000679999980348001))*(px)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(8.02815474597405e-5))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(0.423999987746400))*(pz)*((px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(px)*(r01)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.000679999980348001))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.423999975492801))*(cj0)*(r00)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(4.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-6.75105072318525e-6))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.0449439712358584))*((py)*(py))*((r01)*(r01))))+(((IkReal(-1.36478630681559e-8))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.000679999980348001))*(py)*(r00)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-5.77999983295801e-8))*((px)*(px))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.50984403073250e-6))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(3.99999976880001))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-1.22535996458710e-8))*(pz)*((px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.53708957210214e-10))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.70196880614650e-5))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.000216239993750664))*(py)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(7.11551534325092e-10))*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.000216239993750664))*(py)*(r00)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(-8.50984403073250e-6))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(1.43594146024810e-8))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-1.22535996458710e-8))*((pz)*(pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.000288319991667552))*(cj0)*(pz)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(0.000679999980348001))*(py)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-0.0397120619298817))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(1.38184577328552e-7))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(-0.423999987746400))*((cj0)*(cj0))*((pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(-3.99999988440000))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.000339999990174000))*(cj0)*(sj0)*((py)*(py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-1.44667348522452e-9))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-9.02043467257645e-7))*(r00)*(r02)*(sj0)))+(((IkReal(0.000288319991667552))*(cj0)*(pz)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(0.000339999990174000))*(r00)*(r01)*((pz)*(pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.0198560027746394))*((cj0)*(cj0))*((px)*(px))*((r01)*(r01))))+(((IkReal(0.00531864903841572))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(6.75105072318525e-6))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.00271999992139200))*(cj0)*(py)*(r00)*(r01)*(sj0)*((px)*(px)*(px))))+(((IkReal(0.000216239993750664))*(px)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(3.67607989376129e-8))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(1.36478630681559e-8))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-1.53708957210214e-10))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-3.99999988440000))*(pz)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-6.75104094337739e-6))*(px)*(py)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.000144159995833776))*(cj0)*(pz)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(5.77999983295801e-8))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.00203999994104400))*(px)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(9.77980785828634e-12))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.000216239993750664))*(py)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-2.45071992917419e-8))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(1.70196880614650e-5))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-1.36478630681559e-8))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.00135999996069600))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.00407999988208800))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.999999971100001))*((cj0)*(cj0))*((py)*(py)*(py)*(py))*((r01)*(r01))))+(((IkReal(1.17456890729270e-11))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.000339999990174000))*(r00)*(r01)*((py)*(py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.423999963239201))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-6.75105072318525e-6))*(py)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-2.45071992917419e-8))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-4.18559726073583e-6))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.000679999980348001))*(px)*(py)*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-5.73839311470746e-10))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0198560315387801))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.000339999990174000))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))))+(((IkReal(-6.75105072318525e-6))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-0.847999975492801))*(pz)*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-8.02815474597405e-5))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-9.04170336530672e-7))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.80834067306134e-6))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(1.43594146024810e-8))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-1.80834067306134e-6))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(8.02815474597405e-5))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(3.99999965320001))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(8.50984403073250e-6))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(-7.60959501990047e-5))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-2.88999991647900e-8))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(3.99999976880001))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.000679999980348001))*(px)*(py)*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(py)*((cj0)*(cj0))*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-1.22535996458710e-8))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))); -op[1]=((((IkReal(-0.000144159997916888))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-6.13389548871719e-10))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(-0.000288319995833776))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.000144159997916888))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.69599997549280))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(-0.179775997402237))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(-3.05619195583803e-5))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(5.21381116540961e-14))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-1.80834069919187e-6))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(2.89334692864019e-9))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.78669385728037e-9))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(5.78669385728037e-9))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-1.52809597791901e-5))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-0.847999987746400))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(0.847999987746400))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(-0.0898879987011184))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(1.70196878155305e-5))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-2.54399996323920))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.000288319995833776))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-5.78669385728037e-9))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.000144159997916888))*(pz)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.000288319995833776))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.847999987746400))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.0106372982305404))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-5.78669385728037e-9))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-0.179775997402237))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(0.0106372982305404))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(1.70196878155305e-5))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.000144159997916888))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(2.89334692864019e-9))*(py)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.847999987746400))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(0.000144159997916888))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.179775997402237))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-0.0106372982305404))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-5.78669385728037e-9))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-3.05619195583803e-5))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-3.05619195583803e-5))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(1.70196873236615e-5))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(1.52809597791901e-5))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(4.91868977868832e-13))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.847999987746400))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(-3.61668139838373e-6))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.179775997402237))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(-3.05619195583803e-5))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(5.21381116540961e-14))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.847999987746400))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.847999987746400))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-3.05619195583803e-5))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-1.80834069919187e-6))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-1.70196883073995e-5))*(px)*(r00)*(r02)))+(((IkReal(-1.70196883073995e-5))*(pz)*((r02)*(r02))))+(((IkReal(0.847999987746400))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(-3.05619195583803e-5))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(0.000144159997916888))*(cj0)*(px)*((py)*(py))*((r00)*(r00))))+(((IkReal(-1.70196878155305e-5))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(4.91868977868832e-13))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.847999987746400))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(-3.83368228228676e-7))*(cj0)*(r00)*(r02)))+(((IkReal(-2.89334692864019e-9))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.69599997549280))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(6.13389548871719e-10))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-1.70196883073995e-5))*(py)*(r01)*(r02)))+(((IkReal(0.0898879987011184))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.847999987746400))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.000144159997916888))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(1.69599997549280))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(1.80408690844623e-6))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(1.52809597791901e-5))*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(1.80834069919187e-6))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.0898879987011184))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(2.54399996323920))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(1.52809597791901e-5))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(-0.847999987746400))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.91868977868832e-13))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.179775997402237))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(1.80834069919187e-6))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.000432479993750664))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.0106372982305404))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-2.89334692864019e-9))*(px)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(0.179775997402237))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.00225510722487456))*(cj0)*(r01)*(r02)))+(((IkReal(0.000144159997916888))*(r00)*(r01)*(sj0)*((px)*(px)*(px))))+(((IkReal(0.847999987746400))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-3.05619195583803e-5))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(5.78669385728037e-9))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(0.847999987746400))*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.0898879987011184))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-1.80408696058435e-6))*((r02)*(r02))))+(((IkReal(0.847999987746400))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(1.70196873236615e-5))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-0.000144159997916888))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.00225510722487456))*(r00)*(r02)*(sj0)))+(((IkReal(-1.80834069919187e-6))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.0898879987011184))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-1.52809597791901e-5))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.847999987746400))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.847999987746400))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(-0.179775997402237))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(-0.000432479993750664))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.000144159997916888))*(cj0)*(pz)*(r00)*(r02)*((py)*(py))))+(((IkReal(-3.61668139838373e-6))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.000144159997916888))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-3.60817371261624e-6))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(-0.000288319995833776))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.0212745964610808))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.0898879987011184))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(1.52809597791901e-5))*(cj0)*(r00)*(r02)*((py)*(py))))+(((IkReal(-0.000144159997916888))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-3.40393746473231e-5))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-1.70196878155305e-5))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-1.52809597791901e-5))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.0212745964610808))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-3.05619195583803e-5))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(1.80408690844623e-6))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.847999987746400))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(0.0898879987011184))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(0.0106372982305404))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-0.000144159997916888))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(-1.52809597791901e-5))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.000144159997916888))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-1.80834069919187e-6))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-4.91868977868832e-13))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.847999987746400))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.000144159997916888))*(py)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.000144159997916888))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.000144159997916888))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.179775997402237))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(0.000144159997916888))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-0.0898879987011184))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.000144159997916888))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(-0.847999987746400))*(cj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.847999987746400))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-1.69599997549280))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-3.83368228228676e-7))*(r01)*(r02)*(sj0)))+(((IkReal(-0.0106372982305404))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(-6.13389548871719e-10))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(6.13389548871719e-10))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(0.000144159997916888))*(cj0)*(r00)*(r01)*((py)*(py)*(py))))); -op[2]=((((IkReal(4.90367290720905e-7))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(-1.99999994220000))*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(0.00135999996069600))*(px)*((cj0)*(cj0))*((py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-0.000168934289440953))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.129600006696956))*(pp)*((r02)*(r02))))+(((IkReal(0.140063931726913))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.000152191900398009))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.00543999984278400))*(cj0)*(py)*(r00)*(r01)*(sj0)*((px)*(px)*(px))))+(((IkReal(23.9999986128000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-2.58726230676616e-8))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(2.31199993318320e-7))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(4.76217367871506e-5))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.169312070922195))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.000679999980348001))*(r00)*(r01)*((py)*(py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-2.72957261363118e-8))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(8.37119452147167e-6))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(-7.99999953760001))*(cj0)*(pz)*(r00)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-0.00271999992139200))*(cj0)*(pz)*(r01)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.280127970414691))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.00271999992139200))*(cj0)*(pz)*(r00)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(-9.20084336602798e-10))*(r01)*(r02)*(sj0)))+(((IkReal(4.76217367871506e-5))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-1.95596157165727e-11))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(-1.99999994220000))*((px)*(px)*(px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.00407999988208800))*(cj0)*(sj0)*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-3.99999976880001))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py)*(py)*(py))))+(((IkReal(-4.16812197112770e-11))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-7.99999953760001))*(px)*(r00)*(r01)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-7.99999953760001))*(py)*(r00)*(r01)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-5.77999983295801e-8))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-1.66256733590868e-15))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-7.99999953760001))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0898879424717168))*((py)*(py))*((r01)*(r01))))+(((IkReal(7.99999976880001))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.000160563094919481))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(0.140063931726913))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-1.95596157165727e-11))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(-8.37119452147167e-6))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(-7.99999976880001))*((px)*(px))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.000679999980348001))*(r00)*(r01)*((pz)*(pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-7.99999965320001))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r00)*(r00))))+(((IkReal(0.000160563094919481))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(-2.72957261363118e-8))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-4.16812197112770e-11))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-2.31199993318320e-7))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(3.99999965320001))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-7.99999953760001))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(0.140063989255195))*((cj0)*(cj0))*((px)*(px))*((r01)*(r01))))+(((IkReal(0.00407999988208800))*(py)*(pz)*(r00)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(23.9999986128000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(4.04784928947514e-9))*((px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-7.99999953760001))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-0.00135999996069600))*(px)*(py)*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.00543999984278400))*(cj0)*(px)*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(2.87188292049620e-8))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-8.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.280127970414691))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-4.76217563467663e-5))*(px)*(py)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.00407999988208800))*(px)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(-0.140063989255195))*((cj0)*(cj0))*((py)*(py))*((r01)*(r01))))+(((IkReal(-4.76217367871506e-5))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.00135999996069600))*(py)*((cj0)*(cj0))*((px)*(px)*(px))*((r01)*(r01))))+(((IkReal(4.90367290720905e-7))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.000160563094919481))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-4.76217563467663e-5))*(cj0)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(0.00407999988208800))*(r00)*(r01)*((px)*(px))*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-3.99999988440000))*((py)*(py))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.15599996659160e-7))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-5.77999983295801e-8))*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(3.99999988440000))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-7.99999976880001))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(7.99999953760001))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-7.99999976880001))*(pz)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(8.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.00135999996069600))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.95596157165727e-11))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(0.00135999996069600))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.00288451339146642))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(-0.00135999996069600))*(px)*((py)*(py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-15.9999993064000))*(py)*(pz)*(r01)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(py)*((cj0)*(cj0))*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-5.77999983295801e-8))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00135999996069600))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-0.280127863453827))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(4.76217367871506e-5))*(py)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-15.9999993064000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(0.00135999996069600))*(px)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(3.99999965320001))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.00135999996069600))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-2.31199993318320e-7))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.0794241284504778))*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.280127855358132))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.00407999988208800))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))*((py)*(py))))+(((IkReal(2.72957261363118e-8))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.000160563094919481))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(7.99999976880001))*(py)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-2.31199993318320e-7))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.000679999980348001))*(r00)*(r01)*((px)*(px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(4.04784928947514e-9))*((cj0)*(cj0))*((py)*(py))*((r00)*(r00))))+(((IkReal(-2.72957261363118e-8))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((py)*(py)*(py)*(py))*((r01)*(r01))))+(((IkReal(0.00271999992139200))*(cj0)*(py)*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-4.90367290720905e-7))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.00407999988208800))*(cj0)*(sj0)*((px)*(px))*((py)*(py))*((r00)*(r00))))+(((IkReal(0.000160563094919481))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(4.76217563467663e-5))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-8.09569525381560e-9))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00271999992139200))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-5.77999983295801e-8))*((px)*(px)*(px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.280127855358132))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(-0.00135999996069600))*(pz)*(r00)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(7.99999953760001))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.72957261363118e-8))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.00135999996069600))*(px)*(r01)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-7.99999965320001))*((px)*(px))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.280127863453827))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-5.77999983295801e-8))*((cj0)*(cj0))*((py)*(py)*(py)*(py))*((r00)*(r00))))+(((IkReal(0.000679999980348001))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px)*(px))))+(((IkReal(-3.99999988440000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(2.87188292049620e-8))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(8.00000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(4.76217563467663e-5))*(px)*(py)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00407999988208800))*(px)*(pz)*(r01)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.280127986606089))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(2.31199993318320e-7))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-9.20084336602798e-10))*(cj0)*(r00)*(r02)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.00815999976417601))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.00194040410709565))*((r02)*(r02))))+(((IkReal(-2.58726230676616e-8))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.000168934289440953))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.00135999996069600))*(py)*(r00)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(pz)*(r01)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((px)*(px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-0.140063989255195))*((px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(0.00135999996069600))*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-4.04784928947514e-9))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(0.00271999992139200))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.280127855358132))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.00144225673741443))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-7.99999953760001))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r00)*(r00))))+(((IkReal(0.00135999996069600))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.000160563094919481))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.00271999992139200))*(cj0)*(px)*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-1.42310306865018e-9))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(0.00135999996069600))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-7.99999953760001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(7.99999953760001))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(7.99999953760001))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-4.76217563467663e-5))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.140063989255195))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(15.9999990752000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.00144225673741443))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(0.000679999980348001))*(r00)*(r01)*((cj0)*(cj0))*((py)*(py)*(py)*(py))))+(((IkReal(-2.72957261363118e-8))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-2.31199993318320e-7))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.000160563094919481))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(0.00271999992139200))*(px)*(py)*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-1.15599996659160e-7))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-1.99999994220000))*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(4.04784762690780e-9))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.95596157165727e-11))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-7.99999976880001))*(pz)*(r00)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(23.9999986128000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(-7.99999953760001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.15056559713528e-7))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-16.0000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(1.42310306865018e-9))*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(7.99999930640002))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))))+(((IkReal(-4.90367290720905e-7))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(-5.41226080354587e-6))*(cj0)*(r01)*(r02)))+(((IkReal(4.76217563467663e-5))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(4.76217563467663e-5))*(cj0)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(-1.99999994220000))*((py)*(py)*(py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-8.37119452147167e-6))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-7.99999953760001))*(cj0)*(pz)*(r01)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(3.99999988440000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.0794241284504778))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.0898879424717168))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.00135999996069600))*(py)*((px)*(px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-4.04784928947514e-9))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-5.75282815193314e-8))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.04784762690780e-9))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.42310306865018e-9))*(cj0)*(r00)*(r02)*((py)*(py))))+(((IkReal(-9.52434735743012e-5))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.00407999988208800))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-1.66256733590868e-15))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-4.76217367871506e-5))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.179775884943434))*(px)*(py)*(r00)*(r01)))+(((IkReal(-1.99999994220000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r01)*(r01))))+(((IkReal(7.99999930640002))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.00135999996069600))*(px)*((cj0)*(cj0))*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-1.99999994220000))*((cj0)*(cj0))*((py)*(py)*(py)*(py))*((r01)*(r01))))+(((IkReal(5.41226080354587e-6))*(r00)*(r02)*(sj0)))+(((IkReal(1.15599996659160e-7))*((py)*(py))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.76217367871506e-5))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.00135999996069600))*(px)*((py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(9.52434735743012e-5))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-5.77999983295801e-8))*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(-5.75282815193314e-8))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(-4.76217563467663e-5))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.00271999992139200))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.00135999996069600))*(px)*(py)*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(8.37119452147167e-6))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(3.99999976880001))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz)*(pz)*(pz))))+(((IkReal(-4.76217367871506e-5))*(px)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.280127986606089))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.00815999976417601))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-4.76217367871506e-5))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(2.72957261363118e-8))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.000152191900398009))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-0.00135999996069600))*(py)*((px)*(px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.15599996659160e-7))*((px)*(px))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000160563094919481))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(2.72957261363118e-8))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(-1.42310306865018e-9))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.00135999996069600))*(py)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-16.0000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.09569525381560e-9))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((py)*(py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-3.99999976880001))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px)*(px)*(px))))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))); -op[3]=((((IkReal(-0.000144159997916888))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.000288319995833776))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.000144159997916888))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.69599997549280))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(0.0898879987011184))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-1.52809597791901e-5))*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-1.80834069919187e-6))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-5.21381116540961e-14))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(2.89334692864019e-9))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.78669385728037e-9))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(5.78669385728037e-9))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0898879987011184))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(-0.847999987746400))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(0.847999987746400))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(1.70196878155305e-5))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-2.54399996323920))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.000288319995833776))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.00225510722487456))*(r00)*(r02)*(sj0)))+(((IkReal(-5.78669385728037e-9))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.000144159997916888))*(pz)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.000288319995833776))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.847999987746400))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.0106372982305404))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(3.83368228228676e-7))*(cj0)*(r00)*(r02)))+(((IkReal(-5.78669385728037e-9))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(1.80408696058435e-6))*((r02)*(r02))))+(((IkReal(0.0106372982305404))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(1.70196878155305e-5))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.000144159997916888))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(2.89334692864019e-9))*(py)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.847999987746400))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(0.000144159997916888))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.179775997402237))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(3.05619195583803e-5))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.179775997402237))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(-0.0106372982305404))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-5.78669385728037e-9))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(3.60817371261624e-6))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(1.52809597791901e-5))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(1.70196873236615e-5))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-6.13389548871719e-10))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(3.05619195583803e-5))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(4.91868977868832e-13))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.847999987746400))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(-3.61668139838373e-6))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.847999987746400))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.847999987746400))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.179775997402237))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(-1.80834069919187e-6))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-1.70196883073995e-5))*(px)*(r00)*(r02)))+(((IkReal(-1.70196883073995e-5))*(pz)*((r02)*(r02))))+(((IkReal(0.847999987746400))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(0.000144159997916888))*(cj0)*(px)*((py)*(py))*((r00)*(r00))))+(((IkReal(-1.70196878155305e-5))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(6.13389548871719e-10))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.91868977868832e-13))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.847999987746400))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(-1.52809597791901e-5))*(cj0)*(r00)*(r02)*((py)*(py))))+(((IkReal(-1.52809597791901e-5))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(-2.89334692864019e-9))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.52809597791901e-5))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(1.69599997549280))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.179775997402237))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-1.70196883073995e-5))*(py)*(r01)*(r02)))+(((IkReal(0.179775997402237))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(0.847999987746400))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.000144159997916888))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(1.69599997549280))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.179775997402237))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.0898879987011184))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(1.80834069919187e-6))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(1.52809597791901e-5))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(2.54399996323920))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(3.83368228228676e-7))*(r01)*(r02)*(sj0)))+(((IkReal(-0.847999987746400))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.91868977868832e-13))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.80834069919187e-6))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(1.52809597791901e-5))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.000432479993750664))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(3.05619195583803e-5))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.0106372982305404))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(3.05619195583803e-5))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-2.89334692864019e-9))*(px)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.179775997402237))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-0.0898879987011184))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.000144159997916888))*(r00)*(r01)*(sj0)*((px)*(px)*(px))))+(((IkReal(0.847999987746400))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(5.78669385728037e-9))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(0.847999987746400))*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.0898879987011184))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.847999987746400))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(1.70196873236615e-5))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(6.13389548871719e-10))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(-0.000144159997916888))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.0898879987011184))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-1.80834069919187e-6))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-6.13389548871719e-10))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(-1.80408690844623e-6))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.847999987746400))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.847999987746400))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(3.05619195583803e-5))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.179775997402237))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.000432479993750664))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.000144159997916888))*(cj0)*(pz)*(r00)*(r02)*((py)*(py))))+(((IkReal(-3.61668139838373e-6))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.000144159997916888))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-1.80408690844623e-6))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(3.05619195583803e-5))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.000288319995833776))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.0212745964610808))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.000144159997916888))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(3.05619195583803e-5))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-3.40393746473231e-5))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-1.70196878155305e-5))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.0212745964610808))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(0.0898879987011184))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0898879987011184))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(0.847999987746400))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(0.0106372982305404))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-0.000144159997916888))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(-0.000144159997916888))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-1.80834069919187e-6))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-4.91868977868832e-13))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.52809597791901e-5))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.847999987746400))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.000144159997916888))*(py)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.000144159997916888))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.000144159997916888))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-5.21381116540961e-14))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(3.05619195583803e-5))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(0.000144159997916888))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-0.000144159997916888))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(-0.847999987746400))*(cj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.847999987746400))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-1.69599997549280))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.0106372982305404))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(0.00225510722487456))*(cj0)*(r01)*(r02)))+(((IkReal(0.000144159997916888))*(cj0)*(r00)*(r01)*((py)*(py)*(py))))); -op[4]=((((IkReal(-0.000812850431382459))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(-1.53708957210214e-10))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.424000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(0.000144159995833776))*(cj0)*(pz)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(0.00531864903841572))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(pz)*(r00)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(1.17456890729270e-11))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(pz)*(r01)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(px)*((py)*(py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(5.73838480187078e-10))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-1.27199996323920))*(py)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-9.04170336530672e-7))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.00203999994104400))*(py)*(pz)*(r00)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(-9.77980785828634e-12))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(-8.02815474597405e-5))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(-8.31283667954339e-16))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.000144159995833776))*(cj0)*(sj0)*((pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(1.27199992647840))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.000432479987501328))*(cj0)*(py)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.0397120066969557))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.847999950985601))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(0.000339999990174000))*(cj0)*(sj0)*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-1.29363115338308e-8))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-1.99999994220000))*((py)*(py))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-1.38184577328552e-7))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.000432479987501328))*(cj0)*(px)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.0449439712358584))*((px)*(px))*((r00)*(r00))))+(((IkReal(-9.77980785828634e-12))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(0.00203999994104400))*(px)*(pz)*(r01)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-1.44667348522452e-9))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-5.73838480187078e-10))*((cj0)*(cj0))*((py)*(py))*((r00)*(r00))))+(((IkReal(-0.000216239993750664))*(py)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(5.73838480187078e-10))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.44667348522452e-9))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(8.02815474597405e-5))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(-1.53708957210214e-10))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-8.02815474597405e-5))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.00135999996069600))*(cj0)*(pz)*(r01)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-0.999999971100001))*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(3.99999976880001))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(1.99999982660000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-5.73838480187078e-10))*((px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.000679999980348001))*(px)*((cj0)*(cj0))*((py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-0.848000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.0198560027746394))*((cj0)*(cj0))*((py)*(py))*((r01)*(r01))))+(((IkReal(1.53708957210214e-10))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.000144159995833776))*(r00)*(r01)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.0198560027746394))*((px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-3.99999988440000))*(pz)*(r00)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-8.50984403073250e-6))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(8.50984403073250e-6))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(1.22535996458710e-8))*((cj0)*(cj0))*((pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(11.9999993064000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(-0.423999963239201))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-3.67607989376129e-8))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-4.18559726073583e-6))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(-0.00531864888470676))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.847999950985601))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-2.88999991647900e-8))*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.44667348522452e-9))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.00135999996069600))*(px)*(py)*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.0250879966515221))*(pp)*((r02)*(r02))))+(((IkReal(0.000144159995833776))*(cj0)*(r01)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(1.53347389433800e-10))*(cj0)*(r00)*(r02)))+(((IkReal(1.99999994220000))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.29363115338308e-8))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(8.50984403073250e-6))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-1.15599996659160e-7))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.00531864903841572))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(6.75104094337739e-6))*(px)*(py)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-0.0397120619298817))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0397120044016018))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-1.99999988440000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px)*(px)*(px))))+(((IkReal(4.00000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-0.0198560027746394))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(cj0)*(px)*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-7.99999965320001))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(-0.999999971100001))*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(6.75104094337739e-6))*(cj0)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(1.53347389433800e-10))*(r01)*(r02)*(sj0)))+(((IkReal(1.14767862294149e-9))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-2.87641407596657e-8))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-6.75104094337739e-6))*(cj0)*(sj0)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.000339999990174000))*(cj0)*(sj0)*((pz)*(pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(-6.75104094337739e-6))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(6.75104094337739e-6))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-8.02815474597405e-5))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(-2.88999991647900e-8))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000144159995833776))*(px)*(py)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(7.20799979168881e-5))*(r01)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-6.75105072318525e-6))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(4.18559726073583e-6))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(-2.88999991647900e-8))*((px)*(px)*(px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.80834067306134e-6))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(1.38184577328552e-7))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-8.31283667954339e-16))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(0.0397120630775603))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.00407999988208800))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.000144159995833776))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-0.140063935774761))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-0.0397120044016018))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.847999963239201))*(py)*(r01)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(6.75105072318525e-6))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(9.04170336530672e-7))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.22535996458710e-8))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.000144159995833776))*(px)*(py)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(1.99999988440000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz)*(pz)*(pz))))+(((IkReal(-6.75104094337739e-6))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-7.11551534325092e-10))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(-0.00203999994104400))*(cj0)*(sj0)*((px)*(px))*((py)*(py))*((r00)*(r00))))+(((IkReal(-1.36478630681559e-8))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.000144159995833776))*(px)*(py)*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.423999987746400))*(pz)*((px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(6.75105072318525e-6))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.000679999980348001))*(py)*((cj0)*(cj0))*((px)*(px)*(px))*((r01)*(r01))))+(((IkReal(9.02043467257645e-7))*(cj0)*(r01)*(r02)))+(((IkReal(8.44671447204764e-5))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-1.22535996458710e-8))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.80834067306134e-6))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.000144159995833776))*(cj0)*(pz)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.847999963239201))*(px)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(-0.0198560315387801))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(3.99999988440000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-1.15599996659160e-7))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-1.27199996323920))*(px)*(r00)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(px)*((cj0)*(cj0))*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.00203999994104400))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))*((py)*(py))))+(((IkReal(0.000679999980348001))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-5.73839311470746e-10))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.000339999990174000))*(r00)*(r01)*((cj0)*(cj0))*((py)*(py)*(py)*(py))))+(((IkReal(1.35021014463705e-5))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-3.67607989376129e-8))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-2.87641407596657e-8))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(7.60959501990047e-5))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.848000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.000216239993750664))*(px)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.00531864919212468))*(pz)*((r02)*(r02))))+(((IkReal(1.80834067306134e-6))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-3.99999982660001))*((px)*(px))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.27199992647840))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.00135999996069600))*(cj0)*(py)*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(1.99999994220000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.000679999980348001))*(py)*((px)*(px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(2.45071992917419e-8))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(1.15599996659160e-7))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-1.36478630681559e-8))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(1.53708957210214e-10))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.15599996659160e-7))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.00203999994104400))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-1.99999988440000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py)*(py)*(py))))+(((IkReal(0.000406425227436919))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-2.88999991647900e-8))*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.0951199645389027))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.000144159995833776))*(cj0)*(r00)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(0.00203999994104400))*(cj0)*(sj0)*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.424000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(0.000432479987501328))*(cj0)*(px)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.0397120630775603))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.999999971100001))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-0.999999971100001))*((py)*(py)*(py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.0397120619298817))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-7.20799979168881e-5))*(r01)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.000679999980348001))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.22535996458710e-8))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.000144159995833776))*(cj0)*(pz)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(-1.27199992647840))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.00531864888470676))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-3.99999976880001))*(px)*(r00)*(r01)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-0.000679999980348001))*(py)*((px)*(px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-7.99999965320001))*(py)*(pz)*(r01)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(0.000216239993750664))*(py)*(r00)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(6.75105072318525e-6))*(px)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.000339999990174000))*(cj0)*(sj0)*((py)*(py)*(py)*(py))*((r01)*(r01))))+(((IkReal(1.36478630681559e-8))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-9.04170336530672e-7))*(py)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.000339999990174000))*(cj0)*(sj0)*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.000679999980348001))*(px)*((py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.69599990197120))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.000339999990174000))*(r00)*(r01)*((px)*(px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(-5.77999983295801e-8))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.36478630681559e-8))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-1.35021014463705e-5))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-1.27199992647840))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-8.44671447204764e-5))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(1.69600000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(0.000144159995833776))*(cj0)*(pz)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.847999950985601))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.000339999990174000))*(cj0)*(sj0)*((px)*(px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-3.99999976880001))*(cj0)*(pz)*(r00)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.00531864903841572))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.22535996458710e-8))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.000339999990174000))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px)*(px))))+(((IkReal(11.9999993064000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(1.80834067306134e-6))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(3.99999965320001))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.423999987746400))*(pz)*((cj0)*(cj0))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.000216239993750664))*(px)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(1.14767862294149e-9))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.77999983295801e-8))*((py)*(py))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(6.75104094337739e-6))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.000406425227436919))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.69600000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(0.847999975492801))*(pz)*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-3.99999982660001))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r00)*(r00))))+(((IkReal(0.00135999996069600))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.000288319991667552))*(px)*(py)*(pz)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(7.99999953760001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.00271999992139200))*(cj0)*(px)*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(0.423999975492801))*(cj0)*(r01)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(-3.99999976880001))*(py)*(r00)*(r01)*((px)*(px)*(px))*((sj0)*(sj0))))+(((IkReal(0.00135999996069600))*(cj0)*(pz)*(r00)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(4.18559726073583e-6))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(1.99999982660000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-7.11551534325092e-10))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(3.99999976880001))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-3.99999976880001))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(1.80834067306134e-6))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(0.423999987746400))*((pz)*(pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(5.75282798567641e-8))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.140063935774761))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.000216239993750664))*(py)*(r00)*(r02)*((px)*(px))*((sj0)*(sj0))))+(((IkReal(1.15599996659160e-7))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-1.15599996659160e-7))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-2.88999991647900e-8))*((cj0)*(cj0))*((py)*(py)*(py)*(py))*((r00)*(r00))))+(((IkReal(-0.000157351575182608))*((r02)*(r02))))+(((IkReal(0.000216239993750664))*(px)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(0.0397120066969557))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(11.9999993064000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-1.99999994220000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.000432479987501328))*(cj0)*(py)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.00203999994104400))*(r00)*(r01)*((px)*(px))*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(0.00531864919212468))*(px)*(r00)*(r02)))+(((IkReal(-0.000679999980348001))*(cj0)*(sj0)*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.848000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(3.99999988440000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(0.000679999980348001))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.423999975492801))*(cj0)*(r00)*(r02)*(sj0)*((py)*(py)*(py))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(9.77980785828634e-12))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(-3.99999976880001))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r01)*(r01))))+(((IkReal(-1.80834067306134e-6))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-1.38184577328552e-7))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(-8.50984403073250e-6))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-3.99999988440000))*((px)*(px))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(8.02815474597405e-5))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(1.36478630681559e-8))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(0.000144159995833776))*(px)*(py)*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.423999987746400))*(pz)*((cj0)*(cj0))*((px)*(px))*((r01)*(r01))))+(((IkReal(7.11551534325092e-10))*(cj0)*(r00)*(r02)*((py)*(py))))+(((IkReal(-3.99999976880001))*(cj0)*(pz)*(r01)*(r02)*(sj0)*((px)*(px)*(px))))+(((IkReal(-0.000288319991667552))*(cj0)*(pz)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.999999971100001))*((px)*(px)*(px)*(px))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.000216239993750664))*(py)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(8.50984403073250e-6))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(0.000679999980348001))*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.0898879424717168))*(px)*(py)*(r00)*(r01)))+(((IkReal(0.000679999980348001))*(px)*(r01)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(8.02815474597405e-5))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-8.50984403073250e-6))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(1.44667348522452e-9))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-0.000679999980348001))*(px)*(r01)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.00135999996069600))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.22535996458710e-8))*(pz)*((px)*(px))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.000144159995833776))*(cj0)*(sj0)*((pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(0.000679999980348001))*(px)*(py)*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(4.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.423999987746400))*(pz)*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-6.75105072318525e-6))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-7.20799979168881e-5))*(r00)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(7.20799979168881e-5))*(r00)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.423999963239201))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(0.423999987746400))*(r01)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-0.0449439712358584))*((py)*(py))*((r01)*(r01))))+(((IkReal(-1.36478630681559e-8))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.000679999980348001))*(py)*(r00)*(r02)*((pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-5.77999983295801e-8))*((px)*(px))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.44667348522452e-9))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(3.99999976880001))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.45071992917419e-8))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(7.11551534325092e-10))*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(1.43594146024810e-8))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.000679999980348001))*(py)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)*(pz))))+(((IkReal(-0.0397120619298817))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(1.38184577328552e-7))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(-2.89334697044905e-9))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(0.423999987746400))*((cj0)*(cj0))*((pz)*(pz)*(pz))*((r01)*(r01))))+(((IkReal(-3.99999988440000))*((cj0)*(cj0))*((py)*(py))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.000339999990174000))*(cj0)*(sj0)*((py)*(py)*(py)*(py))*((r00)*(r00))))+(((IkReal(0.848000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-9.02043467257645e-7))*(r00)*(r02)*(sj0)))+(((IkReal(0.000339999990174000))*(r00)*(r01)*((pz)*(pz)*(pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.848000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.0198560027746394))*((cj0)*(cj0))*((px)*(px))*((r01)*(r01))))+(((IkReal(1.70196880614650e-5))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(6.75105072318525e-6))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.00271999992139200))*(cj0)*(py)*(r00)*(r01)*(sj0)*((px)*(px)*(px))))+(((IkReal(9.04170336530672e-7))*(px)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(1.69600000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.22535996458710e-8))*(pz)*((cj0)*(cj0))*((py)*(py))*((r00)*(r00))))+(((IkReal(1.36478630681559e-8))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.0106372977694135))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-1.44667348522452e-9))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-3.99999976880001))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-3.99999988440000))*(pz)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py)*(py))))+(((IkReal(-6.75104094337739e-6))*(px)*(py)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(5.77999983295801e-8))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.00203999994104400))*(px)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((py)*(py))))+(((IkReal(9.77980785828634e-12))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.00531864919212468))*(py)*(r01)*(r02)))+(((IkReal(-0.00531864903841572))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-1.36478630681559e-8))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.00135999996069600))*(cj0)*(sj0)*((py)*(py))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.000216239993750664))*(px)*(r01)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(0.00407999988208800))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.999999971100001))*((cj0)*(cj0))*((py)*(py)*(py)*(py))*((r01)*(r01))))+(((IkReal(1.17456890729270e-11))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.000339999990174000))*(r00)*(r01)*((py)*(py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.424000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(-6.75105072318525e-6))*(py)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-4.18559726073583e-6))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.000679999980348001))*(px)*(py)*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-5.73839311470746e-10))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0198560315387801))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.70196880614650e-5))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-0.000339999990174000))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz)*(pz)*(pz))))+(((IkReal(-6.75105072318525e-6))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(0.000288319991667552))*(px)*(py)*(pz)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(0.847999975492801))*(pz)*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(-8.02815474597405e-5))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(1.43594146024810e-8))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(8.02815474597405e-5))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(0.000288319991667552))*(cj0)*(pz)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(3.99999965320001))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(1.22535996458710e-8))*((pz)*(pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.89334697044905e-9))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-7.60959501990047e-5))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-2.88999991647900e-8))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(3.99999976880001))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.000679999980348001))*(px)*(py)*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.000679999980348001))*(py)*((cj0)*(cj0))*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.423999987746400))*(r00)*(r02)*((px)*(px)*(px))*((sj0)*(sj0))))); +op[0]=((((4.4e-7)*cj0*py*(px*px)*(r01*r01)))+(((-1.0)*(cj0*cj0)*(py*py*py*py)*(r01*r01)))+(((0.005318656)*pz*(cj0*cj0)*(r01*r01)))+(((1.17010432e-9)*r00*r02*sj0))+(((-8.0)*px*pz*r00*r02*(cj0*cj0)*(py*py)))+(((2.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r01*r01)))+(((-4.4e-7)*cj0*py*(px*px)*(r02*r02)))+(((-8.8e-7)*pz*r00*r02*sj0*(py*py)))+(((-4.0)*cj0*pz*r01*r02*sj0*(px*px*px)))+(((0.039712)*cj0*r00*r01*sj0*(pz*pz)))+(((2.0)*cj0*r00*r01*sj0*(pz*pz*pz*pz)))+(((4.0)*py*r00*r01*(cj0*cj0)*(px*px*px)))+(((8.0)*cj0*px*py*sj0*(pz*pz)*(r02*r02)))+(((-5.51936e-9)*cj0*py*(r01*r01)))+(((-0.140064)*py*pz*r01*r02))+(((-8.0)*py*pz*r01*r02*(px*px)*(sj0*sj0)))+(((1.32e-6)*py*r00*r01*sj0*(px*px)))+(((4.0)*cj0*py*sj0*(px*px*px)*(r00*r00)))+(((1.696)*cj0*px*py*pz*sj0*(r02*r02)))+(((-4.0)*(px*px)*(pz*pz)*(r02*r02)*(sj0*sj0)))+(((-4.0)*(px*px)*(pz*pz)*(r00*r00)))+(((0.848)*px*py*pz*r00*r01*(sj0*sj0)))+(((0.848)*px*py*pz*r00*r01*(cj0*cj0)))+(((-0.424)*r01*r02*(cj0*cj0)*(py*py*py)))+(((-0.005318656)*pz*(r02*r02)))+(((-4.0)*px*r00*r01*(cj0*cj0)*(py*py*py)))+(((-0.039712)*cj0*py*pz*r00*r02*sj0))+(((2.0)*(px*px)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.0)*(px*px*px*px)*(r00*r00)*(sj0*sj0)))+(((-8.8e-7)*cj0*py*(px*px)*(r00*r00)))+(((-0.005318656)*py*r01*r02))+(((-1.0)*(cj0*cj0)*(pz*pz*pz*pz)*(r01*r01)))+(((-4.0)*cj0*px*r01*r02*sj0*(pz*pz*pz)))+(((-4.0)*py*r00*r01*(px*px*px)*(sj0*sj0)))+(((4.4e-7)*px*sj0*(pz*pz)*(r02*r02)))+(((0.005318656)*cj0*px*r01*r02*sj0))+(((4.0)*px*py*r00*r01*(cj0*cj0)*(pz*pz)))+(((-0.848)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((-0.424)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.848)*cj0*px*py*pz*sj0*(r01*r01)))+(((0.848)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((0.0250879999999516)*(px*px)*(r02*r02)))+(((-0.019856)*(cj0*cj0)*(px*px)*(r01*r01)))+(((0.424)*py*r01*r02*(px*px)))+(((-0.424)*(pz*pz*pz)*(r02*r02)))+(((12.0)*cj0*px*pz*r01*r02*sj0*(py*py)))+(((-5.51936e-9)*px*sj0*(r02*r02)))+(((-4.0)*cj0*py*r00*r02*sj0*(pz*pz*pz)))+(((-0.019856)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((5.51936e-9)*py*r00*r01*sj0))+(((-2.0)*(px*px)*(py*py)*(r02*r02)))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r01*r01)))+(((0.424)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-8.0)*px*py*r00*r01*(pz*pz)))+(((2.0)*(cj0*cj0)*(px*px)*(py*py)*(r01*r01)))+(((-0.039712)*cj0*px*py*sj0*(r01*r01)))+(((1.103872e-8)*pz*r00*r02*sj0))+(((-0.000157351935999393)*(r02*r02)))+(((-0.005318656)*px*r00*r02*(sj0*sj0)))+(((0.019856)*(cj0*cj0)*(py*py)*(r01*r01)))+(((5.51936e-9)*cj0*py*(r02*r02)))+(((-0.019856)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-4.0)*cj0*px*sj0*(py*py*py)*(r00*r00)))+(((-2.0)*cj0*r00*r01*sj0*(py*py*py*py)))+(((2.0)*(px*px)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.039712)*cj0*px*py*sj0*(r00*r00)))+(((0.039712)*py*pz*r01*r02*(cj0*cj0)))+(((-9.328e-8)*cj0*px*py*r00*r02))+(((1.272)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((-0.039712)*cj0*px*pz*r01*r02*sj0))+(((4.0)*pz*r01*r02*(py*py*py)))+(((-0.424)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((1.272)*cj0*py*r00*r02*sj0*(px*px)))+(((-0.848)*px*r00*r02*(cj0*cj0)*(py*py)))+(((-1.32e-6)*cj0*px*r00*r01*(py*py)))+(((-4.4e-7)*cj0*py*(pz*pz)*(r02*r02)))+(((-0.848)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((-4.0)*py*r01*r02*(pz*pz*pz)))+(((-0.424)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((-4.0)*cj0*pz*r00*r02*sj0*(py*py*py)))+(((-1.272)*cj0*py*r00*r02*sj0*(pz*pz)))+(((-4.0)*cj0*py*sj0*(px*px*px)*(r01*r01)))+(((-0.424)*cj0*r01*r02*sj0*(px*px*px)))+(((-1.17010432e-9)*cj0*r01*r02))+(((4.4e-7)*sj0*(px*px*px)*(r02*r02)))+(((4.0)*px*r00*r01*(py*py*py)*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(px*px)))+(((9.328e-8)*py*pz*r00*r01*sj0))+(((-4.4e-7)*px*sj0*(py*py)*(r00*r00)))+(((-4.4e-7)*cj0*py*(pz*pz)*(r01*r01)))+(((0.424)*px*r00*r02*(py*py)))+(((-4.4e-7)*cj0*px*r00*r01*(pz*pz)))+(((4.4e-7)*px*sj0*(py*py)*(r02*r02)))+(((-9.328e-8)*r00*r02*sj0*(py*py)))+(((9.328e-8)*px*pz*sj0*(r00*r00)))+(((4.0)*cj0*px*sj0*(py*py*py)*(r01*r01)))+(((4.0)*pz*r00*r02*(px*px*px)))+(((0.0250879999999516)*(py*py)*(r02*r02)))+(((-0.424)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((4.4e-7)*cj0*r00*r01*(px*px*px)))+(((-1.272)*cj0*px*r01*r02*sj0*(pz*pz)))+(((-0.848)*pz*(px*px)*(r00*r00)))+(((1.272)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((0.424)*r00*r02*(px*px*px)))+(((5.51936e-9)*px*sj0*(r00*r00)))+(((-4.0)*pz*r01*r02*(cj0*cj0)*(py*py*py)))+(((-4.4e-7)*r00*r01*sj0*(py*py*py)))+(((-0.140064)*px*pz*r00*r02))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r00*r00)))+(((-4.0)*pz*r00*r02*(px*px*px)*(sj0*sj0)))+(((-1.696)*px*py*pz*r00*r01))+(((-2.0)*(py*py)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.848)*pz*(py*py)*(r01*r01)))+(((-0.848)*cj0*px*py*pz*sj0*(r00*r00)))+(((-9.328e-8)*cj0*py*pz*(r01*r01)))+(((2.0)*(px*px)*(pz*pz)*(r02*r02)))+(((4.4e-7)*sj0*(px*px*px)*(r00*r00)))+(((0.424)*pz*(px*px)*(r02*r02)))+(((-0.0008128512)*cj0*r00*r01*sj0))+(((-1.0)*(py*py*py*py)*(r02*r02)))+(((0.019856)*(px*px)*(r00*r00)*(sj0*sj0)))+(((-2.0)*(cj0*cj0)*(px*px)*(pz*pz)*(r01*r01)))+(((4.0)*px*r00*r02*(pz*pz*pz)*(sj0*sj0)))+(((-0.010637312)*cj0*pz*r00*r01*sj0))+(((-0.070032)*(pz*pz)*(r02*r02)))+(((0.005318656)*pz*(r00*r00)*(sj0*sj0)))+(((-0.424)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((-2.0)*cj0*r00*r01*sj0*(px*px*px*px)))+(((0.0004064256)*(cj0*cj0)*(r01*r01)))+(((-1.0)*(pz*pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-1.272)*px*r00*r02*(pz*pz)))+(((4.0)*px*pz*r00*r02*(py*py)*(sj0*sj0)))+(((9.328e-8)*px*pz*sj0*(r02*r02)))+(((-4.4e-7)*cj0*(py*py*py)*(r02*r02)))+(((-8.8e-7)*cj0*px*py*pz*r00*r02))+(((0.039712)*px*py*r00*r01*(sj0*sj0)))+(((-0.424)*cj0*r00*r02*sj0*(py*py*py)))+(((0.424)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((4.0)*py*r01*r02*(cj0*cj0)*(pz*pz*pz)))+(((9.328e-8)*px*py*r01*r02*sj0))+(((-4.0)*(cj0*cj0)*(px*px)*(py*py)*(r00*r00)))+(((9.328e-8)*cj0*r01*r02*(px*px)))+(((-4.0)*(py*py)*(pz*pz)*(r01*r01)))+(((-1.0)*(pz*pz*pz*pz)*(r02*r02)))+(((0.424)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((12.0)*cj0*r00*r01*sj0*(px*px)*(py*py)))+(((-0.005318656)*px*r00*r02))+(((-4.0)*px*r00*r02*(pz*pz*pz)))+(((0.0004064256)*(r00*r00)*(sj0*sj0)))+(((-9.328e-8)*cj0*px*pz*r00*r01))+(((4.4e-7)*px*sj0*(pz*pz)*(r00*r00)))+(((-0.0449440000000484)*(py*py)*(r01*r01)))+(((0.424)*r01*r02*(py*py*py)))+(((8.8e-7)*cj0*pz*r01*r02*(px*px)))+(((4.0)*px*py*r00*r01*(pz*pz)*(sj0*sj0)))+(((-0.005318656)*py*r01*r02*(cj0*cj0)))+(((-1.272)*py*r01*r02*(pz*pz)))+(((8.8e-7)*px*sj0*(py*py)*(r01*r01)))+(((0.039712)*px*pz*r00*r02*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(cj0*cj0)*(px*px)))+(((1.272)*cj0*px*r01*r02*sj0*(py*py)))+(((0.424)*px*r00*r02*(py*py)*(sj0*sj0)))+(((4.0)*px*pz*r00*r02*(py*py)))+(((2.0)*(py*py)*(pz*pz)*(r02*r02)))+(((-1.103872e-8)*cj0*pz*r01*r02))+(((0.424)*pz*(py*py)*(r02*r02)))+(((-5.51936e-9)*cj0*px*r00*r01))+(((-4.0)*(px*px)*(py*py)*(r01*r01)*(sj0*sj0)))+(((4.4e-7)*py*r00*r01*sj0*(pz*pz)))+(((-1.0)*(cj0*cj0)*(px*px*px*px)*(r01*r01)))+(((-1.0)*(py*py*py*py)*(r00*r00)*(sj0*sj0)))+(((0.039712)*px*py*r00*r01*(cj0*cj0)))+(((8.8e-7)*px*py*pz*r01*r02*sj0))+(((-9.328e-8)*cj0*py*pz*(r02*r02)))+(((-4.4e-7)*cj0*(py*py*py)*(r01*r01)))+(((0.005318656)*cj0*py*r00*r02*sj0))+(((-0.019856)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.0)*(px*px*px*px)*(r02*r02)))+(((-4.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r02*r02)))+(((12.0)*cj0*py*pz*r00*r02*sj0*(px*px)))+(((-0.0449440000000484)*(px*px)*(r00*r00)))+(((-0.848)*py*r01*r02*(px*px)*(sj0*sj0)))+(((-0.0898880000000968)*px*py*r00*r01))); +op[1]=((((1.696)*px*sj0*(py*py)*(r01*r01)))+(((1.696)*cj0*pz*r01*r02*(px*px)))+(((0.179775999999806)*px*py*r01*r02*sj0))+(((-8.8e-7)*r00*r02*(px*px*px)))+(((-8.8e-7)*pz*(px*px)*(r02*r02)))+(((-0.179775999999806)*cj0*px*pz*r00*r01))+(((-0.848)*cj0*py*(pz*pz)*(r01*r01)))+(((-0.848)*cj0*px*r00*r01*(pz*pz)))+(((-0.848)*cj0*(py*py*py)*(r01*r01)))+(((1.8656e-7)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.696)*cj0*px*py*pz*r00*r02))+(((-8.8e-7)*px*r00*r02*(py*py)))+(((-0.010637312)*cj0*py*(r01*r01)))+(((-8.8e-7)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-0.848)*px*sj0*(py*py)*(r00*r00)))+(((1.103872e-8)*px*r00*r02*(sj0*sj0)))+(((-2.64e-6)*cj0*py*r00*r02*sj0*(px*px)))+(((2.207744e-8)*cj0*pz*r00*r01*sj0))+(((-2.34020864e-9)*(r00*r00)*(sj0*sj0)))+(((-1.8656e-7)*(cj0*cj0)*(py*py)*(r01*r01)))+(((2.544)*py*r00*r01*sj0*(px*px)))+(((-1.76e-6)*px*py*pz*r00*r01*(cj0*cj0)))+(((0.848)*sj0*(px*px*px)*(r00*r00)))+(((0.00225511014399757)*r00*r02*sj0))+(((-0.179775999999806)*r00*r02*sj0*(py*py)))+(((1.8656e-7)*(cj0*cj0)*(px*px)*(r01*r01)))+(((1.76e-6)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((0.010637312)*py*r00*r01*sj0))+(((8.8e-7)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-3.7312e-7)*py*pz*r01*r02*(cj0*cj0)))+(((-0.179775999999806)*cj0*py*pz*(r01*r01)))+(((-1.696)*pz*r00*r02*sj0*(py*py)))+(((-2.64e-6)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((1.76e-6)*pz*(px*px)*(r00*r00)))+(((0.179775999999806)*px*pz*sj0*(r02*r02)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r01*r01)))+(((-3.7312e-7)*px*pz*r00*r02*(sj0*sj0)))+(((3.7312e-7)*cj0*px*py*sj0*(r00*r00)))+(((-1.103872e-8)*pz*(cj0*cj0)*(r01*r01)))+(((-3.52e-6)*cj0*px*py*pz*sj0*(r02*r02)))+(((3.7312e-7)*cj0*py*pz*r00*r02*sj0))+(((-1.8656e-7)*(px*px)*(r02*r02)))+(((8.8e-7)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((-0.848)*cj0*py*(px*px)*(r02*r02)))+(((0.848)*sj0*(px*px*px)*(r02*r02)))+(((1.8656e-7)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((3.7312e-7)*cj0*px*py*sj0*(r01*r01)))+(((-8.8e-7)*pz*(py*py)*(r02*r02)))+(((8.8e-7)*cj0*r01*r02*sj0*(px*px*px)))+(((-1.76e-6)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((-1.103872e-8)*pz*(r00*r00)*(sj0*sj0)))+(((1.103872e-8)*py*r01*r02))+(((-1.103872e-8)*cj0*px*r01*r02*sj0))+(((2.64e-6)*cj0*px*r01*r02*sj0*(pz*pz)))+(((-3.7312e-7)*cj0*r00*r01*sj0*(pz*pz)))+(((3.7312e-7)*cj0*px*pz*r01*r02*sj0))+(((2.64e-6)*py*r01*r02*(pz*pz)))+(((0.179775999999806)*cj0*r01*r02*(px*px)))+(((0.010637312)*px*sj0*(r00*r00)))+(((-2.34020864e-9)*(cj0*cj0)*(r01*r01)))+(((1.76e-6)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((8.8e-7)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((-0.00225511014399757)*cj0*r01*r02))+(((2.64e-6)*px*r00*r02*(pz*pz)))+(((1.76e-6)*pz*(py*py)*(r01*r01)))+(((0.848)*py*r00*r01*sj0*(pz*pz)))+(((2.64e-6)*cj0*py*r00*r02*sj0*(pz*pz)))+(((0.848)*px*sj0*(py*py)*(r02*r02)))+(((3.52e-6)*px*py*pz*r00*r01))+(((8.8e-7)*(pz*pz*pz)*(r02*r02)))+(((0.021274624)*pz*r00*r02*sj0))+(((-0.179775999999806)*cj0*px*py*r00*r02))+(((1.8656e-7)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((-2.64e-6)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((-0.848)*cj0*py*(pz*pz)*(r02*r02)))+(((-0.848)*cj0*(py*py*py)*(r02*r02)))+(((0.179775999999806)*px*pz*sj0*(r00*r00)))+(((1.8656e-7)*(pz*pz)*(r02*r02)))+(((1.696)*px*py*pz*r01*r02*sj0))+(((-1.696)*cj0*py*(px*px)*(r00*r00)))+(((-8.8e-7)*px*r00*r02*(py*py)*(sj0*sj0)))+(((-1.103872e-8)*cj0*py*r00*r02*sj0))+(((0.848)*cj0*py*(px*px)*(r01*r01)))+(((-1.76e-6)*px*py*pz*r00*r01*(sj0*sj0)))+(((1.103872e-8)*px*r00*r02))+(((-8.8e-7)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((4.68041728e-9)*cj0*r00*r01*sj0))+(((-0.021274624)*cj0*pz*r01*r02))+(((2.34020864e-9)*(r02*r02)))+(((8.8e-7)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.848)*r00*r01*sj0*(py*py*py)))+(((-8.8e-7)*py*r01*r02*(px*px)))+(((-2.64e-6)*cj0*px*r01*r02*sj0*(py*py)))+(((-3.7312e-7)*px*py*r00*r01*(cj0*cj0)))+(((3.7312e-7)*px*pz*r00*r02))+(((-1.8656e-7)*(py*py)*(r02*r02)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r00*r00)))+(((-8.8e-7)*r01*r02*(py*py*py)))+(((-0.010637312)*cj0*px*r00*r01))+(((0.179775999999806)*py*pz*r00*r01*sj0))+(((8.8e-7)*cj0*r00*r02*sj0*(py*py*py)))+(((0.848)*px*sj0*(pz*pz)*(r00*r00)))+(((1.76e-6)*py*r01*r02*(px*px)*(sj0*sj0)))+(((0.848)*px*sj0*(pz*pz)*(r02*r02)))+(((8.8e-7)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((1.103872e-8)*pz*(r02*r02)))+(((1.103872e-8)*py*r01*r02*(cj0*cj0)))+(((3.7312e-7)*py*pz*r01*r02))+(((0.010637312)*cj0*py*(r02*r02)))+(((-0.179775999999806)*cj0*py*pz*(r02*r02)))+(((-0.010637312)*px*sj0*(r02*r02)))+(((-3.7312e-7)*px*py*r00*r01*(sj0*sj0)))+(((-1.8656e-7)*(px*px)*(r00*r00)*(sj0*sj0)))+(((-2.544)*cj0*px*r00*r01*(py*py)))+(((0.848)*cj0*r00*r01*(px*px*px)))+(((-8.8e-7)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((1.76e-6)*px*r00*r02*(cj0*cj0)*(py*py)))+(((8.8e-7)*r01*r02*(cj0*cj0)*(py*py*py)))); +op[2]=((((-8.0)*cj0*px*py*sj0*(pz*pz)*(r01*r01)))+(((0.0794239999996128)*px*pz*r00*r02))+(((7.02062592e-9)*cj0*r01*r02))+(((-2.0)*(px*px*px*px)*(r00*r00)*(sj0*sj0)))+(((-8.0)*px*r00*r02*(pz*pz*pz)))+(((-2.0)*(pz*pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-2.0)*(cj0*cj0)*(pz*pz*pz*pz)*(r01*r01)))+(((-8.0)*cj0*pz*r01*r02*sj0*(px*px*px)))+(((8.0)*px*py*r00*r01*(pz*pz)*(sj0*sj0)))+(((0.00288451788799514)*cj0*r00*r01*sj0))+(((-2.0)*(cj0*cj0)*(px*px*px*px)*(r01*r01)))+(((-2.0)*(px*px*px*px)*(r02*r02)))+(((-0.140063999999806)*(px*px)*(r00*r00)*(sj0*sj0)))+(((0.280127999999613)*cj0*py*pz*r00*r02*sj0))+(((0.00194040627199879)*(r02*r02)))+(((5.5968e-7)*cj0*px*py*r00*r02))+(((4.0)*(px*px)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((8.0)*px*r00*r02*(pz*pz*pz)*(sj0*sj0)))+(((4.0)*(px*px)*(pz*pz)*(r02*r02)))+(((-2.0)*(py*py*py*py)*(r00*r00)*(sj0*sj0)))+(((24.0)*cj0*px*pz*r01*r02*sj0*(py*py)))+(((-0.00144225894399757)*(r00*r00)*(sj0*sj0)))+(((0.140063999999806)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-8.0)*(px*px)*(pz*pz)*(r00*r00)))+(((-8.0)*(px*px)*(pz*pz)*(r02*r02)*(sj0*sj0)))+(((8.0)*pz*r01*r02*(py*py*py)))+(((0.280127999999613)*cj0*px*py*sj0*(r00*r00)))+(((-4.0)*cj0*r00*r01*sj0*(py*py*py*py)))+(((-0.140063999999806)*(cj0*cj0)*(py*py)*(r01*r01)))+(((-8.0)*py*r01*r02*(pz*pz*pz)))+(((4.0)*(cj0*cj0)*(px*px)*(py*py)*(r01*r01)))+(((-2.0)*(pz*pz*pz*pz)*(r02*r02)))+(((-8.0)*px*r00*r01*(cj0*cj0)*(py*py*py)))+(((-8.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r02*r02)))+(((8.0)*px*pz*r00*r02*(py*py)*(sj0*sj0)))+(((24.0)*cj0*py*pz*r00*r02*sj0*(px*px)))+(((-5.5968e-7)*px*pz*sj0*(r02*r02)))+(((-8.0)*cj0*pz*r00*r02*sj0*(py*py*py)))+(((-5.5968e-7)*py*pz*r00*r01*sj0))+(((-8.0)*pz*r00*r02*(px*px*px)*(sj0*sj0)))+(((8.0)*py*r00*r01*(cj0*cj0)*(px*px*px)))+(((-0.129599999999903)*(py*py)*(r02*r02)))+(((8.0)*px*r00*r01*(py*py*py)*(sj0*sj0)))+(((-4.0)*(px*px)*(py*py)*(r02*r02)))+(((-16.0)*px*pz*r00*r02*(cj0*cj0)*(py*py)))+(((8.0)*cj0*px*sj0*(py*py*py)*(r01*r01)))+(((-0.129599999999903)*(px*px)*(r02*r02)))+(((-8.0)*(cj0*cj0)*(px*px)*(py*py)*(r00*r00)))+(((0.280127999999613)*cj0*px*pz*r01*r02*sj0))+(((0.140063999999806)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((4.0)*cj0*r00*r01*sj0*(pz*pz*pz*pz)))+(((-5.5968e-7)*px*pz*sj0*(r00*r00)))+(((8.0)*px*py*r00*r01*(cj0*cj0)*(pz*pz)))+(((0.0794239999996128)*py*pz*r01*r02))+(((-0.280127999999613)*px*py*r00*r01*(sj0*sj0)))+(((-4.0)*(cj0*cj0)*(px*px)*(pz*pz)*(r01*r01)))+(((-0.280127999999613)*px*pz*r00*r02*(sj0*sj0)))+(((24.0)*cj0*r00*r01*sj0*(px*px)*(py*py)))+(((4.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r01*r01)))+(((8.0)*cj0*py*sj0*(px*px*px)*(r00*r00)))+(((0.140063999999806)*(cj0*cj0)*(px*px)*(r01*r01)))+(((-0.280127999999613)*px*py*r00*r01*(cj0*cj0)))+(((-16.0)*py*pz*r01*r02*(px*px)*(sj0*sj0)))+(((8.0)*py*pz*r01*r02*(cj0*cj0)*(px*px)))+(((8.0)*py*r01*r02*(cj0*cj0)*(pz*pz*pz)))+(((0.280127999999613)*cj0*px*py*sj0*(r01*r01)))+(((-8.0)*pz*r01*r02*(cj0*cj0)*(py*py*py)))+(((16.0)*cj0*px*py*sj0*(pz*pz)*(r02*r02)))+(((-8.0)*py*r00*r01*(px*px*px)*(sj0*sj0)))+(((0.0397119999998064)*(pz*pz)*(r02*r02)))+(((0.140063999999806)*(py*py)*(r00*r00)*(sj0*sj0)))+(((8.0)*py*pz*r01*r02*(px*px)))+(((-16.0)*px*py*r00*r01*(pz*pz)))+(((5.5968e-7)*cj0*py*pz*(r01*r01)))+(((4.0)*(px*px)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-8.0)*cj0*px*py*sj0*(pz*pz)*(r00*r00)))+(((-0.280127999999613)*cj0*r00*r01*sj0*(pz*pz)))+(((-2.0)*(py*py*py*py)*(r02*r02)))+(((-5.5968e-7)*cj0*r01*r02*(px*px)))+(((-2.0)*(cj0*cj0)*(py*py*py*py)*(r01*r01)))+(((-8.0)*(px*px)*(py*py)*(r01*r01)*(sj0*sj0)))+(((5.5968e-7)*cj0*py*pz*(r02*r02)))+(((-8.0)*cj0*py*r00*r02*sj0*(pz*pz*pz)))+(((8.0)*pz*r00*r02*(px*px*px)))+(((-5.5968e-7)*px*py*r01*r02*sj0))+(((-4.0)*cj0*r00*r01*sj0*(px*px*px*px)))+(((8.0)*px*pz*r00*r02*(py*py)))+(((-0.0898880000000968)*(py*py)*(r01*r01)))+(((-8.0)*cj0*px*r01*r02*sj0*(pz*pz*pz)))+(((5.5968e-7)*cj0*px*pz*r00*r01))+(((-8.0)*(py*py)*(pz*pz)*(r01*r01)))+(((4.0)*(py*py)*(pz*pz)*(r02*r02)))+(((-0.179776000000194)*px*py*r00*r01))+(((-0.00144225894399757)*(cj0*cj0)*(r01*r01)))+(((-7.02062592e-9)*r00*r02*sj0))+(((-0.280127999999613)*py*pz*r01*r02*(cj0*cj0)))+(((-8.0)*cj0*px*sj0*(py*py*py)*(r00*r00)))+(((-8.0)*cj0*py*sj0*(px*px*px)*(r01*r01)))+(((5.5968e-7)*r00*r02*sj0*(py*py)))+(((-4.0)*(py*py)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.0898880000000968)*(px*px)*(r00*r00)))); +op[3]=((((1.696)*px*sj0*(py*py)*(r01*r01)))+(((1.696)*cj0*pz*r01*r02*(px*px)))+(((-3.7312e-7)*px*pz*r00*r02))+(((-2.34020864e-9)*(r02*r02)))+(((0.179775999999806)*r00*r02*sj0*(py*py)))+(((-3.7312e-7)*cj0*px*py*sj0*(r01*r01)))+(((-8.8e-7)*r00*r02*(px*px*px)))+(((-8.8e-7)*pz*(px*px)*(r02*r02)))+(((-0.179775999999806)*px*py*r01*r02*sj0))+(((-0.848)*cj0*py*(pz*pz)*(r01*r01)))+(((2.34020864e-9)*(r00*r00)*(sj0*sj0)))+(((-0.848)*cj0*px*r00*r01*(pz*pz)))+(((-3.7312e-7)*cj0*px*pz*r01*r02*sj0))+(((-0.848)*cj0*(py*py*py)*(r01*r01)))+(((-1.696)*cj0*px*py*pz*r00*r02))+(((-8.8e-7)*px*r00*r02*(py*py)))+(((-0.010637312)*cj0*py*(r01*r01)))+(((-8.8e-7)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-0.848)*px*sj0*(py*py)*(r00*r00)))+(((1.103872e-8)*px*r00*r02*(sj0*sj0)))+(((-2.64e-6)*cj0*py*r00*r02*sj0*(px*px)))+(((3.7312e-7)*px*py*r00*r01*(cj0*cj0)))+(((-1.8656e-7)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((2.207744e-8)*cj0*pz*r00*r01*sj0))+(((1.8656e-7)*(px*px)*(r00*r00)*(sj0*sj0)))+(((2.544)*py*r00*r01*sj0*(px*px)))+(((-4.68041728e-9)*cj0*r00*r01*sj0))+(((-0.00225511014399757)*r00*r02*sj0))+(((-1.76e-6)*px*py*pz*r00*r01*(cj0*cj0)))+(((0.848)*sj0*(px*px*px)*(r00*r00)))+(((3.7312e-7)*px*pz*r00*r02*(sj0*sj0)))+(((0.179775999999806)*cj0*py*pz*(r02*r02)))+(((1.76e-6)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((0.010637312)*py*r00*r01*sj0))+(((8.8e-7)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((2.34020864e-9)*(cj0*cj0)*(r01*r01)))+(((-1.8656e-7)*(cj0*cj0)*(px*px)*(r01*r01)))+(((-1.696)*pz*r00*r02*sj0*(py*py)))+(((-2.64e-6)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((3.7312e-7)*cj0*r00*r01*sj0*(pz*pz)))+(((-0.179775999999806)*py*pz*r00*r01*sj0))+(((1.76e-6)*pz*(px*px)*(r00*r00)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r01*r01)))+(((-1.103872e-8)*pz*(cj0*cj0)*(r01*r01)))+(((-3.52e-6)*cj0*px*py*pz*sj0*(r02*r02)))+(((8.8e-7)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((-0.848)*cj0*py*(px*px)*(r02*r02)))+(((0.179775999999806)*cj0*px*pz*r00*r01))+(((0.848)*sj0*(px*px*px)*(r02*r02)))+(((1.8656e-7)*(py*py)*(r02*r02)))+(((-0.179775999999806)*cj0*r01*r02*(px*px)))+(((-8.8e-7)*pz*(py*py)*(r02*r02)))+(((8.8e-7)*cj0*r01*r02*sj0*(px*px*px)))+(((-1.76e-6)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((-1.8656e-7)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.103872e-8)*pz*(r00*r00)*(sj0*sj0)))+(((1.103872e-8)*py*r01*r02))+(((-1.103872e-8)*cj0*px*r01*r02*sj0))+(((2.64e-6)*cj0*px*r01*r02*sj0*(pz*pz)))+(((2.64e-6)*py*r01*r02*(pz*pz)))+(((0.010637312)*px*sj0*(r00*r00)))+(((1.76e-6)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((-1.8656e-7)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((8.8e-7)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((2.64e-6)*px*r00*r02*(pz*pz)))+(((1.76e-6)*pz*(py*py)*(r01*r01)))+(((0.848)*py*r00*r01*sj0*(pz*pz)))+(((-3.7312e-7)*cj0*px*py*sj0*(r00*r00)))+(((-0.179775999999806)*px*pz*sj0*(r02*r02)))+(((2.64e-6)*cj0*py*r00*r02*sj0*(pz*pz)))+(((0.848)*px*sj0*(py*py)*(r02*r02)))+(((3.52e-6)*px*py*pz*r00*r01))+(((8.8e-7)*(pz*pz*pz)*(r02*r02)))+(((0.021274624)*pz*r00*r02*sj0))+(((1.8656e-7)*(cj0*cj0)*(py*py)*(r01*r01)))+(((-2.64e-6)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((-0.848)*cj0*py*(pz*pz)*(r02*r02)))+(((-0.848)*cj0*(py*py*py)*(r02*r02)))+(((1.696)*px*py*pz*r01*r02*sj0))+(((-1.696)*cj0*py*(px*px)*(r00*r00)))+(((3.7312e-7)*px*py*r00*r01*(sj0*sj0)))+(((-8.8e-7)*px*r00*r02*(py*py)*(sj0*sj0)))+(((-1.103872e-8)*cj0*py*r00*r02*sj0))+(((-3.7312e-7)*cj0*py*pz*r00*r02*sj0))+(((0.848)*cj0*py*(px*px)*(r01*r01)))+(((-1.76e-6)*px*py*pz*r00*r01*(sj0*sj0)))+(((1.103872e-8)*px*r00*r02))+(((1.8656e-7)*(px*px)*(r02*r02)))+(((-8.8e-7)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((-0.021274624)*cj0*pz*r01*r02))+(((8.8e-7)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.848)*r00*r01*sj0*(py*py*py)))+(((-8.8e-7)*py*r01*r02*(px*px)))+(((-1.8656e-7)*(pz*pz)*(r02*r02)))+(((-2.64e-6)*cj0*px*r01*r02*sj0*(py*py)))+(((-0.179775999999806)*px*pz*sj0*(r00*r00)))+(((0.179775999999806)*cj0*py*pz*(r01*r01)))+(((1.76e-6)*cj0*px*py*pz*sj0*(r00*r00)))+(((-8.8e-7)*r01*r02*(py*py*py)))+(((-0.010637312)*cj0*px*r00*r01))+(((8.8e-7)*cj0*r00*r02*sj0*(py*py*py)))+(((3.7312e-7)*py*pz*r01*r02*(cj0*cj0)))+(((0.00225511014399757)*cj0*r01*r02))+(((0.848)*px*sj0*(pz*pz)*(r00*r00)))+(((0.179775999999806)*cj0*px*py*r00*r02))+(((1.76e-6)*py*r01*r02*(px*px)*(sj0*sj0)))+(((0.848)*px*sj0*(pz*pz)*(r02*r02)))+(((8.8e-7)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((-3.7312e-7)*py*pz*r01*r02))+(((1.103872e-8)*pz*(r02*r02)))+(((1.103872e-8)*py*r01*r02*(cj0*cj0)))+(((0.010637312)*cj0*py*(r02*r02)))+(((-0.010637312)*px*sj0*(r02*r02)))+(((-2.544)*cj0*px*r00*r01*(py*py)))+(((0.848)*cj0*r00*r01*(px*px*px)))+(((-8.8e-7)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((1.76e-6)*px*r00*r02*(cj0*cj0)*(py*py)))+(((8.8e-7)*r01*r02*(cj0*cj0)*(py*py*py)))); +op[4]=((((1.32e-6)*cj0*px*r00*r01*(py*py)))+(((-1.0)*(cj0*cj0)*(py*py*py*py)*(r01*r01)))+(((4.4e-7)*cj0*px*r00*r01*(pz*pz)))+(((8.8e-7)*cj0*py*(px*px)*(r00*r00)))+(((1.17010432e-9)*r00*r02*sj0))+(((-8.0)*px*pz*r00*r02*(cj0*cj0)*(py*py)))+(((2.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r01*r01)))+(((-4.0)*cj0*pz*r01*r02*sj0*(px*px*px)))+(((0.039712)*cj0*r00*r01*sj0*(pz*pz)))+(((2.0)*cj0*r00*r01*sj0*(pz*pz*pz*pz)))+(((4.0)*py*r00*r01*(cj0*cj0)*(px*px*px)))+(((8.0)*cj0*px*py*sj0*(pz*pz)*(r02*r02)))+(((-0.140064)*py*pz*r01*r02))+(((-8.0)*py*pz*r01*r02*(px*px)*(sj0*sj0)))+(((4.0)*cj0*py*sj0*(px*px*px)*(r00*r00)))+(((-1.272)*cj0*px*r01*r02*sj0*(py*py)))+(((-4.0)*(px*px)*(pz*pz)*(r02*r02)*(sj0*sj0)))+(((-4.0)*(px*px)*(pz*pz)*(r00*r00)))+(((-0.848)*px*py*pz*r00*r01*(sj0*sj0)))+(((-4.4e-7)*cj0*py*(px*px)*(r01*r01)))+(((-4.4e-7)*py*r00*r01*sj0*(pz*pz)))+(((-4.0)*px*r00*r01*(cj0*cj0)*(py*py*py)))+(((0.424)*cj0*r01*r02*sj0*(px*px*px)))+(((-0.424)*py*r01*r02*(cj0*cj0)*(px*px)))+(((-0.039712)*cj0*py*pz*r00*r02*sj0))+(((2.0)*(px*px)*(py*py)*(r00*r00)*(sj0*sj0)))+(((-1.0)*(px*px*px*px)*(r00*r00)*(sj0*sj0)))+(((1.272)*cj0*px*r01*r02*sj0*(pz*pz)))+(((-1.0)*(cj0*cj0)*(pz*pz*pz*pz)*(r01*r01)))+(((0.848)*py*r01*r02*(px*px)*(sj0*sj0)))+(((-4.0)*cj0*px*r01*r02*sj0*(pz*pz*pz)))+(((-1.272)*px*r00*r02*(pz*pz)*(sj0*sj0)))+(((-4.0)*py*r00*r01*(px*px*px)*(sj0*sj0)))+(((-1.32e-6)*py*r00*r01*sj0*(px*px)))+(((4.0)*px*py*r00*r01*(cj0*cj0)*(pz*pz)))+(((0.0250879999999516)*(px*px)*(r02*r02)))+(((-0.019856)*(cj0*cj0)*(px*px)*(r01*r01)))+(((4.4e-7)*cj0*py*(pz*pz)*(r02*r02)))+(((12.0)*cj0*px*pz*r01*r02*sj0*(py*py)))+(((-4.0)*cj0*py*r00*r02*sj0*(pz*pz*pz)))+(((-0.019856)*(cj0*cj0)*(pz*pz)*(r01*r01)))+(((0.424)*cj0*r00*r02*sj0*(py*py*py)))+(((-4.4e-7)*px*sj0*(pz*pz)*(r02*r02)))+(((-2.0)*(px*px)*(py*py)*(r02*r02)))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r01*r01)))+(((-8.0)*px*py*r00*r01*(pz*pz)))+(((0.848)*cj0*px*py*pz*sj0*(r01*r01)))+(((2.0)*(cj0*cj0)*(px*px)*(py*py)*(r01*r01)))+(((-0.039712)*cj0*px*py*sj0*(r01*r01)))+(((-0.424)*py*r01*r02*(px*px)))+(((0.424)*(pz*pz*pz)*(r02*r02)))+(((-5.51936e-9)*cj0*py*(r02*r02)))+(((0.424)*(pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.000157351935999393)*(r02*r02)))+(((0.019856)*(cj0*cj0)*(py*py)*(r01*r01)))+(((0.424)*(cj0*cj0)*(pz*pz*pz)*(r01*r01)))+(((-0.019856)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((0.424)*r00*r02*(px*px*px)*(sj0*sj0)))+(((-0.005318656)*pz*(r00*r00)*(sj0*sj0)))+(((-4.0)*cj0*px*sj0*(py*py*py)*(r00*r00)))+(((-2.0)*cj0*r00*r01*sj0*(py*py*py*py)))+(((2.0)*(px*px)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.039712)*cj0*px*py*sj0*(r00*r00)))+(((0.039712)*py*pz*r01*r02*(cj0*cj0)))+(((-9.328e-8)*cj0*px*py*r00*r02))+(((-1.272)*cj0*py*r00*r02*sj0*(px*px)))+(((4.4e-7)*px*sj0*(py*py)*(r00*r00)))+(((-0.039712)*cj0*px*pz*r01*r02*sj0))+(((4.0)*pz*r01*r02*(py*py*py)))+(((-0.005318656)*cj0*px*r01*r02*sj0))+(((0.010637312)*cj0*pz*r00*r01*sj0))+(((0.848)*cj0*px*py*pz*sj0*(r00*r00)))+(((5.51936e-9)*px*sj0*(r02*r02)))+(((-0.848)*px*py*pz*r00*r01*(cj0*cj0)))+(((1.103872e-8)*cj0*pz*r01*r02))+(((-0.424)*pz*(px*px)*(r02*r02)))+(((0.424)*pz*(cj0*cj0)*(px*px)*(r01*r01)))+(((-4.4e-7)*cj0*r00*r01*(px*px*px)))+(((-4.0)*py*r01*r02*(pz*pz*pz)))+(((-4.0)*cj0*pz*r00*r02*sj0*(py*py*py)))+(((-0.424)*px*r00*r02*(py*py)))+(((-4.0)*cj0*py*sj0*(px*px*px)*(r01*r01)))+(((-1.17010432e-9)*cj0*r01*r02))+(((4.0)*px*r00*r01*(py*py*py)*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(px*px)))+(((9.328e-8)*py*pz*r00*r01*sj0))+(((1.272)*py*r01*r02*(pz*pz)))+(((5.51936e-9)*cj0*py*(r01*r01)))+(((4.4e-7)*cj0*py*(pz*pz)*(r01*r01)))+(((-9.328e-8)*r00*r02*sj0*(py*py)))+(((0.005318656)*px*r00*r02*(sj0*sj0)))+(((-0.424)*r01*r02*(py*py*py)))+(((9.328e-8)*px*pz*sj0*(r00*r00)))+(((4.0)*cj0*px*sj0*(py*py*py)*(r01*r01)))+(((4.0)*pz*r00*r02*(px*px*px)))+(((0.005318656)*px*r00*r02))+(((4.4e-7)*cj0*(py*py*py)*(r02*r02)))+(((0.0250879999999516)*(py*py)*(r02*r02)))+(((0.424)*pz*(py*py)*(r00*r00)*(sj0*sj0)))+(((1.272)*cj0*py*r00*r02*sj0*(pz*pz)))+(((-1.103872e-8)*pz*r00*r02*sj0))+(((-4.0)*pz*r01*r02*(cj0*cj0)*(py*py*py)))+(((0.005318656)*pz*(r02*r02)))+(((-0.005318656)*pz*(cj0*cj0)*(r01*r01)))+(((-0.140064)*px*pz*r00*r02))+(((0.005318656)*py*r01*r02))+(((-4.0)*cj0*px*py*sj0*(pz*pz)*(r00*r00)))+(((-4.0)*pz*r00*r02*(px*px*px)*(sj0*sj0)))+(((-2.0)*(py*py)*(pz*pz)*(r00*r00)*(sj0*sj0)))+(((-0.424)*px*r00*r02*(py*py)*(sj0*sj0)))+(((-9.328e-8)*cj0*py*pz*(r01*r01)))+(((2.0)*(px*px)*(pz*pz)*(r02*r02)))+(((-0.848)*cj0*r00*r01*sj0*(pz*pz*pz)))+(((-0.0008128512)*cj0*r00*r01*sj0))+(((-1.0)*(py*py*py*py)*(r02*r02)))+(((-0.005318656)*cj0*py*r00*r02*sj0))+(((0.019856)*(px*px)*(r00*r00)*(sj0*sj0)))+(((-2.0)*(cj0*cj0)*(px*px)*(pz*pz)*(r01*r01)))+(((4.0)*px*r00*r02*(pz*pz*pz)*(sj0*sj0)))+(((-4.4e-7)*px*sj0*(py*py)*(r02*r02)))+(((-1.696)*cj0*px*py*pz*sj0*(r02*r02)))+(((-0.070032)*(pz*pz)*(r02*r02)))+(((-8.8e-7)*px*sj0*(py*py)*(r01*r01)))+(((8.8e-7)*cj0*px*py*pz*r00*r02))+(((-2.0)*cj0*r00*r01*sj0*(px*px*px*px)))+(((0.0004064256)*(cj0*cj0)*(r01*r01)))+(((-0.424)*r00*r02*(px*px*px)))+(((-1.0)*(pz*pz*pz*pz)*(r00*r00)*(sj0*sj0)))+(((4.0)*px*pz*r00*r02*(py*py)*(sj0*sj0)))+(((-0.424)*pz*(px*px)*(r00*r00)*(sj0*sj0)))+(((9.328e-8)*px*pz*sj0*(r02*r02)))+(((0.848)*pz*(py*py)*(r01*r01)))+(((-8.8e-7)*cj0*pz*r01*r02*(px*px)))+(((0.039712)*px*py*r00*r01*(sj0*sj0)))+(((0.424)*r01*r02*(cj0*cj0)*(py*py*py)))+(((-4.4e-7)*sj0*(px*px*px)*(r02*r02)))+(((4.0)*py*r01*r02*(cj0*cj0)*(pz*pz*pz)))+(((9.328e-8)*px*py*r01*r02*sj0))+(((-4.0)*(cj0*cj0)*(px*px)*(py*py)*(r00*r00)))+(((9.328e-8)*cj0*r01*r02*(px*px)))+(((-4.0)*(py*py)*(pz*pz)*(r01*r01)))+(((-1.0)*(pz*pz*pz*pz)*(r02*r02)))+(((-4.4e-7)*sj0*(px*px*px)*(r00*r00)))+(((12.0)*cj0*r00*r01*sj0*(px*px)*(py*py)))+(((-4.0)*px*r00*r02*(pz*pz*pz)))+(((0.0004064256)*(r00*r00)*(sj0*sj0)))+(((-9.328e-8)*cj0*px*pz*r00*r01))+(((0.848)*px*r00*r02*(cj0*cj0)*(py*py)))+(((-5.51936e-9)*py*r00*r01*sj0))+(((-5.51936e-9)*px*sj0*(r00*r00)))+(((-4.4e-7)*px*sj0*(pz*pz)*(r00*r00)))+(((-0.0449440000000484)*(py*py)*(r01*r01)))+(((-0.424)*pz*(cj0*cj0)*(py*py)*(r01*r01)))+(((0.848)*pz*(px*px)*(r02*r02)*(sj0*sj0)))+(((8.8e-7)*pz*r00*r02*sj0*(py*py)))+(((4.0)*px*py*r00*r01*(pz*pz)*(sj0*sj0)))+(((-8.8e-7)*px*py*pz*r01*r02*sj0))+(((0.039712)*px*pz*r00*r02*(sj0*sj0)))+(((4.0)*py*pz*r01*r02*(cj0*cj0)*(px*px)))+(((4.0)*px*pz*r00*r02*(py*py)))+(((2.0)*(py*py)*(pz*pz)*(r02*r02)))+(((0.848)*pz*(px*px)*(r00*r00)))+(((1.696)*px*py*pz*r00*r01))+(((0.005318656)*py*r01*r02*(cj0*cj0)))+(((0.848)*pz*(cj0*cj0)*(py*py)*(r02*r02)))+(((1.272)*px*r00*r02*(pz*pz)))+(((-1.272)*py*r01*r02*(cj0*cj0)*(pz*pz)))+(((-4.0)*(px*px)*(py*py)*(r01*r01)*(sj0*sj0)))+(((-1.0)*(cj0*cj0)*(px*px*px*px)*(r01*r01)))+(((-1.0)*(py*py*py*py)*(r00*r00)*(sj0*sj0)))+(((4.4e-7)*cj0*py*(px*px)*(r02*r02)))+(((0.039712)*px*py*r00*r01*(cj0*cj0)))+(((-9.328e-8)*cj0*py*pz*(r02*r02)))+(((-0.424)*pz*(py*py)*(r02*r02)))+(((-0.019856)*(py*py)*(r00*r00)*(sj0*sj0)))+(((5.51936e-9)*cj0*px*r00*r01))+(((-1.0)*(px*px*px*px)*(r02*r02)))+(((-4.0)*(cj0*cj0)*(py*py)*(pz*pz)*(r02*r02)))+(((12.0)*cj0*py*pz*r00*r02*sj0*(px*px)))+(((4.4e-7)*cj0*(py*py*py)*(r01*r01)))+(((4.4e-7)*r00*r01*sj0*(py*py*py)))+(((-0.0449440000000484)*(px*px)*(r00*r00)))+(((-0.0898880000000968)*px*py*r00*r01))); polyroots4(op,zeror,numroots); IkReal j1array[4], cj1array[4], sj1array[4], tempj1array[1]; int numsolutions = 0; for(int ij1 = 0; ij1 < numroots; ++ij1) { IkReal htj1 = zeror[ij1]; -tempj1array[0]=((IkReal(2.00000000000000))*(atan(htj1))); +tempj1array[0]=((2.0)*(atan(htj1))); for(int kj1 = 0; kj1 < 1; ++kj1) { j1array[numsolutions] = tempj1array[kj1]; @@ -389,7 +478,7 @@ if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRES IkReal j5array[2], cj5array[2], sj5array[2]; bool j5valid[2]={false}; _nj5 = 2; -sj5array[0]=((((IkReal(-0.999999985550000))*(cj0)*(r00)))+(((IkReal(0.000169999997543500))*(cj0)*(r01)))+(((IkReal(-0.999999985550000))*(r01)*(sj0)))+(((IkReal(-0.000169999997543500))*(r00)*(sj0)))); +sj5array[0]=((((-1.0)*r01*sj0))+(((-1.0)*cj0*r00))); if( sj5array[0] >= -1-IKFAST_SINCOS_THRESH && sj5array[0] <= 1+IKFAST_SINCOS_THRESH ) { j5valid[0] = j5valid[1] = true; @@ -399,7 +488,7 @@ if( sj5array[0] >= -1-IKFAST_SINCOS_THRESH && sj5array[0] <= 1+IKFAST_SINCOS_THR j5array[1] = j5array[0] > 0 ? (IKPI-j5array[0]) : (-IKPI-j5array[0]); cj5array[1] = -cj5array[0]; } -else if( std::isnan(sj5array[0]) ) +else if( isnan(sj5array[0]) ) { // probably any value will work j5valid[0] = true; @@ -422,100 +511,53 @@ if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRES j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; { -IkReal dummyeval[1]; -IkReal gconst0; -gconst0=IKsign(cj5); -dummyeval[0]=cj5; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=cj5; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=cj5; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[9]; -IkReal x43=(py)*(py); -IkReal x44=(pz)*(pz); -IkReal x45=(px)*(px); -IkReal x46=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x47=((cj0)*(r00)); -IkReal x48=((r01)*(sj0)); -IkReal x49=((IkReal(0.000169999997543500))*(sj1)); -IkReal x50=((IkReal(0.000339999995087000))*(py)); -IkReal x51=((pz)*(sj1)); -IkReal x52=((r02)*(sj0)); -IkReal x53=((IkReal(0.000169999997543500))*(cj0)); -IkReal x54=((py)*(r01)); -IkReal x55=((IkReal(0.000169999997543500))*(pz)); -IkReal x56=((r00)*(sj0)); -IkReal x57=((px)*(r00)); -IkReal x58=((IkReal(0.000169999997543500))*(cj1)); -IkReal x59=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x60=((cj0)*(r01)); -IkReal x61=((IkReal(0.999999985550000))*(px)); -IkReal x62=((IkReal(0.105999998468300))*(sj1)); -IkReal x63=((cj1)*(r02)); -IkReal x64=((cj0)*(r02)); -IkReal x65=((IkReal(0.000339999995087000))*(px)); -IkReal x66=((IkReal(0.999999985550000))*(py)); -IkReal x67=((IkReal(0.999999985550000))*(sj1)); -IkReal x68=((IkReal(0.999999985550000))*(cj1)); -IkReal x69=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x70=((px)*(sj1)); -IkReal x71=((IkReal(0.0112359998376398))*(sj1)); -IkReal x72=((cj1)*(px)); -IkReal x73=((cj0)*(pz)); -IkReal x74=((r02)*(sj1)); -IkReal x75=((IkReal(1.99999997110000))*(py)); -IkReal x76=((pz)*(r02)); -IkReal x77=((IkReal(0.999999985550000))*(pz)); -IkReal x78=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x79=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x80=((cj1)*(pz)); -IkReal x81=((IkReal(0.211999996936600))*(pz)); -IkReal x82=((cj1)*(x60)); -IkReal x83=((px)*(x75)); -IkReal x84=((IkReal(2.00000000000000))*(x44)); -IkReal x85=((x44)*(x67)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(cj0)*(x61)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((IkReal(-1.00000000000000))*(sj0)*(x66)))+(((py)*(x53)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-0.000169999997543500))*(x56)))+(((r01)*(x53)))+(((IkReal(-0.999999985550000))*(x48)))+(((IkReal(-0.999999985550000))*(x47)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x48)*(x69)))+(((IkReal(-1.00000000000000))*(x47)*(x69)))+(((IkReal(0.106000000000000))*(x63)))+(((x56)*(x62)))+(((IkReal(-1.00000000000000))*(x76)))+(((IkReal(-1.00000000000000))*(x60)*(x62)))+(((IkReal(-1.00000000000000))*(x57)))+(((IkReal(-1.00000000000000))*(x54)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x56)*(x68)))+(((x60)*(x68)))+(x74)+(((x48)*(x58)))+(((x47)*(x58)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x48)*(x49)))+(x63)+(((x56)*(x67)))+(((IkReal(-1.00000000000000))*(x60)*(x67)))+(((IkReal(-1.00000000000000))*(x47)*(x49)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x52)*(x61)))+(((IkReal(0.106000000000000))*(x74)))+(((IkReal(0.000169999997543500))*(py)*(x52)))+(((IkReal(-1.00000000000000))*(x48)*(x55)))+(((x48)*(x78)))+(((IkReal(-1.00000000000000))*(x60)*(x77)))+(((x47)*(x78)))+(((x56)*(x77)))+(((IkReal(-0.105999998468300))*(cj1)*(x56)))+(((IkReal(-1.00000000000000))*(x47)*(x55)))+(((px)*(r02)*(x53)))+(((IkReal(0.105999998468300))*(x82)))+(((x64)*(x66)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x48)*(x70)*(x75)))+(((IkReal(-0.0112360000000000))*(x63)))+(((pp)*(x63)))+(((x51)*(x64)*(x75)))+(((IkReal(-1.00000000000000))*(x63)*(x84)))+(((x43)*(x60)*(x67)))+(((x48)*(x79)))+(((x51)*(x64)*(x65)))+(((IkReal(0.212000000000000))*(x54)))+(((IkReal(0.212000000000000))*(x57)))+(((IkReal(-2.00000000000000))*(x54)*(x80)))+(((x47)*(x79)))+(((IkReal(-1.00000000000000))*(x45)*(x60)*(x67)))+(((x56)*(x85)))+(((x60)*(x71)))+(((x43)*(x56)*(x67)))+(((IkReal(-1.00000000000000))*(x44)*(x47)*(x49)))+(((IkReal(-1.00000000000000))*(x44)*(x48)*(x49)))+(((x50)*(x60)*(x70)))+(((IkReal(-1.99999997110000))*(px)*(x51)*(x52)))+(((x43)*(x48)*(x49)))+(((IkReal(-1.00000000000000))*(x45)*(x48)*(x49)))+(((IkReal(-1.00000000000000))*(x60)*(x85)))+(((x50)*(x56)*(x70)))+(((x45)*(x47)*(x49)))+(((IkReal(0.212000000000000))*(x76)))+(((x47)*(x70)*(x75)))+(((IkReal(-1.00000000000000))*(x56)*(x71)))+(((IkReal(-1.00000000000000))*(x45)*(x56)*(x67)))+(((IkReal(-2.00000000000000))*(x57)*(x80)))+(((IkReal(-1.00000000000000))*(x43)*(x47)*(x49)))+(((x50)*(x51)*(x52)))); -evalcond[8]=((((x43)*(x47)*(x58)))+(((IkReal(-0.0112359998376398))*(cj1)*(x56)))+(((x44)*(x48)*(x58)))+(((IkReal(-1.00000000000000))*(x50)*(x60)*(x72)))+(((IkReal(1.99999997110000))*(pz)*(x52)*(x72)))+(((x48)*(x72)*(x75)))+(((IkReal(-1.00000000000000))*(x47)*(x72)*(x75)))+(((x44)*(x47)*(x58)))+(((IkReal(-1.00000000000000))*(x50)*(x56)*(x72)))+(((x44)*(x60)*(x68)))+(((IkReal(-0.211999996936600))*(px)*(x52)))+(((IkReal(0.0112359998376398))*(x82)))+(((x56)*(x81)))+(((IkReal(0.211999996936600))*(py)*(x64)))+(((IkReal(-1.00000000000000))*(x74)*(x84)))+(((IkReal(-1.00000000000000))*(x63)*(x65)*(x73)))+(((x48)*(x59)))+(((IkReal(-1.00000000000000))*(x43)*(x48)*(x58)))+(((IkReal(-1.00000000000000))*(x60)*(x81)))+(((IkReal(-1.00000000000000))*(x63)*(x73)*(x75)))+(((IkReal(-1.00000000000000))*(x43)*(x56)*(x68)))+(((IkReal(3.60399994792220e-5))*(px)*(x64)))+(((IkReal(-1.00000000000000))*(x43)*(x60)*(x68)))+(((IkReal(-2.00000000000000))*(x51)*(x54)))+(((IkReal(-2.00000000000000))*(x51)*(x57)))+(((pp)*(x74)))+(((x47)*(x59)))+(((x45)*(x48)*(x58)))+(((IkReal(-1.00000000000000))*(x44)*(x56)*(x68)))+(((IkReal(0.0112360000000000))*(x74)))+(((IkReal(-1.00000000000000))*(x45)*(x47)*(x58)))+(((IkReal(-1.00000000000000))*(x50)*(x52)*(x80)))+(((IkReal(-1.00000000000000))*(x46)*(x48)))+(((IkReal(-1.00000000000000))*(x46)*(x47)))+(((IkReal(3.60399994792220e-5))*(py)*(x52)))+(((x45)*(x56)*(x68)))+(((x45)*(x60)*(x68)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 ) +IkReal j3eval[2]; +j3eval[0]=cj5; +j3eval[1]=IKsign(cj5); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) { +bgotonextstatement=false; { IkReal j3array[2], cj3array[2], sj3array[2]; bool j3valid[2]={false}; _nj3 = 2; -IkReal x86=((IkReal(8.92857129955357))*(sj1)); -IkReal x87=((IkReal(0.00151785712092411))*(sj1)); -cj3array[0]=((IkReal(-0.528301886792453))+(((IkReal(-8.92857142857143))*(cj1)*(pz)))+(((cj0)*(py)*(x86)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x86)))+(((cj0)*(px)*(x87)))+(((py)*(sj0)*(x87)))); -if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +IkReal x30=((8.92857142856662)*sj1); +IkReal x31=(px*sj0); +IkReal x32=(cj0*py); +IkReal x33=((9.26549865228612e-6)*cj1); +if( (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x30*x31))+(((-1.0)*x32*x33))+(((42.1159029649369)*(pz*pz)))+((x30*x32))+((x31*x33)))) < -1-IKFAST_SINCOS_THRESH || (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x30*x31))+(((-1.0)*x32*x33))+(((42.1159029649369)*(pz*pz)))+((x30*x32))+((x31*x33)))) > 1+IKFAST_SINCOS_THRESH ) + continue; +IkReal x34=((-1.0)*(IKasin(((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x30*x31))+(((-1.0)*x32*x33))+(((42.1159029649369)*(pz*pz)))+((x30*x32))+((x31*x33)))))); +j3array[0]=((-1.57079736453075)+(((-1.0)*x34))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((1.57079528905905)+x34); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) { - j3valid[0] = j3valid[1] = true; - j3array[0] = IKacos(cj3array[0]); - sj3array[0] = IKsin(j3array[0]); - cj3array[1] = cj3array[0]; - j3array[1] = -j3array[0]; - sj3array[1] = -sj3array[0]; + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; } -else if( std::isnan(cj3array[0]) ) +j3valid[0] = true; +if( j3array[1] > IKPI ) { - // probably any value will work - j3valid[0] = true; - cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; + j3array[1]-=IK2PI; } +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; for(int ij3 = 0; ij3 < 2; ++ij3) { if( !j3valid[ij3] ) @@ -533,48 +575,40 @@ if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRES j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; { IkReal evalcond[1]; -IkReal x88=(px)*(px); -IkReal x89=(py)*(py); -IkReal x90=(pz)*(pz); -IkReal x91=((r00)*(sj0)); -IkReal x92=((cj0)*(r00)); -IkReal x93=((IkReal(0.212000000000000))*(sj1)); -IkReal x94=((cj0)*(r01)); -IkReal x95=((r01)*(sj0)); -IkReal x96=((px)*(py)); -IkReal x97=((IkReal(0.211999996936600))*(cj1)); -IkReal x98=((r02)*(sj0)); -IkReal x99=((IkReal(3.60399994792220e-5))*(cj1)); -IkReal x100=((IkReal(0.999999985550000))*(x89)); -IkReal x101=((IkReal(0.999999985550000))*(x90)); -IkReal x102=((IkReal(0.000169999997543500))*(x88)); -IkReal x103=((cj0)*(cj1)*(r02)); -IkReal x104=((IkReal(0.000169999997543500))*(x89)); -IkReal x105=((IkReal(0.999999985550000))*(x88)); -IkReal x106=((cj0)*(pz)*(r02)); -IkReal x107=((IkReal(0.000169999997543500))*(x90)); -evalcond[0]=((IkReal(-0.0237800000000000))+(((IkReal(-1.00000000000000))*(py)*(x97)*(x98)))+(((IkReal(-1.00000000000000))*(pz)*(x94)*(x99)))+(((IkReal(-1.00000000000000))*(x107)*(x91)))+(((IkReal(-0.000339999995087000))*(py)*(x106)))+(((x100)*(x95)))+(((x107)*(x94)))+(((x102)*(x91)))+(((x102)*(x94)))+(((pz)*(x92)*(x97)))+(((IkReal(-1.91011997239877e-6))*(x91)))+(((pz)*(x91)*(x99)))+(((px)*(r01)*(x93)))+(((IkReal(-0.000339999995087000))*(x92)*(x96)))+(((IkReal(-1.00000000000000))*(x105)*(x95)))+(((IkReal(-1.00000000000000))*(px)*(x98)*(x99)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(r02)*(x97)))+(((IkReal(0.000339999995087000))*(x95)*(x96)))+(((IkReal(-0.0237440000000000))*(IKcos(j3))))+(((IkReal(1.99999997110000))*(x91)*(x96)))+(((IkReal(-1.00000000000000))*(x101)*(x92)))+(((IkReal(-1.00000000000000))*(x101)*(x95)))+(((IkReal(1.91011997239877e-6))*(x94)))+(((IkReal(-1.00000000000000))*(x100)*(x92)))+(((IkReal(1.99999997110000))*(py)*(pz)*(x98)))+(((IkReal(1.99999997110000))*(px)*(x106)))+(((pz)*(x95)*(x97)))+(((IkReal(0.000339999995087000))*(px)*(pz)*(x98)))+(((IkReal(-0.0112359998376398))*(x92)))+(((IkReal(-0.0112359998376398))*(x95)))+(((cj0)*(py)*(r02)*(x99)))+(((x105)*(x92)))+(((IkReal(-1.00000000000000))*(x104)*(x91)))+(((IkReal(-1.00000000000000))*(x104)*(x94)))+(((IkReal(1.99999997110000))*(x94)*(x96)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x93)))); -if( IKabs(evalcond[0]) > 0.000001 ) +IkReal x35=px*px; +IkReal x36=py*py; +IkReal x37=pz*pz; +IkReal x38=((0.212)*py); +IkReal x39=(r00*sj1); +IkReal x40=(cj0*r00); +IkReal x41=(px*r01); +IkReal x42=((2.2e-7)*cj1); +IkReal x43=(cj1*r02); +IkReal x44=((2.0)*px); +IkReal x45=(pz*r02); +IkReal x46=(py*sj0); +IkReal x47=(r01*sj0); +IkReal x48=((2.2e-7)*sj1); +IkReal x49=(cj0*px); +IkReal x50=((1.0)*x40); +IkReal x51=((0.212)*cj1*pz); +evalcond[0]=((-0.0237800000000121)+(((2.0)*x45*x46))+(((2.0)*cj0*py*x41))+(((-1.0)*sj0*x38*x43))+(((-0.212)*x43*x49))+(((-1.0)*x38*x39))+((x35*x40))+((cj0*x44*x45))+(((2.464e-8)*(IKsin(j3))))+((x36*x47))+(((0.212)*sj1*x41))+((py*r00*x42))+(((-1.0)*x41*x42))+(((-1.0)*x37*x50))+(((-1.0)*x37*x47))+(((-0.023744)*(IKcos(j3))))+((pz*x47*x48))+(((-1.0)*r02*x48*x49))+((x40*x51))+(((-1.0)*x36*x50))+((x47*x51))+((r00*x44*x46))+(((-0.0112360000000121)*x47))+(((-0.0112360000000121)*x40))+(((-1.0)*r02*x46*x48))+(((2.2e-7)*cj0*pz*x39))+(((-1.0)*x35*x47))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) { continue; } } { -IkReal dummyeval[1]; -IkReal gconst7; -gconst7=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ +IkReal j2eval[2]; +sj5=1.0; +cj5=0; +j5=1.5707963267949; +j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3))); +j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) { -IkReal dummyeval[1]; -IkReal gconst8; -gconst8=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; +continue; // no branches [j2] } else { @@ -582,33 +616,25 @@ continue; IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x108=((py)*(r02)); -IkReal x109=((IkReal(1750.00000000000))*(cj1)); -IkReal x110=((px)*(sj3)); -IkReal x111=((cj3)*(pz)); -IkReal x112=((cj0)*(sj1)); -IkReal x113=((IkReal(1749.99997471250))*(r02)); -IkReal x114=((px)*(r01)); -IkReal x115=((r00)*(sj3)); -IkReal x116=((pz)*(sj3)); -IkReal x117=((pz)*(r01)); -IkReal x118=((IkReal(0.297499995701125))*(sj3)); -IkReal x119=((IkReal(1656.25000000000))*(cj1)); -IkReal x120=((IkReal(0.297499995701125))*(cj3)); -IkReal x121=((pz)*(r00)); -IkReal x122=((cj3)*(px)); -IkReal x123=((cj3)*(py)); -IkReal x124=((px)*(r02)); -IkReal x125=((IkReal(0.297499995701125))*(r01)); -IkReal x126=((sj0)*(sj1)); -IkReal x127=((IkReal(0.281562495931422))*(x112)); -IkReal x128=((IkReal(1656.24997606719))*(x126)); -IkReal x129=((IkReal(1749.99997471250))*(x126)); -IkReal x130=((py)*(x126)); -IkReal x131=((IkReal(0.281562495931422))*(x126)); -if( IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(py)*(r00)*(x119)))+(((x117)*(x127)))+(((x114)*(x119)))+(((IkReal(-185.500000000000))*(sj3)))+(((x108)*(x128)))+(((IkReal(-1656.24997606719))*(x112)*(x121)))+(((x109)*(x116)))+(((IkReal(-1.00000000000000))*(x117)*(x128)))+(((x112)*(x113)*(x122)))+(((cj3)*(x108)*(x129)))+(((x124)*(x131)))+(((x120)*(x124)*(x126)))+(((IkReal(-0.297499995701125))*(r00)*(x111)*(x126)))+(((IkReal(-0.297499995701125))*(x110)*(x112)))+(((cj3)*(x109)*(x114)))+(((IkReal(-1.00000000000000))*(x121)*(x131)))+(((IkReal(-1.00000000000000))*(r01)*(x111)*(x129)))+(((IkReal(-1.00000000000000))*(x108)*(x127)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x112)))+(((IkReal(1656.24997606719))*(x112)*(x124)))+(((IkReal(-1749.99997471250))*(r00)*(x111)*(x112)))+(((x110)*(x129)))+(((IkReal(-1.00000000000000))*(x108)*(x112)*(x120)))+(((IkReal(-1.00000000000000))*(r00)*(x109)*(x123)))+(((IkReal(-1.00000000000000))*(x118)*(x130)))+(((x111)*(x112)*(x125))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((IkReal(175.562500000000))+(((IkReal(1656.24997606719))*(py)*(x112)))+(((IkReal(-1749.99997471250))*(pz)*(x112)*(x115)))+(((px)*(x112)*(x120)))+(((IkReal(-0.297499995701125))*(pz)*(x115)*(x126)))+(((IkReal(0.281562495931422))*(x130)))+(((sj3)*(x108)*(x129)))+(((x110)*(x112)*(x113)))+(((IkReal(-1.00000000000000))*(px)*(x128)))+(((px)*(x127)))+(((IkReal(-1.00000000000000))*(x109)*(x111)))+(((x120)*(x130)))+(((IkReal(-1.00000000000000))*(py)*(x109)*(x115)))+(((IkReal(185.500000000000))*(cj3)))+(((x112)*(x116)*(x125)))+(((r01)*(x109)*(x110)))+(((IkReal(0.297499995701125))*(r02)*(x110)*(x126)))+(((IkReal(-1.00000000000000))*(r01)*(x116)*(x129)))+(((IkReal(-1.00000000000000))*(x122)*(x129)))+(((IkReal(-1.00000000000000))*(pz)*(x119)))+(((IkReal(1749.99997471250))*(x112)*(x123)))+(((IkReal(-1.00000000000000))*(x108)*(x112)*(x118))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst8)*(((((IkReal(-1.00000000000000))*(py)*(r00)*(x119)))+(((x117)*(x127)))+(((x114)*(x119)))+(((IkReal(-185.500000000000))*(sj3)))+(((x108)*(x128)))+(((IkReal(-1656.24997606719))*(x112)*(x121)))+(((x109)*(x116)))+(((IkReal(-1.00000000000000))*(x117)*(x128)))+(((x112)*(x113)*(x122)))+(((cj3)*(x108)*(x129)))+(((x124)*(x131)))+(((x120)*(x124)*(x126)))+(((IkReal(-0.297499995701125))*(r00)*(x111)*(x126)))+(((IkReal(-0.297499995701125))*(x110)*(x112)))+(((cj3)*(x109)*(x114)))+(((IkReal(-1.00000000000000))*(x121)*(x131)))+(((IkReal(-1.00000000000000))*(r01)*(x111)*(x129)))+(((IkReal(-1.00000000000000))*(x108)*(x127)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x112)))+(((IkReal(1656.24997606719))*(x112)*(x124)))+(((IkReal(-1749.99997471250))*(r00)*(x111)*(x112)))+(((x110)*(x129)))+(((IkReal(-1.00000000000000))*(x108)*(x112)*(x120)))+(((IkReal(-1.00000000000000))*(r00)*(x109)*(x123)))+(((IkReal(-1.00000000000000))*(x118)*(x130)))+(((x111)*(x112)*(x125)))))), ((gconst8)*(((IkReal(175.562500000000))+(((IkReal(1656.24997606719))*(py)*(x112)))+(((IkReal(-1749.99997471250))*(pz)*(x112)*(x115)))+(((px)*(x112)*(x120)))+(((IkReal(-0.297499995701125))*(pz)*(x115)*(x126)))+(((IkReal(0.281562495931422))*(x130)))+(((sj3)*(x108)*(x129)))+(((x110)*(x112)*(x113)))+(((IkReal(-1.00000000000000))*(px)*(x128)))+(((px)*(x127)))+(((IkReal(-1.00000000000000))*(x109)*(x111)))+(((x120)*(x130)))+(((IkReal(-1.00000000000000))*(py)*(x109)*(x115)))+(((IkReal(185.500000000000))*(cj3)))+(((x112)*(x116)*(x125)))+(((r01)*(x109)*(x110)))+(((IkReal(0.297499995701125))*(r02)*(x110)*(x126)))+(((IkReal(-1.00000000000000))*(r01)*(x116)*(x129)))+(((IkReal(-1.00000000000000))*(x122)*(x129)))+(((IkReal(-1.00000000000000))*(pz)*(x119)))+(((IkReal(1749.99997471250))*(x112)*(x123)))+(((IkReal(-1.00000000000000))*(x108)*(x112)*(x118))))))); +IkReal x52=((0.112)*cj1); +IkReal x53=(cj0*py); +IkReal x54=((0.106)*cj1); +IkReal x55=(px*sj0); +IkReal x56=(cj3*pz); +IkReal x57=((1.1e-7)*cj1); +IkReal x58=((0.106)*sj1); +IkReal x59=((1.1e-7)*sj1); +IkReal x60=((0.112)*sj1); +IkReal x61=((0.112)*pz*sj3); +CheckValue x62=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1); +if(!x62.valid){ +continue; +} +CheckValue x63 = IKatan2WithCheck(IkReal(((((-1.0)*x53*x59))+(((-1.0)*x53*x54))+(((-1.0)*x56*x60))+(((1.232e-8)*cj3))+((cj3*x52*x55))+(((-1.0)*pz*sj3*x52))+(((-1.0)*sj3*x55*x60))+(((-1.0)*pz*x58))+((sj3*x53*x60))+((pz*x57))+(((-1.0)*cj3*x52*x53))+(((0.011872)*sj3))+((x55*x59))+((x54*x55)))),IkReal(((-0.0112360000000121)+(((-1.0)*x53*x58))+(((-1.0)*sj3*x52*x53))+(((-1.0)*pz*sj3*x60))+((cj3*x55*x60))+(((1.232e-8)*sj3))+(((-1.0)*x55*x57))+((x53*x57))+((x52*x56))+(((-0.011872)*cj3))+((pz*x54))+((pz*x59))+((sj3*x52*x55))+((x55*x58))+(((-1.0)*cj3*x53*x60)))),IKFAST_ATAN2_MAGTHRESH); +if(!x63.valid){ +continue; +} +j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x62.value)))+(x63.value)); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -636,38 +662,37 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { IkReal evalcond[4]; -IkReal x132=IKsin(j2); -IkReal x133=IKcos(j2); -IkReal x134=((r00)*(sj0)); -IkReal x135=((r01)*(sj0)); -IkReal x136=((py)*(sj1)); -IkReal x137=((IkReal(0.999999985550000))*(r02)); -IkReal x138=((px)*(sj1)); -IkReal x139=((IkReal(0.999999985550000))*(sj0)); -IkReal x140=((IkReal(0.000169999997543500))*(cj0)); -IkReal x141=((IkReal(0.000169999997543500))*(sj0)); -IkReal x142=((cj1)*(px)); -IkReal x143=((pz)*(sj1)); -IkReal x144=((cj0)*(r00)); -IkReal x145=((cj1)*(pz)); -IkReal x146=((IkReal(1.00000000000000))*(r01)); -IkReal x147=((cj1)*(py)); -IkReal x148=((IkReal(0.999999985550000))*(cj0)); -IkReal x149=((IkReal(0.106000000000000))*(x132)); -IkReal x150=((IkReal(0.106000000000000))*(x133)); -IkReal x151=((IkReal(0.112000000000000))*(x133)); -IkReal x152=((IkReal(0.112000000000000))*(x132)); -IkReal x153=((cj3)*(x152)); -IkReal x154=((sj3)*(x151)); -IkReal x155=((sj3)*(x152)); -IkReal x156=((cj3)*(x151)); -IkReal x157=((x156)+(x150)); -IkReal x158=((x154)+(x153)+(x149)); -evalcond[0]=((((x140)*(x142)))+(((IkReal(-1.00000000000000))*(x139)*(x142)))+(x158)+(x143)+(((x141)*(x147)))+(((x147)*(x148)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((x138)*(x139)))+(((IkReal(-1.00000000000000))*(x157)))+(((IkReal(-1.00000000000000))*(x138)*(x140)))+(x155)+(x145)+(((IkReal(-1.00000000000000))*(x136)*(x148)))+(((IkReal(-1.00000000000000))*(x136)*(x141)))); -evalcond[2]=((((IkReal(0.999999985550000))*(x135)*(x143)))+(((IkReal(-1.00000000000000))*(x158)))+(((IkReal(-1.00000000000000))*(x142)*(x146)))+(((r00)*(x147)))+(((r02)*(x136)*(x140)))+(((IkReal(-1.00000000000000))*(cj0)*(x137)*(x138)))+(((IkReal(0.999999985550000))*(x143)*(x144)))+(((IkReal(-1.00000000000000))*(r01)*(x140)*(x143)))+(((IkReal(0.000169999997543500))*(x134)*(x143)))+(((IkReal(-1.00000000000000))*(r02)*(x138)*(x141)))+(((IkReal(-1.00000000000000))*(sj0)*(x136)*(x137)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x157)))+(((IkReal(-1.00000000000000))*(x138)*(x146)))+(((IkReal(1.80199997396110e-5))*(x134)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(0.105999998468300))*(x144)))+(((IkReal(-0.000169999997543500))*(x134)*(x145)))+(((cj0)*(x137)*(x142)))+(((sj0)*(x137)*(x147)))+(((IkReal(-0.999999985550000))*(x135)*(x145)))+(((r00)*(x136)))+(x155)+(((IkReal(0.105999998468300))*(x135)))+(((r02)*(x141)*(x142)))+(((r01)*(x140)*(x145)))+(((IkReal(-0.999999985550000))*(x144)*(x145)))+(((IkReal(-1.00000000000000))*(r02)*(x140)*(x147)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IkReal x64=IKsin(j2); +IkReal x65=IKcos(j2); +IkReal x66=((1.0)*r01); +IkReal x67=(cj1*pz); +IkReal x68=(r00*sj1); +IkReal x69=(cj0*r00); +IkReal x70=(px*sj1); +IkReal x71=(cj0*r02); +IkReal x72=(cj1*py); +IkReal x73=(r02*sj0); +IkReal x74=(r01*sj0); +IkReal x75=(cj1*px); +IkReal x76=(pz*sj1); +IkReal x77=((1.1e-7)*x64); +IkReal x78=((0.106)*x65); +IkReal x79=((1.1e-7)*x65); +IkReal x80=((0.106)*x64); +IkReal x81=((0.112)*x65); +IkReal x82=((0.112)*x64); +IkReal x83=((1.0)*py*sj1); +IkReal x84=(sj3*x82); +IkReal x85=(cj3*x81); +IkReal x86=(cj3*x82); +IkReal x87=(sj3*x81); +IkReal x88=(x77+x78+x85); +IkReal x89=(x80+x86+x87); +evalcond[0]=((-1.1e-7)+(((-1.0)*x79))+((cj0*x72))+x76+x89+(((-1.0)*sj0*x75))); +evalcond[1]=((-0.106)+(((-1.0)*cj0*x83))+x67+x84+((sj0*x70))+(((-1.0)*x88))); +evalcond[2]=((((-1.0)*sj0*x66*x67))+(((-1.0)*x66*x70))+(((0.106)*x74))+(((0.106)*x69))+x84+(((-1.0)*x67*x69))+((x72*x73))+(((-1.0)*x88))+((py*x68))+((x71*x75))); +evalcond[3]=((((-1.0)*x73*x83))+(((-1.0)*x70*x71))+(((-1.0)*x66*x75))+((cj0*pz*x68))+(((-1.1e-7)*x74))+x79+(((-1.1e-7)*x69))+(((-1.0)*x89))+((x74*x76))+((r00*x72))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -709,6 +734,104 @@ solutions.AddSolution(vinfos,vfree); } } +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +IkReal x90=((8.92857142856662)*sj1); +IkReal x91=(px*sj0); +IkReal x92=(cj0*py); +IkReal x93=((9.26549865228612e-6)*cj1); +if( (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x90*x91))+((x90*x92))+(((42.1159029649369)*(pz*pz)))+(((-1.0)*x92*x93))+((x91*x93)))) < -1-IKFAST_SINCOS_THRESH || (((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x90*x91))+((x90*x92))+(((42.1159029649369)*(pz*pz)))+(((-1.0)*x92*x93))+((x91*x93)))) > 1+IKFAST_SINCOS_THRESH ) + continue; +IkReal x94=((-1.0)*(IKasin(((-0.528301886792168)+(((42.1159029649369)*(py*py)))+(((-8.92857142856662)*cj1*pz))+(((42.1159029649369)*(px*px)))+(((-9.26549865228612e-6)*pz*sj1))+(((-1.0)*x90*x91))+((x90*x92))+(((42.1159029649369)*(pz*pz)))+(((-1.0)*x92*x93))+((x91*x93)))))); +j3array[0]=((-1.57079736453075)+(((-1.0)*x94))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((1.57079528905905)+x94); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++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[1]; +IkReal x95=px*px; +IkReal x96=py*py; +IkReal x97=pz*pz; +IkReal x98=((0.212)*py); +IkReal x99=(r00*sj1); +IkReal x100=(cj0*r00); +IkReal x101=(px*r01); +IkReal x102=((2.2e-7)*cj1); +IkReal x103=(cj1*r02); +IkReal x104=((2.0)*px); +IkReal x105=(pz*r02); +IkReal x106=(py*sj0); +IkReal x107=(r01*sj0); +IkReal x108=((2.2e-7)*sj1); +IkReal x109=(cj0*px); +IkReal x110=((1.0)*x100); +IkReal x111=((0.212)*cj1*pz); +evalcond[0]=((0.0237800000000121)+(((2.2e-7)*cj0*pz*x99))+((x100*x111))+(((-1.0)*x98*x99))+((x100*x95))+(((-1.0)*x101*x102))+(((2.0)*x105*x106))+((x107*x96))+(((-1.0)*r02*x106*x108))+((pz*x107*x108))+(((-1.0)*r02*x108*x109))+(((-2.464e-8)*(IKsin(j3))))+(((-1.0)*x107*x97))+(((-1.0)*x107*x95))+(((-1.0)*x110*x96))+(((-1.0)*x110*x97))+(((2.0)*cj0*py*x101))+(((-0.212)*x103*x109))+(((0.212)*sj1*x101))+(((0.023744)*(IKcos(j3))))+(((-1.0)*sj0*x103*x98))+((cj0*x104*x105))+((r00*x104*x106))+(((-0.0112360000000121)*x100))+(((-0.0112360000000121)*x107))+((py*r00*x102))+((x107*x111))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j2eval[2]; +sj5=-1.0; +cj5=0; +j5=-1.5707963267949; +j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3))); +j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j2] } else { @@ -716,26 +839,25 @@ solutions.AddSolution(vinfos,vfree); IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x159=((cj0)*(sj1)); -IkReal x160=((IkReal(0.281562495931422))*(px)); -IkReal x161=((cj1)*(sj0)); -IkReal x162=((IkReal(0.281562495931422))*(py)); -IkReal x163=((IkReal(1656.24997606719))*(px)); -IkReal x164=((IkReal(1656.24997606719))*(py)); -IkReal x165=((cj0)*(cj1)); -IkReal x166=((IkReal(1749.99997471250))*(px)); -IkReal x167=((IkReal(0.297499995701125))*(px)); -IkReal x168=((IkReal(1749.99997471250))*(py)); -IkReal x169=((IkReal(0.297499995701125))*(py)); -IkReal x170=((sj0)*(sj1)); -IkReal x171=((IkReal(1656.25000000000))*(pz)); -IkReal x172=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x173=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x174=((sj3)*(x170)); -IkReal x175=((cj3)*(x170)); -if( IKabs(((gconst7)*(((((x161)*(x162)))+(((sj1)*(x171)))+(((sj1)*(x172)))+(((cj3)*(x161)*(x169)))+(((IkReal(-1.00000000000000))*(cj3)*(x161)*(x166)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x161)*(x163)))+(((x166)*(x174)))+(((x164)*(x165)))+(((IkReal(-1.00000000000000))*(sj3)*(x159)*(x167)))+(((IkReal(-1.00000000000000))*(sj3)*(x159)*(x168)))+(((cj1)*(x173)))+(((IkReal(-1.00000000000000))*(x169)*(x174)))+(((cj3)*(x165)*(x167)))+(((cj3)*(x165)*(x168)))+(((x160)*(x165))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(sj3)*(x161)*(x166)))+(((sj1)*(x173)))+(((x162)*(x170)))+(((IkReal(-1.00000000000000))*(x163)*(x170)))+(((x169)*(x175)))+(((sj3)*(x165)*(x167)))+(((sj3)*(x165)*(x168)))+(((sj3)*(x161)*(x169)))+(((IkReal(-1.00000000000000))*(cj1)*(x172)))+(((IkReal(-1.00000000000000))*(cj1)*(x171)))+(((IkReal(185.500000000000))*(cj3)))+(((x159)*(x160)))+(((x159)*(x164)))+(((IkReal(-1.00000000000000))*(x166)*(x175)))+(((cj3)*(x159)*(x168)))+(((cj3)*(x159)*(x167))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst7)*(((((x161)*(x162)))+(((sj1)*(x171)))+(((sj1)*(x172)))+(((cj3)*(x161)*(x169)))+(((IkReal(-1.00000000000000))*(cj3)*(x161)*(x166)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x161)*(x163)))+(((x166)*(x174)))+(((x164)*(x165)))+(((IkReal(-1.00000000000000))*(sj3)*(x159)*(x167)))+(((IkReal(-1.00000000000000))*(sj3)*(x159)*(x168)))+(((cj1)*(x173)))+(((IkReal(-1.00000000000000))*(x169)*(x174)))+(((cj3)*(x165)*(x167)))+(((cj3)*(x165)*(x168)))+(((x160)*(x165)))))), ((gconst7)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(sj3)*(x161)*(x166)))+(((sj1)*(x173)))+(((x162)*(x170)))+(((IkReal(-1.00000000000000))*(x163)*(x170)))+(((x169)*(x175)))+(((sj3)*(x165)*(x167)))+(((sj3)*(x165)*(x168)))+(((sj3)*(x161)*(x169)))+(((IkReal(-1.00000000000000))*(cj1)*(x172)))+(((IkReal(-1.00000000000000))*(cj1)*(x171)))+(((IkReal(185.500000000000))*(cj3)))+(((x159)*(x160)))+(((x159)*(x164)))+(((IkReal(-1.00000000000000))*(x166)*(x175)))+(((cj3)*(x159)*(x168)))+(((cj3)*(x159)*(x167))))))); +IkReal x112=((0.112)*cj1); +IkReal x113=(cj0*py); +IkReal x114=((0.106)*cj1); +IkReal x115=(px*sj0); +IkReal x116=(cj3*pz); +IkReal x117=((1.1e-7)*cj1); +IkReal x118=((0.106)*sj1); +IkReal x119=((1.1e-7)*sj1); +IkReal x120=((0.112)*sj1); +IkReal x121=((0.112)*pz*sj3); +CheckValue x122=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1); +if(!x122.valid){ +continue; +} +CheckValue x123 = IKatan2WithCheck(IkReal((((cj3*x112*x115))+(((-1.0)*sj3*x115*x120))+(((1.232e-8)*cj3))+((pz*x117))+((x114*x115))+(((-1.0)*x113*x119))+(((-1.0)*x113*x114))+(((-1.0)*x116*x120))+(((-1.0)*cj3*x112*x113))+(((-1.0)*pz*sj3*x112))+(((0.011872)*sj3))+((x115*x119))+((sj3*x113*x120))+(((-1.0)*pz*x118)))),IkReal(((-0.0112360000000121)+((sj3*x112*x115))+((x112*x116))+(((-1.0)*cj3*x113*x120))+((pz*x119))+((pz*x114))+(((-1.0)*x113*x118))+(((1.232e-8)*sj3))+((x113*x117))+((cj3*x115*x120))+(((-1.0)*x115*x117))+(((-1.0)*pz*sj3*x120))+(((-1.0)*sj3*x112*x113))+(((-0.011872)*cj3))+((x115*x118)))),IKFAST_ATAN2_MAGTHRESH); +if(!x123.valid){ +continue; +} +j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x122.value)))+(x123.value)); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -763,38 +885,37 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { IkReal evalcond[4]; -IkReal x176=IKsin(j2); -IkReal x177=IKcos(j2); -IkReal x178=((r00)*(sj0)); -IkReal x179=((r01)*(sj0)); -IkReal x180=((py)*(sj1)); -IkReal x181=((IkReal(0.999999985550000))*(r02)); -IkReal x182=((px)*(sj1)); -IkReal x183=((IkReal(0.999999985550000))*(sj0)); -IkReal x184=((IkReal(0.000169999997543500))*(cj0)); -IkReal x185=((IkReal(0.000169999997543500))*(sj0)); -IkReal x186=((cj1)*(px)); -IkReal x187=((pz)*(sj1)); -IkReal x188=((cj0)*(r00)); -IkReal x189=((cj1)*(pz)); -IkReal x190=((IkReal(1.00000000000000))*(r01)); -IkReal x191=((cj1)*(py)); -IkReal x192=((IkReal(0.999999985550000))*(cj0)); -IkReal x193=((IkReal(0.106000000000000))*(x176)); -IkReal x194=((IkReal(0.106000000000000))*(x177)); -IkReal x195=((IkReal(0.112000000000000))*(x177)); -IkReal x196=((IkReal(0.112000000000000))*(x176)); -IkReal x197=((cj3)*(x196)); -IkReal x198=((sj3)*(x195)); -IkReal x199=((sj3)*(x196)); -IkReal x200=((cj3)*(x195)); -IkReal x201=((x194)+(x200)); -IkReal x202=((x198)+(x193)+(x197)); -evalcond[0]=((x187)+(((x185)*(x191)))+(((x184)*(x186)))+(x202)+(((x191)*(x192)))+(((IkReal(-1.00000000000000))*(x183)*(x186)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x180)*(x192)))+(((IkReal(-1.00000000000000))*(x180)*(x185)))+(x199)+(x189)+(((IkReal(-1.00000000000000))*(x201)))+(((IkReal(-1.00000000000000))*(x182)*(x184)))+(((x182)*(x183)))); -evalcond[2]=((((r00)*(x191)))+(((IkReal(-1.00000000000000))*(x186)*(x190)))+(((IkReal(-1.00000000000000))*(cj0)*(x181)*(x182)))+(((IkReal(-1.00000000000000))*(x202)))+(((IkReal(-1.00000000000000))*(sj0)*(x180)*(x181)))+(((IkReal(-1.00000000000000))*(r01)*(x184)*(x187)))+(((IkReal(-1.00000000000000))*(r02)*(x182)*(x185)))+(((IkReal(0.999999985550000))*(x179)*(x187)))+(((IkReal(0.000169999997543500))*(x178)*(x187)))+(((r02)*(x180)*(x184)))+(((IkReal(0.999999985550000))*(x187)*(x188)))); -evalcond[3]=((((IkReal(-0.000169999997543500))*(x178)*(x189)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(r02)*(x184)*(x191)))+(((r02)*(x185)*(x186)))+(((IkReal(0.105999998468300))*(x188)))+(((IkReal(0.105999998468300))*(x179)))+(((r00)*(x180)))+(x199)+(((IkReal(-1.00000000000000))*(x201)))+(((IkReal(-0.999999985550000))*(x188)*(x189)))+(((IkReal(-0.999999985550000))*(x179)*(x189)))+(((sj0)*(x181)*(x191)))+(((cj0)*(x181)*(x186)))+(((IkReal(1.80199997396110e-5))*(x178)))+(((IkReal(-1.00000000000000))*(x182)*(x190)))+(((r01)*(x184)*(x189)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IkReal x124=IKsin(j2); +IkReal x125=IKcos(j2); +IkReal x126=((1.0)*r01); +IkReal x127=(cj1*pz); +IkReal x128=(r00*sj1); +IkReal x129=(cj0*r00); +IkReal x130=(px*sj1); +IkReal x131=(cj0*r02); +IkReal x132=(cj1*py); +IkReal x133=(r02*sj0); +IkReal x134=(r01*sj0); +IkReal x135=(cj1*px); +IkReal x136=(pz*sj1); +IkReal x137=((1.1e-7)*x124); +IkReal x138=((0.106)*x125); +IkReal x139=((0.106)*x124); +IkReal x140=((1.1e-7)*x125); +IkReal x141=((0.112)*x125); +IkReal x142=((0.112)*x124); +IkReal x143=((1.0)*py*sj1); +IkReal x144=(cj3*x141); +IkReal x145=(sj3*x142); +IkReal x146=(cj3*x142); +IkReal x147=(sj3*x141); +IkReal x148=(x144+x137+x138); +IkReal x149=(x146+x147+x139); +evalcond[0]=((-1.1e-7)+(((-1.0)*x140))+((cj0*x132))+x149+x136+(((-1.0)*sj0*x135))); +evalcond[1]=((-0.106)+(((-1.0)*x148))+(((-1.0)*cj0*x143))+x145+x127+((sj0*x130))); +evalcond[2]=((((-1.0)*x145))+(((0.106)*x134))+(((0.106)*x129))+(((-1.0)*x127*x129))+((py*x128))+(((-1.0)*x126*x130))+x148+((x131*x135))+(((-1.0)*sj0*x126*x127))+((x132*x133))); +evalcond[3]=(((cj0*pz*x128))+(((-1.0)*x140))+(((-1.0)*x130*x131))+(((-1.0)*x133*x143))+(((-1.1e-7)*x129))+(((-1.0)*x126*x135))+(((-1.1e-7)*x134))+((r00*x132))+x149+((x134*x136))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -839,92 +960,72 @@ solutions.AddSolution(vinfos,vfree); } } -} else +} +} while(0); +if( bgotonextstatement ) { -IkReal x203=(py)*(py); -IkReal x204=(pz)*(pz); -IkReal x205=(px)*(px); -IkReal x206=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x207=((cj0)*(r00)); -IkReal x208=((r01)*(sj0)); -IkReal x209=((IkReal(0.000169999997543500))*(sj1)); -IkReal x210=((IkReal(0.000339999995087000))*(py)); -IkReal x211=((pz)*(sj1)); -IkReal x212=((r02)*(sj0)); -IkReal x213=((IkReal(0.000169999997543500))*(cj0)); -IkReal x214=((py)*(r01)); -IkReal x215=((IkReal(0.000169999997543500))*(pz)); -IkReal x216=((r00)*(sj0)); -IkReal x217=((px)*(r00)); -IkReal x218=((IkReal(0.000169999997543500))*(cj1)); -IkReal x219=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x220=((cj0)*(r01)); -IkReal x221=((IkReal(0.999999985550000))*(px)); -IkReal x222=((IkReal(0.105999998468300))*(sj1)); -IkReal x223=((cj1)*(r02)); -IkReal x224=((cj0)*(r02)); -IkReal x225=((IkReal(0.000339999995087000))*(px)); -IkReal x226=((IkReal(0.999999985550000))*(py)); -IkReal x227=((IkReal(0.999999985550000))*(sj1)); -IkReal x228=((IkReal(0.999999985550000))*(cj1)); -IkReal x229=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x230=((px)*(sj1)); -IkReal x231=((IkReal(0.0112359998376398))*(sj1)); -IkReal x232=((cj1)*(px)); -IkReal x233=((cj0)*(pz)); -IkReal x234=((r02)*(sj1)); -IkReal x235=((IkReal(1.99999997110000))*(py)); -IkReal x236=((pz)*(r02)); -IkReal x237=((IkReal(0.999999985550000))*(pz)); -IkReal x238=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x239=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x240=((cj1)*(pz)); -IkReal x241=((IkReal(0.211999996936600))*(pz)); -IkReal x242=((cj1)*(x220)); -IkReal x243=((px)*(x235)); -IkReal x244=((IkReal(2.00000000000000))*(x204)); -IkReal x245=((x204)*(x227)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((py)*(x213)))+(((IkReal(-1.00000000000000))*(cj0)*(x221)))+(((IkReal(-1.00000000000000))*(sj0)*(x226)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))); -evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-0.000169999997543500))*(x216)))+(((IkReal(-0.999999985550000))*(x208)))+(((IkReal(-0.999999985550000))*(x207)))+(((r01)*(x213)))); -evalcond[3]=((((IkReal(0.106000000000000))*(x223)))+(((IkReal(-1.00000000000000))*(x207)*(x229)))+(((x216)*(x222)))+(((IkReal(-1.00000000000000))*(x208)*(x229)))+(((IkReal(-1.00000000000000))*(x220)*(x222)))+(((IkReal(-1.00000000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x214)))+(((IkReal(-1.00000000000000))*(x236)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x216)*(x228)))+(((x208)*(x218)))+(((x207)*(x218)))+(x234)+(((x220)*(x228)))); -evalcond[5]=((((x216)*(x227)))+(((IkReal(-1.00000000000000))*(x208)*(x209)))+(((IkReal(-1.00000000000000))*(x207)*(x209)))+(x223)+(((IkReal(-1.00000000000000))*(x220)*(x227)))); -evalcond[6]=((((px)*(r02)*(x213)))+(((x224)*(x226)))+(((IkReal(-0.105999998468300))*(cj1)*(x216)))+(((IkReal(-1.00000000000000))*(x207)*(x215)))+(((IkReal(0.000169999997543500))*(py)*(x212)))+(((IkReal(-1.00000000000000))*(x220)*(x237)))+(((IkReal(0.105999998468300))*(x242)))+(((IkReal(-1.00000000000000))*(x212)*(x221)))+(((IkReal(0.106000000000000))*(x234)))+(((x216)*(x237)))+(((IkReal(-1.00000000000000))*(x208)*(x215)))+(((x208)*(x238)))+(((x207)*(x238)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x208)*(x230)*(x235)))+(((IkReal(-1.00000000000000))*(x203)*(x207)*(x209)))+(((x207)*(x230)*(x235)))+(((IkReal(-2.00000000000000))*(x214)*(x240)))+(((IkReal(-1.00000000000000))*(x204)*(x207)*(x209)))+(((IkReal(-1.00000000000000))*(x204)*(x208)*(x209)))+(((x203)*(x208)*(x209)))+(((IkReal(-1.00000000000000))*(x220)*(x245)))+(((x211)*(x224)*(x225)))+(((x205)*(x207)*(x209)))+(((IkReal(-1.00000000000000))*(x216)*(x231)))+(((x211)*(x224)*(x235)))+(((x216)*(x245)))+(((IkReal(-1.00000000000000))*(x223)*(x244)))+(((IkReal(-1.99999997110000))*(px)*(x211)*(x212)))+(((IkReal(-1.00000000000000))*(x205)*(x208)*(x209)))+(((IkReal(-0.0112360000000000))*(x223)))+(((IkReal(0.212000000000000))*(x236)))+(((IkReal(-1.00000000000000))*(x205)*(x216)*(x227)))+(((pp)*(x223)))+(((x210)*(x220)*(x230)))+(((x203)*(x220)*(x227)))+(((IkReal(-1.00000000000000))*(x205)*(x220)*(x227)))+(((x210)*(x211)*(x212)))+(((x220)*(x231)))+(((x208)*(x239)))+(((x203)*(x216)*(x227)))+(((x207)*(x239)))+(((x210)*(x216)*(x230)))+(((IkReal(0.212000000000000))*(x214)))+(((IkReal(0.212000000000000))*(x217)))+(((IkReal(-2.00000000000000))*(x217)*(x240)))); -evalcond[8]=((((IkReal(0.0112359998376398))*(x242)))+(((pp)*(x234)))+(((IkReal(-1.00000000000000))*(x210)*(x212)*(x240)))+(((IkReal(0.0112360000000000))*(x234)))+(((x205)*(x216)*(x228)))+(((IkReal(-1.00000000000000))*(x207)*(x232)*(x235)))+(((IkReal(3.60399994792220e-5))*(py)*(x212)))+(((IkReal(-1.00000000000000))*(x220)*(x241)))+(((IkReal(-1.00000000000000))*(x210)*(x216)*(x232)))+(((IkReal(-1.00000000000000))*(x203)*(x220)*(x228)))+(((x204)*(x207)*(x218)))+(((IkReal(-1.00000000000000))*(x203)*(x216)*(x228)))+(((IkReal(-2.00000000000000))*(x211)*(x214)))+(((IkReal(-2.00000000000000))*(x211)*(x217)))+(((x208)*(x219)))+(((x207)*(x219)))+(((IkReal(-1.00000000000000))*(x204)*(x216)*(x228)))+(((IkReal(-1.00000000000000))*(x234)*(x244)))+(((IkReal(-1.00000000000000))*(x206)*(x208)))+(((IkReal(-1.00000000000000))*(x206)*(x207)))+(((x216)*(x241)))+(((IkReal(-1.00000000000000))*(x205)*(x207)*(x218)))+(((x205)*(x220)*(x228)))+(((IkReal(-1.00000000000000))*(x223)*(x233)*(x235)))+(((IkReal(3.60399994792220e-5))*(px)*(x224)))+(((IkReal(0.211999996936600))*(py)*(x224)))+(((x205)*(x208)*(x218)))+(((x204)*(x220)*(x228)))+(((IkReal(-0.0112359998376398))*(cj1)*(x216)))+(((x203)*(x207)*(x218)))+(((IkReal(-1.00000000000000))*(x210)*(x220)*(x232)))+(((IkReal(-0.211999996936600))*(px)*(x212)))+(((x208)*(x232)*(x235)))+(((IkReal(1.99999997110000))*(pz)*(x212)*(x232)))+(((x204)*(x208)*(x218)))+(((IkReal(-1.00000000000000))*(x203)*(x208)*(x218)))+(((IkReal(-1.00000000000000))*(x223)*(x225)*(x233)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 ) +bool bgotonextstatement = true; +do { +if( 1 ) { -IkReal j3array[2], cj3array[2], sj3array[2]; -bool j3valid[2]={false}; -_nj3 = 2; -IkReal x246=((IkReal(8.92857129955357))*(sj1)); -IkReal x247=((IkReal(0.00151785712092411))*(sj1)); -cj3array[0]=((IkReal(-0.528301886792453))+(((cj0)*(py)*(x246)))+(((IkReal(-8.92857142857143))*(cj1)*(pz)))+(((py)*(sj0)*(x247)))+(((cj0)*(px)*(x247)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x246)))); -if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +bgotonextstatement=false; +continue; // branch miss [j2, j3] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else { - j3valid[0] = j3valid[1] = true; - j3array[0] = IKacos(cj3array[0]); - sj3array[0] = IKsin(j3array[0]); - cj3array[1] = cj3array[0]; - j3array[1] = -j3array[0]; - sj3array[1] = -sj3array[0]; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x150=((1100000000.0)*px); +IkReal x151=(r02*sj0); +IkReal x152=((112360000000121.0)*r02); +IkReal x153=((1.06e+15)*r00); +IkReal x154=(pz*sj0); +IkReal x155=((112360000000121.0)*sj1); +IkReal x156=(cj0*r01); +IkReal x157=(py*r01); +IkReal x158=(r00*sj0); +IkReal x159=((112360000000121.0)*cj1); +IkReal x160=(pz*r02); +IkReal x161=(cj0*py*r02); +CheckValue x162 = IKatan2WithCheck(IkReal(((((1100000000.0)*r00*x154))+(((1.06e+15)*x160))+(((1.06e+15)*x157))+(((1100000000.0)*x161))+(((123200000.0)*cj5))+(((-1.0)*x150*x151))+((x155*x156))+(((-1100000000.0)*pz*x156))+((px*x153))+(((-1.0)*cj1*x152))+(((-1.0)*x155*x158)))),IkReal(((((-1.06e+15)*x161))+(((1100000000.0)*x157))+(((1100000000.0)*x160))+(((-1.0)*x153*x154))+((r00*x150))+(((-118720000000000.0)*cj5))+(((1.06e+15)*px*x151))+(((1.06e+15)*pz*x156))+(((-1.0)*x156*x159))+((x158*x159))+(((-1.0)*sj1*x152)))),IKFAST_ATAN2_MAGTHRESH); +if(!x162.valid){ +continue; +} +CheckValue x163=IKPowWithIntegerCheck(IKsign(cj5),-1); +if(!x163.valid){ +continue; } -else if( std::isnan(cj3array[0]) ) +j3array[0]=((-1.5707963267949)+(x162.value)+(((1.5707963267949)*(x163.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) { - // probably any value will work - j3valid[0] = true; - cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; + j3array[0]-=IK2PI; } -for(int ij3 = 0; ij3 < 2; ++ij3) +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 < 2; ++iij3) +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 ) { @@ -933,49 +1034,91 @@ if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRES } j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; { -IkReal evalcond[1]; -IkReal x248=(px)*(px); -IkReal x249=(py)*(py); -IkReal x250=(pz)*(pz); -IkReal x251=((r00)*(sj0)); -IkReal x252=((cj0)*(r00)); -IkReal x253=((IkReal(0.212000000000000))*(sj1)); -IkReal x254=((cj0)*(r01)); -IkReal x255=((r01)*(sj0)); -IkReal x256=((px)*(py)); -IkReal x257=((IkReal(0.211999996936600))*(cj1)); -IkReal x258=((r02)*(sj0)); -IkReal x259=((IkReal(3.60399994792220e-5))*(cj1)); -IkReal x260=((IkReal(0.999999985550000))*(x249)); -IkReal x261=((IkReal(0.999999985550000))*(x250)); -IkReal x262=((IkReal(0.000169999997543500))*(x248)); -IkReal x263=((cj0)*(cj1)*(r02)); -IkReal x264=((IkReal(0.000169999997543500))*(x249)); -IkReal x265=((IkReal(0.999999985550000))*(x248)); -IkReal x266=((cj0)*(pz)*(r02)); -IkReal x267=((IkReal(0.000169999997543500))*(x250)); -evalcond[0]=((IkReal(0.0237800000000000))+(((IkReal(-1.00000000000000))*(x255)*(x265)))+(((IkReal(-1.00000000000000))*(x255)*(x261)))+(((pz)*(x252)*(x257)))+(((IkReal(1.91011997239877e-6))*(x254)))+(((IkReal(1.99999997110000))*(x254)*(x256)))+(((IkReal(-0.000339999995087000))*(x252)*(x256)))+(((cj0)*(py)*(r02)*(x259)))+(((pz)*(x251)*(x259)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x253)))+(((IkReal(-1.00000000000000))*(py)*(x257)*(x258)))+(((pz)*(x255)*(x257)))+(((IkReal(-1.00000000000000))*(pz)*(x254)*(x259)))+(((IkReal(1.99999997110000))*(x251)*(x256)))+(((IkReal(1.99999997110000))*(px)*(x266)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(r02)*(x257)))+(((IkReal(1.99999997110000))*(py)*(pz)*(x258)))+(((IkReal(-0.000339999995087000))*(py)*(x266)))+(((IkReal(-1.91011997239877e-6))*(x251)))+(((x252)*(x265)))+(((px)*(r01)*(x253)))+(((x254)*(x262)))+(((x254)*(x267)))+(((IkReal(-1.00000000000000))*(x251)*(x267)))+(((IkReal(-1.00000000000000))*(x251)*(x264)))+(((x255)*(x260)))+(((IkReal(-1.00000000000000))*(px)*(x258)*(x259)))+(((IkReal(0.000339999995087000))*(px)*(pz)*(x258)))+(((IkReal(0.0237440000000000))*(IKcos(j3))))+(((IkReal(-0.0112359998376398))*(x252)))+(((IkReal(-0.0112359998376398))*(x255)))+(((IkReal(-1.00000000000000))*(x252)*(x260)))+(((IkReal(-1.00000000000000))*(x252)*(x261)))+(((IkReal(0.000339999995087000))*(x255)*(x256)))+(((x251)*(x262)))+(((IkReal(-1.00000000000000))*(x254)*(x264)))); -if( IKabs(evalcond[0]) > 0.000001 ) +IkReal evalcond[4]; +IkReal x164=IKcos(j3); +IkReal x165=IKsin(j3); +IkReal x166=px*px; +IkReal x167=py*py; +IkReal x168=pz*pz; +IkReal x169=(py*r00); +IkReal x170=((2.0)*px); +IkReal x171=((1.1e-7)*cj1); +IkReal x172=(cj0*r01); +IkReal x173=((0.212)*sj1); +IkReal x174=(cj0*r00); +IkReal x175=(r00*sj0); +IkReal x176=((0.212)*cj1); +IkReal x177=(cj0*py); +IkReal x178=((1.1e-7)*sj1); +IkReal x179=((2.2e-7)*cj1); +IkReal x180=(px*r01); +IkReal x181=((0.106)*cj1); +IkReal x182=(cj0*r02); +IkReal x183=(px*sj0); +IkReal x184=((1.0)*pz); +IkReal x185=(r01*sj0); +IkReal x186=((2.2e-7)*sj1); +IkReal x187=((0.106)*sj1); +IkReal x188=(cj5*x165); +IkReal x189=((1.0)*x167); +IkReal x190=(py*r02*sj0); +IkReal x191=((0.023744)*x164); +IkReal x192=((2.464e-8)*x165); +IkReal x193=(pz*x186); +IkReal x194=(cj5*x164); +IkReal x195=((1.0)*x168); +IkReal x196=((1.0)*x166); +evalcond[0]=((((-1.0)*px*r00))+(((1.1e-7)*x194))+(((-1.0)*r02*x184))+(((0.106)*x188))+((x175*x187))+((x171*x172))+(((-1.0)*x171*x175))+(((-1.0)*py*r01))+((r02*x181))+(((-1.0)*x172*x187))+((r02*x178))); +evalcond[1]=((0.012544)+((pz*x176))+((x173*x183))+(((-1.0)*x189))+(((-1.0)*x195))+(((-1.0)*x196))+(((-1.0)*x192))+x193+x191+(((-1.0)*x179*x183))+(((-1.0)*x173*x177))+((x177*x179))); +evalcond[2]=(((pz*x175))+(((-1.0)*x175*x178))+(((-1.0)*r02*x183))+(((0.112)*cj5))+((x172*x181))+(((-1.0)*r02*x171))+((x172*x178))+(((0.106)*x194))+(((-1.1e-7)*x188))+(((-1.0)*x175*x181))+((r02*x187))+(((-1.0)*x172*x184))+((r02*x177))); +evalcond[3]=((((-1.0)*px*x182*x186))+((pz*x176*x185))+((pz*x170*x182))+(((-0.0112360000000121)*x185))+((py*x170*x172))+((x173*x180))+(((-1.0)*x186*x190))+(((-0.0112360000000121)*x174))+((x169*x179))+(((-1.0)*px*x176*x182))+((x174*x193))+((pz*x174*x176))+((x185*x193))+(((2.0)*pz*x190))+((x167*x185))+((x166*x174))+(((-0.0237800000000121)*sj5))+(((-1.0)*x176*x190))+(((-1.0)*sj5*x191))+(((-1.0)*x185*x196))+(((-1.0)*x185*x195))+(((-1.0)*x169*x173))+(((-1.0)*x179*x180))+(((-1.0)*x174*x195))+(((-1.0)*x174*x189))+((sj0*x169*x170))+((sj5*x192))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) { continue; } } { -IkReal dummyeval[1]; -IkReal gconst9; -gconst9=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) +IkReal j2eval[2]; +j2eval[0]=cj5; +j2eval[1]=IKsign(cj5); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) { { -IkReal dummyeval[1]; -IkReal gconst10; -gconst10=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) +IkReal j2eval[2]; +IkReal x197=(cj3*cj5); +IkReal x198=(cj5*sj3); +j2eval[0]=((((-1.0)*x197))+(((-963636.363636364)*x198))); +j2eval[1]=IKsign(((((-1.1e-7)*x197))+(((-0.106)*x198)))); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) { -continue; +{ +IkReal j2eval[2]; +IkReal x199=(cj3*cj5); +IkReal x200=(cj5*sj3); +j2eval[0]=((((1018181.81818182)*cj5))+(((963636.363636364)*x199))+(((-1.0)*x200))); +j2eval[1]=IKsign(((((0.112)*cj5))+(((0.106)*x199))+(((-1.1e-7)*x200)))); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j2eval[2]; +sj5=1.0; +cj5=0; +j5=1.5707963267949; +j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3))); +j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j2] } else { @@ -983,32 +1126,25 @@ continue; IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x268=((py)*(r02)); -IkReal x269=((cj0)*(sj1)); -IkReal x270=((px)*(r01)); -IkReal x271=((py)*(r00)); -IkReal x272=((pz)*(r01)); -IkReal x273=((IkReal(0.297499995701125))*(py)); -IkReal x274=((sj0)*(sj1)); -IkReal x275=((IkReal(1656.25000000000))*(cj1)); -IkReal x276=((IkReal(0.297499995701125))*(px)); -IkReal x277=((pz)*(r00)); -IkReal x278=((IkReal(1749.99997471250))*(px)); -IkReal x279=((IkReal(1749.99997471250))*(py)); -IkReal x280=((px)*(r02)); -IkReal x281=((IkReal(1749.99997471250))*(sj3)); -IkReal x282=((IkReal(0.297499995701125))*(cj3)); -IkReal x283=((IkReal(0.297499995701125))*(sj3)); -IkReal x284=((IkReal(0.281562495931422))*(x269)); -IkReal x285=((IkReal(1750.00000000000))*(cj1)*(cj3)); -IkReal x286=((IkReal(1656.24997606719))*(x274)); -IkReal x287=((sj3)*(x278)); -IkReal x288=((IkReal(1750.00000000000))*(cj1)*(sj3)); -IkReal x289=((cj3)*(x274)); -IkReal x290=((IkReal(1749.99997471250))*(x277)); -if( IKabs(((gconst10)*(((((x268)*(x284)))+(((IkReal(-1.00000000000000))*(x270)*(x285)))+(((x271)*(x275)))+(((x268)*(x269)*(x282)))+(((IkReal(1656.24997606719))*(x269)*(x277)))+(((x274)*(x287)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj3)*(x269)*(x290)))+(((x271)*(x285)))+(((IkReal(-1656.24997606719))*(x269)*(x280)))+(((x272)*(x286)))+(((IkReal(1749.99997471250))*(x272)*(x289)))+(((IkReal(0.281562495931422))*(x274)*(x277)))+(((IkReal(-1.00000000000000))*(cj3)*(r02)*(x269)*(x278)))+(((IkReal(-0.281562495931422))*(x274)*(x280)))+(((IkReal(-1.00000000000000))*(sj3)*(x269)*(x276)))+(((IkReal(-1.00000000000000))*(sj3)*(x269)*(x279)))+(((IkReal(-1.00000000000000))*(x268)*(x286)))+(((IkReal(-1.00000000000000))*(x269)*(x272)*(x282)))+(((IkReal(-1749.99997471250))*(x268)*(x289)))+(((IkReal(-1.00000000000000))*(x270)*(x275)))+(((x274)*(x277)*(x282)))+(((IkReal(-1.00000000000000))*(x272)*(x284)))+(((IkReal(-1.00000000000000))*(r02)*(x276)*(x289)))+(((IkReal(-1.00000000000000))*(sj3)*(x273)*(x274)))+(((pz)*(x288))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x274)*(x276)))+(((IkReal(-1.00000000000000))*(pz)*(x275)))+(((IkReal(-1.00000000000000))*(x270)*(x288)))+(((x272)*(x274)*(x281)))+(((x268)*(x269)*(x283)))+(((IkReal(-1.00000000000000))*(px)*(x286)))+(((x269)*(x277)*(x281)))+(((IkReal(-1.00000000000000))*(x268)*(x274)*(x281)))+(((x271)*(x288)))+(((IkReal(0.281562495931422))*(py)*(x274)))+(((x273)*(x289)))+(((IkReal(-1.00000000000000))*(x269)*(x272)*(x283)))+(((IkReal(1656.24997606719))*(py)*(x269)))+(((IkReal(-1.00000000000000))*(pz)*(x285)))+(((IkReal(185.500000000000))*(cj3)))+(((x274)*(x277)*(x283)))+(((cj3)*(x269)*(x279)))+(((cj3)*(x269)*(x276)))+(((px)*(x284)))+(((IkReal(-1.00000000000000))*(x278)*(x289)))+(((IkReal(-1.00000000000000))*(r02)*(x269)*(x287))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst10)*(((((x268)*(x284)))+(((IkReal(-1.00000000000000))*(x270)*(x285)))+(((x271)*(x275)))+(((x268)*(x269)*(x282)))+(((IkReal(1656.24997606719))*(x269)*(x277)))+(((x274)*(x287)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj3)*(x269)*(x290)))+(((x271)*(x285)))+(((IkReal(-1656.24997606719))*(x269)*(x280)))+(((x272)*(x286)))+(((IkReal(1749.99997471250))*(x272)*(x289)))+(((IkReal(0.281562495931422))*(x274)*(x277)))+(((IkReal(-1.00000000000000))*(cj3)*(r02)*(x269)*(x278)))+(((IkReal(-0.281562495931422))*(x274)*(x280)))+(((IkReal(-1.00000000000000))*(sj3)*(x269)*(x276)))+(((IkReal(-1.00000000000000))*(sj3)*(x269)*(x279)))+(((IkReal(-1.00000000000000))*(x268)*(x286)))+(((IkReal(-1.00000000000000))*(x269)*(x272)*(x282)))+(((IkReal(-1749.99997471250))*(x268)*(x289)))+(((IkReal(-1.00000000000000))*(x270)*(x275)))+(((x274)*(x277)*(x282)))+(((IkReal(-1.00000000000000))*(x272)*(x284)))+(((IkReal(-1.00000000000000))*(r02)*(x276)*(x289)))+(((IkReal(-1.00000000000000))*(sj3)*(x273)*(x274)))+(((pz)*(x288)))))), ((gconst10)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x274)*(x276)))+(((IkReal(-1.00000000000000))*(pz)*(x275)))+(((IkReal(-1.00000000000000))*(x270)*(x288)))+(((x272)*(x274)*(x281)))+(((x268)*(x269)*(x283)))+(((IkReal(-1.00000000000000))*(px)*(x286)))+(((x269)*(x277)*(x281)))+(((IkReal(-1.00000000000000))*(x268)*(x274)*(x281)))+(((x271)*(x288)))+(((IkReal(0.281562495931422))*(py)*(x274)))+(((x273)*(x289)))+(((IkReal(-1.00000000000000))*(x269)*(x272)*(x283)))+(((IkReal(1656.24997606719))*(py)*(x269)))+(((IkReal(-1.00000000000000))*(pz)*(x285)))+(((IkReal(185.500000000000))*(cj3)))+(((x274)*(x277)*(x283)))+(((cj3)*(x269)*(x279)))+(((cj3)*(x269)*(x276)))+(((px)*(x284)))+(((IkReal(-1.00000000000000))*(x278)*(x289)))+(((IkReal(-1.00000000000000))*(r02)*(x269)*(x287))))))); +IkReal x201=((0.112)*cj1); +IkReal x202=(cj0*py); +IkReal x203=((0.106)*cj1); +IkReal x204=(px*sj0); +IkReal x205=(cj3*pz); +IkReal x206=((1.1e-7)*cj1); +IkReal x207=((0.106)*sj1); +IkReal x208=((1.1e-7)*sj1); +IkReal x209=((0.112)*sj1); +IkReal x210=((0.112)*pz*sj3); +CheckValue x211 = IKatan2WithCheck(IkReal(((((1.232e-8)*cj3))+((cj3*x201*x204))+(((-1.0)*pz*sj3*x201))+((sj3*x202*x209))+(((-1.0)*pz*x207))+(((-1.0)*x202*x203))+(((-1.0)*x202*x208))+(((-1.0)*sj3*x204*x209))+((pz*x206))+((x204*x208))+(((-1.0)*cj3*x201*x202))+(((-1.0)*x205*x209))+((x203*x204))+(((0.011872)*sj3)))),IkReal(((-0.0112360000000121)+(((-1.0)*cj3*x202*x209))+(((-1.0)*x204*x206))+(((-1.0)*sj3*x201*x202))+((x201*x205))+(((-1.0)*pz*sj3*x209))+(((1.232e-8)*sj3))+(((-1.0)*x202*x207))+((pz*x203))+((pz*x208))+((x204*x207))+((cj3*x204*x209))+((x202*x206))+(((-0.011872)*cj3))+((sj3*x201*x204)))),IKFAST_ATAN2_MAGTHRESH); +if(!x211.valid){ +continue; +} +CheckValue x212=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1); +if(!x212.valid){ +continue; +} +j2array[0]=((-1.5707963267949)+(x211.value)+(((1.5707963267949)*(x212.value)))); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -1036,38 +1172,37 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { IkReal evalcond[4]; -IkReal x291=IKsin(j2); -IkReal x292=IKcos(j2); -IkReal x293=((r00)*(sj0)); -IkReal x294=((r01)*(sj0)); -IkReal x295=((py)*(sj1)); -IkReal x296=((IkReal(0.999999985550000))*(r02)); -IkReal x297=((px)*(sj1)); -IkReal x298=((IkReal(0.999999985550000))*(sj0)); -IkReal x299=((IkReal(0.000169999997543500))*(cj0)); -IkReal x300=((IkReal(0.000169999997543500))*(sj0)); -IkReal x301=((cj1)*(px)); -IkReal x302=((pz)*(sj1)); -IkReal x303=((cj0)*(r00)); -IkReal x304=((cj1)*(pz)); -IkReal x305=((IkReal(1.00000000000000))*(r01)); -IkReal x306=((cj1)*(py)); -IkReal x307=((IkReal(0.999999985550000))*(cj0)); -IkReal x308=((IkReal(0.106000000000000))*(x291)); -IkReal x309=((IkReal(0.106000000000000))*(x292)); -IkReal x310=((IkReal(0.112000000000000))*(x292)); -IkReal x311=((IkReal(0.112000000000000))*(x291)); -IkReal x312=((cj3)*(x311)); -IkReal x313=((sj3)*(x310)); -IkReal x314=((cj3)*(x310)); -IkReal x315=((sj3)*(x311)); -IkReal x316=((x309)+(x314)); -IkReal x317=((x308)+(x313)+(x312)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x298)*(x301)))+(((x299)*(x301)))+(x302)+(x317)+(((x300)*(x306)))+(((x306)*(x307)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x297)*(x299)))+(((IkReal(-1.00000000000000))*(x295)*(x307)))+(((IkReal(-1.00000000000000))*(x295)*(x300)))+(x304)+(x315)+(((IkReal(-1.00000000000000))*(x316)))+(((x297)*(x298)))); -evalcond[2]=((((IkReal(0.999999985550000))*(x294)*(x302)))+(((IkReal(-1.00000000000000))*(x301)*(x305)))+(((IkReal(0.999999985550000))*(x302)*(x303)))+(x317)+(((IkReal(-1.00000000000000))*(cj0)*(x296)*(x297)))+(((r02)*(x295)*(x299)))+(((IkReal(0.000169999997543500))*(x293)*(x302)))+(((IkReal(-1.00000000000000))*(sj0)*(x295)*(x296)))+(((IkReal(-1.00000000000000))*(r01)*(x299)*(x302)))+(((IkReal(-1.00000000000000))*(r02)*(x297)*(x300)))+(((r00)*(x306)))); -evalcond[3]=((((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((cj0)*(x296)*(x301)))+(((IkReal(0.105999998468300))*(x294)))+(((IkReal(0.105999998468300))*(x303)))+(((IkReal(-1.00000000000000))*(x297)*(x305)))+(((IkReal(1.80199997396110e-5))*(x293)))+(((r02)*(x300)*(x301)))+(((r01)*(x299)*(x304)))+(((sj0)*(x296)*(x306)))+(x316)+(((IkReal(-1.00000000000000))*(x315)))+(((IkReal(-0.000169999997543500))*(x293)*(x304)))+(((IkReal(-0.999999985550000))*(x303)*(x304)))+(((r00)*(x295)))+(((IkReal(-1.00000000000000))*(r02)*(x299)*(x306)))+(((IkReal(-0.999999985550000))*(x294)*(x304)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IkReal x213=IKsin(j2); +IkReal x214=IKcos(j2); +IkReal x215=((1.0)*r01); +IkReal x216=(cj1*pz); +IkReal x217=(r00*sj1); +IkReal x218=(cj0*r00); +IkReal x219=(px*sj1); +IkReal x220=(cj0*r02); +IkReal x221=(cj1*py); +IkReal x222=(r02*sj0); +IkReal x223=(r01*sj0); +IkReal x224=(cj1*px); +IkReal x225=(pz*sj1); +IkReal x226=((1.1e-7)*x213); +IkReal x227=((0.106)*x214); +IkReal x228=((1.1e-7)*x214); +IkReal x229=((0.106)*x213); +IkReal x230=((0.112)*x214); +IkReal x231=((0.112)*x213); +IkReal x232=((1.0)*py*sj1); +IkReal x233=(sj3*x231); +IkReal x234=(cj3*x230); +IkReal x235=(cj3*x231); +IkReal x236=(sj3*x230); +IkReal x237=(x227+x226+x234); +IkReal x238=(x229+x236+x235); +evalcond[0]=((-1.1e-7)+(((-1.0)*sj0*x224))+((cj0*x221))+x225+x238+(((-1.0)*x228))); +evalcond[1]=((-0.106)+(((-1.0)*cj0*x232))+(((-1.0)*x237))+x216+x233+((sj0*x219))); +evalcond[2]=((((0.106)*x223))+((py*x217))+(((-1.0)*x216*x218))+(((0.106)*x218))+(((-1.0)*x237))+x233+((x220*x224))+((x221*x222))+(((-1.0)*x215*x219))+(((-1.0)*sj0*x215*x216))); +evalcond[3]=((((-1.1e-7)*x223))+((cj0*pz*x217))+(((-1.0)*x215*x224))+(((-1.1e-7)*x218))+((r00*x221))+((x223*x225))+(((-1.0)*x238))+x228+(((-1.0)*x219*x220))+(((-1.0)*x222*x232))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -1110,30 +1245,53 @@ solutions.AddSolution(vinfos,vfree); } +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j2eval[2]; +sj5=-1.0; +cj5=0; +j5=-1.5707963267949; +j2eval[0]=((965097.402597894)+(((-1.0)*sj3))+(((963636.363636364)*cj3))); +j2eval[1]=IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))); +if( IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j2] + } else { { IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x318=((px)*(sj1)); -IkReal x319=((IkReal(0.281562495931422))*(cj0)); -IkReal x320=((cj1)*(sj0)); -IkReal x321=((IkReal(0.281562495931422))*(py)); -IkReal x322=((IkReal(1749.99997471250))*(sj3)); -IkReal x323=((IkReal(1749.99997471250))*(cj3)); -IkReal x324=((cj0)*(cj1)); -IkReal x325=((IkReal(0.297499995701125))*(sj3)); -IkReal x326=((IkReal(0.297499995701125))*(cj3)); -IkReal x327=((IkReal(1656.25000000000))*(pz)); -IkReal x328=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x329=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x330=((IkReal(1656.24997606719))*(cj0)*(py)); -IkReal x331=((py)*(sj0)*(sj1)); -IkReal x332=((cj0)*(py)*(sj1)); -if( IKabs(((gconst9)*(((((IkReal(-1656.24997606719))*(px)*(x320)))+(((IkReal(-1.00000000000000))*(px)*(x320)*(x323)))+(((cj1)*(x329)))+(((cj1)*(px)*(x319)))+(((IkReal(-1.00000000000000))*(cj0)*(x318)*(x325)))+(((sj0)*(x318)*(x322)))+(((x320)*(x321)))+(((IkReal(1656.24997606719))*(py)*(x324)))+(((IkReal(-185.500000000000))*(sj3)))+(((px)*(x324)*(x326)))+(((py)*(x323)*(x324)))+(((py)*(x320)*(x326)))+(((sj1)*(x328)))+(((sj1)*(x327)))+(((IkReal(-1.00000000000000))*(x322)*(x332)))+(((IkReal(-1.00000000000000))*(x325)*(x331))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(px)*(x320)*(x322)))+(((IkReal(-1.00000000000000))*(cj1)*(x327)))+(((IkReal(-1.00000000000000))*(cj1)*(x328)))+(((IkReal(-1656.24997606719))*(sj0)*(x318)))+(((px)*(x324)*(x325)))+(((py)*(x322)*(x324)))+(((py)*(x320)*(x325)))+(((cj0)*(x318)*(x326)))+(((sj1)*(x329)))+(((x323)*(x332)))+(((IkReal(185.500000000000))*(cj3)))+(((x326)*(x331)))+(((IkReal(-1.00000000000000))*(sj0)*(x318)*(x323)))+(((sj0)*(sj1)*(x321)))+(((x318)*(x319)))+(((sj1)*(x330))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst9)*(((((IkReal(-1656.24997606719))*(px)*(x320)))+(((IkReal(-1.00000000000000))*(px)*(x320)*(x323)))+(((cj1)*(x329)))+(((cj1)*(px)*(x319)))+(((IkReal(-1.00000000000000))*(cj0)*(x318)*(x325)))+(((sj0)*(x318)*(x322)))+(((x320)*(x321)))+(((IkReal(1656.24997606719))*(py)*(x324)))+(((IkReal(-185.500000000000))*(sj3)))+(((px)*(x324)*(x326)))+(((py)*(x323)*(x324)))+(((py)*(x320)*(x326)))+(((sj1)*(x328)))+(((sj1)*(x327)))+(((IkReal(-1.00000000000000))*(x322)*(x332)))+(((IkReal(-1.00000000000000))*(x325)*(x331)))))), ((gconst9)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(px)*(x320)*(x322)))+(((IkReal(-1.00000000000000))*(cj1)*(x327)))+(((IkReal(-1.00000000000000))*(cj1)*(x328)))+(((IkReal(-1656.24997606719))*(sj0)*(x318)))+(((px)*(x324)*(x325)))+(((py)*(x322)*(x324)))+(((py)*(x320)*(x325)))+(((cj0)*(x318)*(x326)))+(((sj1)*(x329)))+(((x323)*(x332)))+(((IkReal(185.500000000000))*(cj3)))+(((x326)*(x331)))+(((IkReal(-1.00000000000000))*(sj0)*(x318)*(x323)))+(((sj0)*(sj1)*(x321)))+(((x318)*(x319)))+(((sj1)*(x330))))))); +IkReal x239=((0.112)*cj1); +IkReal x240=(cj0*py); +IkReal x241=((0.106)*cj1); +IkReal x242=(px*sj0); +IkReal x243=(cj3*pz); +IkReal x244=((1.1e-7)*cj1); +IkReal x245=((0.106)*sj1); +IkReal x246=((1.1e-7)*sj1); +IkReal x247=((0.112)*sj1); +IkReal x248=((0.112)*pz*sj3); +CheckValue x249=IKPowWithIntegerCheck(IKsign(((0.0237800000000121)+(((-2.464e-8)*sj3))+(((0.023744)*cj3)))),-1); +if(!x249.valid){ +continue; +} +CheckValue x250 = IKatan2WithCheck(IkReal(((((-1.0)*pz*x245))+((cj3*x239*x242))+(((1.232e-8)*cj3))+(((-1.0)*pz*sj3*x239))+(((-1.0)*x243*x247))+((x241*x242))+(((-1.0)*sj3*x242*x247))+((x242*x246))+((sj3*x240*x247))+(((-1.0)*x240*x241))+(((-1.0)*x240*x246))+(((0.011872)*sj3))+(((-1.0)*cj3*x239*x240))+((pz*x244)))),IkReal(((-0.0112360000000121)+(((-1.0)*cj3*x240*x247))+(((-1.0)*sj3*x239*x240))+(((-1.0)*x242*x244))+((x240*x244))+(((1.232e-8)*sj3))+((cj3*x242*x247))+((x239*x243))+((sj3*x239*x242))+((x242*x245))+(((-1.0)*x240*x245))+(((-0.011872)*cj3))+((pz*x246))+((pz*x241))+(((-1.0)*pz*sj3*x247)))),IKFAST_ATAN2_MAGTHRESH); +if(!x250.valid){ +continue; +} +j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x249.value)))+(x250.value)); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -1161,38 +1319,37 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { IkReal evalcond[4]; -IkReal x333=IKsin(j2); -IkReal x334=IKcos(j2); -IkReal x335=((r00)*(sj0)); -IkReal x336=((r01)*(sj0)); -IkReal x337=((py)*(sj1)); -IkReal x338=((IkReal(0.999999985550000))*(r02)); -IkReal x339=((px)*(sj1)); -IkReal x340=((IkReal(0.999999985550000))*(sj0)); -IkReal x341=((IkReal(0.000169999997543500))*(cj0)); -IkReal x342=((IkReal(0.000169999997543500))*(sj0)); -IkReal x343=((cj1)*(px)); -IkReal x344=((pz)*(sj1)); -IkReal x345=((cj0)*(r00)); -IkReal x346=((cj1)*(pz)); -IkReal x347=((IkReal(1.00000000000000))*(r01)); -IkReal x348=((cj1)*(py)); -IkReal x349=((IkReal(0.999999985550000))*(cj0)); -IkReal x350=((IkReal(0.106000000000000))*(x333)); -IkReal x351=((IkReal(0.106000000000000))*(x334)); -IkReal x352=((IkReal(0.112000000000000))*(x334)); -IkReal x353=((IkReal(0.112000000000000))*(x333)); -IkReal x354=((cj3)*(x353)); -IkReal x355=((sj3)*(x352)); -IkReal x356=((cj3)*(x352)); -IkReal x357=((sj3)*(x353)); -IkReal x358=((x351)+(x356)); -IkReal x359=((x350)+(x355)+(x354)); -evalcond[0]=((((x348)*(x349)))+(x359)+(x344)+(((IkReal(-1.00000000000000))*(x340)*(x343)))+(((x341)*(x343)))+(((x342)*(x348)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x337)*(x349)))+(((IkReal(-1.00000000000000))*(x337)*(x342)))+(((IkReal(-1.00000000000000))*(x358)))+(x357)+(x346)+(((x339)*(x340)))+(((IkReal(-1.00000000000000))*(x339)*(x341)))); -evalcond[2]=((((IkReal(0.999999985550000))*(x336)*(x344)))+(((IkReal(-1.00000000000000))*(r02)*(x339)*(x342)))+(((r00)*(x348)))+(x359)+(((IkReal(0.000169999997543500))*(x335)*(x344)))+(((IkReal(-1.00000000000000))*(cj0)*(x338)*(x339)))+(((IkReal(0.999999985550000))*(x344)*(x345)))+(((IkReal(-1.00000000000000))*(sj0)*(x337)*(x338)))+(((IkReal(-1.00000000000000))*(x343)*(x347)))+(((IkReal(-1.00000000000000))*(r01)*(x341)*(x344)))+(((r02)*(x337)*(x341)))); -evalcond[3]=((((IkReal(0.105999998468300))*(x336)))+(((r00)*(x337)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(1.80199997396110e-5))*(x335)))+(((IkReal(-1.00000000000000))*(x357)))+(((cj0)*(x338)*(x343)))+(((IkReal(-0.000169999997543500))*(x335)*(x346)))+(((sj0)*(x338)*(x348)))+(((IkReal(0.105999998468300))*(x345)))+(((r02)*(x342)*(x343)))+(x358)+(((IkReal(-0.999999985550000))*(x336)*(x346)))+(((r01)*(x341)*(x346)))+(((IkReal(-0.999999985550000))*(x345)*(x346)))+(((IkReal(-1.00000000000000))*(r02)*(x341)*(x348)))+(((IkReal(-1.00000000000000))*(x339)*(x347)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IkReal x251=IKsin(j2); +IkReal x252=IKcos(j2); +IkReal x253=((1.0)*r01); +IkReal x254=(cj1*pz); +IkReal x255=(r00*sj1); +IkReal x256=(cj0*r00); +IkReal x257=(px*sj1); +IkReal x258=(cj0*r02); +IkReal x259=(cj1*py); +IkReal x260=(r02*sj0); +IkReal x261=(r01*sj0); +IkReal x262=(cj1*px); +IkReal x263=(pz*sj1); +IkReal x264=((1.1e-7)*x251); +IkReal x265=((0.106)*x252); +IkReal x266=((0.106)*x251); +IkReal x267=((1.1e-7)*x252); +IkReal x268=((0.112)*x252); +IkReal x269=((0.112)*x251); +IkReal x270=((1.0)*py*sj1); +IkReal x271=(cj3*x268); +IkReal x272=(sj3*x269); +IkReal x273=(cj3*x269); +IkReal x274=(sj3*x268); +IkReal x275=(x265+x264+x271); +IkReal x276=(x266+x274+x273); +evalcond[0]=((-1.1e-7)+(((-1.0)*sj0*x262))+x263+x276+((cj0*x259))+(((-1.0)*x267))); +evalcond[1]=((-0.106)+((sj0*x257))+x254+x272+(((-1.0)*cj0*x270))+(((-1.0)*x275))); +evalcond[2]=((((0.106)*x256))+((x258*x262))+(((-1.0)*sj0*x253*x254))+((x259*x260))+((py*x255))+(((0.106)*x261))+x275+(((-1.0)*x254*x256))+(((-1.0)*x272))+(((-1.0)*x253*x257))); +evalcond[3]=((((-1.1e-7)*x256))+((r00*x259))+(((-1.1e-7)*x261))+(((-1.0)*x253*x262))+(((-1.0)*x257*x258))+x276+((cj0*pz*x255))+((x261*x263))+(((-1.0)*x267))+(((-1.0)*x260*x270))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -1234,250 +1391,54 @@ solutions.AddSolution(vinfos,vfree); } } -} -} - -} else -{ -if( 1 ) -{ -continue; -} else -{ -} -} -} } - -} else -{ -{ -IkReal j3array[1], cj3array[1], sj3array[1]; -bool j3valid[1]={false}; -_nj3 = 1; -IkReal x360=((sj0)*(sj1)); -IkReal x361=((IkReal(743.750227252771))*(cj0)); -IkReal x362=((pz)*(r00)); -IkReal x363=((cj0)*(r01)); -IkReal x364=((IkReal(463750.141698787))*(cj1)); -IkReal x365=((px)*(r02)); -IkReal x366=((cj1)*(pz)); -IkReal x367=((cj5)*(py)); -IkReal x368=((cj0)*(sj1)); -IkReal x369=((r01)*(sj0)); -IkReal x370=((IkReal(78.8375240887937))*(cj1)); -IkReal x371=((cj5)*(px)); -IkReal x372=((py)*(r02)); -IkReal x373=((IkReal(4375001.33678101))*(sj0)); -if( IKabs(((IkReal(0.00269541778975741))*(((IKabs(cj5) != 0)?((IkReal)1/(cj5)):(IkReal)1.0e30))*(((((x361)*(x365)))+(((IkReal(4375001.33678101))*(cj0)*(x372)))+(((IkReal(4140624.94016797))*(x367)*(x368)))+(((IkReal(703.906239828555))*(x360)*(x367)))+(((IkReal(-1.00000000000000))*(x361)*(x362)))+(((IkReal(-4140625.00000000))*(cj5)*(x366)))+(((IkReal(743.750227252771))*(sj0)*(x372)))+(((x369)*(x370)))+(((IkReal(-1.00000000000000))*(r00)*(sj0)*(x364)))+(((IkReal(19531250.0000000))*(cj5)*(pp)))+(((IkReal(463750.148399976))*(r02)*(sj1)))+(((IkReal(-4375001.33678101))*(pz)*(x363)))+(((cj0)*(r00)*(x370)))+(((IkReal(245000.000000000))*(cj5)))+(((IkReal(-743.750227252771))*(pz)*(x369)))+(((IkReal(-4140624.94016797))*(x360)*(x371)))+(((x363)*(x364)))+(((IkReal(703.906239828555))*(x368)*(x371)))+(((x362)*(x373)))+(((IkReal(-1.00000000000000))*(x365)*(x373))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.528301886792453))+(((IkReal(0.00151785712092411))*(py)*(x360)))+(((IkReal(8.92857129955357))*(py)*(x368)))+(((IkReal(0.00151785712092411))*(px)*(x368)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-8.92857129955357))*(px)*(x360)))+(((IkReal(-8.92857142857143))*(x366))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.00269541778975741))*(((IKabs(cj5) != 0)?((IkReal)1/(cj5)):(IkReal)1.0e30))*(((((x361)*(x365)))+(((IkReal(4375001.33678101))*(cj0)*(x372)))+(((IkReal(4140624.94016797))*(x367)*(x368)))+(((IkReal(703.906239828555))*(x360)*(x367)))+(((IkReal(-1.00000000000000))*(x361)*(x362)))+(((IkReal(-4140625.00000000))*(cj5)*(x366)))+(((IkReal(743.750227252771))*(sj0)*(x372)))+(((x369)*(x370)))+(((IkReal(-1.00000000000000))*(r00)*(sj0)*(x364)))+(((IkReal(19531250.0000000))*(cj5)*(pp)))+(((IkReal(463750.148399976))*(r02)*(sj1)))+(((IkReal(-4375001.33678101))*(pz)*(x363)))+(((cj0)*(r00)*(x370)))+(((IkReal(245000.000000000))*(cj5)))+(((IkReal(-743.750227252771))*(pz)*(x369)))+(((IkReal(-4140624.94016797))*(x360)*(x371)))+(((x363)*(x364)))+(((IkReal(703.906239828555))*(x368)*(x371)))+(((x362)*(x373)))+(((IkReal(-1.00000000000000))*(x365)*(x373)))))))+IKsqr(((IkReal(-0.528301886792453))+(((IkReal(0.00151785712092411))*(py)*(x360)))+(((IkReal(8.92857129955357))*(py)*(x368)))+(((IkReal(0.00151785712092411))*(px)*(x368)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-8.92857129955357))*(px)*(x360)))+(((IkReal(-8.92857142857143))*(x366)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j3array[0]=IKatan2(((IkReal(0.00269541778975741))*(((IKabs(cj5) != 0)?((IkReal)1/(cj5)):(IkReal)1.0e30))*(((((x361)*(x365)))+(((IkReal(4375001.33678101))*(cj0)*(x372)))+(((IkReal(4140624.94016797))*(x367)*(x368)))+(((IkReal(703.906239828555))*(x360)*(x367)))+(((IkReal(-1.00000000000000))*(x361)*(x362)))+(((IkReal(-4140625.00000000))*(cj5)*(x366)))+(((IkReal(743.750227252771))*(sj0)*(x372)))+(((x369)*(x370)))+(((IkReal(-1.00000000000000))*(r00)*(sj0)*(x364)))+(((IkReal(19531250.0000000))*(cj5)*(pp)))+(((IkReal(463750.148399976))*(r02)*(sj1)))+(((IkReal(-4375001.33678101))*(pz)*(x363)))+(((cj0)*(r00)*(x370)))+(((IkReal(245000.000000000))*(cj5)))+(((IkReal(-743.750227252771))*(pz)*(x369)))+(((IkReal(-4140624.94016797))*(x360)*(x371)))+(((x363)*(x364)))+(((IkReal(703.906239828555))*(x368)*(x371)))+(((x362)*(x373)))+(((IkReal(-1.00000000000000))*(x365)*(x373)))))), ((IkReal(-0.528301886792453))+(((IkReal(0.00151785712092411))*(py)*(x360)))+(((IkReal(8.92857129955357))*(py)*(x368)))+(((IkReal(0.00151785712092411))*(px)*(x368)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-8.92857129955357))*(px)*(x360)))+(((IkReal(-8.92857142857143))*(x366))))); -sj3array[0]=IKsin(j3array[0]); -cj3array[0]=IKcos(j3array[0]); -if( j3array[0] > IKPI ) +} while(0); +if( bgotonextstatement ) { - j3array[0]-=IK2PI; -} -else if( j3array[0] < -IKPI ) -{ j3array[0]+=IK2PI; -} -j3valid[0] = true; -for(int ij3 = 0; ij3 < 1; ++ij3) +bool bgotonextstatement = true; +do { -if( !j3valid[ij3] ) +if( 1 ) { - continue; +bgotonextstatement=false; +continue; // branch miss [j2] + } -_ij3[0] = ij3; _ij3[1] = -1; -for(int iij3 = ij3+1; iij3 < 1; ++iij3) +} while(0); +if( bgotonextstatement ) { -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 x374=IKcos(j3); -IkReal x375=IKsin(j3); -IkReal x376=(px)*(px); -IkReal x377=(py)*(py); -IkReal x378=(pz)*(pz); -IkReal x379=((r00)*(sj0)); -IkReal x380=((cj0)*(r00)); -IkReal x381=((py)*(sj1)); -IkReal x382=((cj1)*(pz)); -IkReal x383=((cj0)*(r01)); -IkReal x384=((px)*(py)); -IkReal x385=((r01)*(sj0)); -IkReal x386=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x387=((cj0)*(r02)); -IkReal x388=((IkReal(3.60399994792220e-5))*(cj1)); -IkReal x389=((IkReal(0.105999966080016))*(cj5)); -IkReal x390=((IkReal(0.999999985550000))*(pz)); -IkReal x391=((px)*(sj1)); -IkReal x392=((IkReal(0.106000000000000))*(r02)); -IkReal x393=((IkReal(8.47999728640130e-5))*(cj5)); -IkReal x394=((IkReal(0.000169999997543500))*(pz)); -IkReal x395=((IkReal(0.211999996936600))*(cj1)); -IkReal x396=((IkReal(0.000339999995087000))*(pz)); -IkReal x397=((IkReal(0.105999998468300))*(cj1)); -IkReal x398=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x399=((IkReal(1.99999997110000))*(pz)); -IkReal x400=((IkReal(0.105999998468300))*(sj1)); -IkReal x401=((IkReal(0.999999985550000))*(x377)); -IkReal x402=((IkReal(0.999999985550000))*(x378)); -IkReal x403=((IkReal(0.000169999997543500))*(x376)); -IkReal x404=((IkReal(0.000169999997543500))*(x377)); -IkReal x405=((IkReal(0.000169999997543500))*(x378)); -IkReal x406=((px)*(r02)*(sj0)); -IkReal x407=((IkReal(0.999999985550000))*(x376)); -IkReal x408=((py)*(r02)*(sj0)); -IkReal x409=((IkReal(0.0237440000000000))*(x374)); -evalcond[0]=((IkReal(0.0125440000000000))+(((IkReal(0.211999996936600))*(sj0)*(x391)))+(((IkReal(-3.60399994792220e-5))*(cj0)*(x391)))+(((IkReal(0.212000000000000))*(x382)))+(((IkReal(-3.60399994792220e-5))*(sj0)*(x381)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.211999996936600))*(cj0)*(x381)))+(x409)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x383)*(x400)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-1.00000000000000))*(x380)*(x386)))+(((IkReal(8.95999713280138e-5))*(cj5)))+(((x379)*(x400)))+(((IkReal(-1.00000000000000))*(pz)*(r02)))+(((cj1)*(x392)))+(((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x385)*(x386)))+(((x375)*(x389)))+(((x374)*(x393)))); -evalcond[2]=((((x385)*(x398)))+(((x379)*(x390)))+(((IkReal(0.999999985550000))*(py)*(x387)))+(((x380)*(x398)))+(((IkReal(-1.00000000000000))*(x385)*(x394)))+(((sj1)*(x392)))+(((x374)*(x389)))+(((IkReal(-1.00000000000000))*(x383)*(x390)))+(((IkReal(-0.999999985550000))*(x406)))+(((IkReal(0.000169999997543500))*(x408)))+(((IkReal(-1.00000000000000))*(x380)*(x394)))+(((IkReal(0.000169999997543500))*(px)*(x387)))+(((IkReal(0.111999964160017))*(cj5)))+(((IkReal(-1.00000000000000))*(x379)*(x397)))+(((IkReal(-1.00000000000000))*(x375)*(x393)))+(((x383)*(x397)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x383)*(x404)))+(((x380)*(x407)))+(((IkReal(-0.0112359998376398))*(x380)))+(((IkReal(-0.0112359998376398))*(x385)))+(((IkReal(-1.00000000000000))*(sj5)*(x409)))+(((IkReal(-1.00000000000000))*(x380)*(x402)))+(((IkReal(-1.00000000000000))*(x380)*(x401)))+(((IkReal(-0.000339999995087000))*(x380)*(x384)))+(((IkReal(1.99999997110000))*(x379)*(x384)))+(((IkReal(-0.212000000000000))*(r00)*(x381)))+(((IkReal(3.60399994792220e-5))*(x379)*(x382)))+(((IkReal(0.211999996936600))*(x380)*(x382)))+(((IkReal(-1.00000000000000))*(x379)*(x404)))+(((IkReal(-1.00000000000000))*(x379)*(x405)))+(((IkReal(-1.00000000000000))*(x395)*(x408)))+(((IkReal(-3.60399994792220e-5))*(x382)*(x383)))+(((IkReal(1.99999997110000))*(x383)*(x384)))+(((px)*(x387)*(x399)))+(((IkReal(-0.0237800000000000))*(sj5)))+(((x379)*(x403)))+(((IkReal(-1.00000000000000))*(py)*(x387)*(x396)))+(((IkReal(-1.00000000000000))*(px)*(x387)*(x395)))+(((x383)*(x405)))+(((x383)*(x403)))+(((IkReal(0.211999996936600))*(x382)*(x385)))+(((x399)*(x408)))+(((py)*(x387)*(x388)))+(((IkReal(0.212000000000000))*(r01)*(x391)))+(((x385)*(x401)))+(((IkReal(-1.00000000000000))*(x385)*(x407)))+(((IkReal(-1.00000000000000))*(x385)*(x402)))+(((IkReal(-1.00000000000000))*(x388)*(x406)))+(((IkReal(0.000339999995087000))*(x384)*(x385)))+(((x396)*(x406)))+(((IkReal(-1.91011997239877e-6))*(x379)))+(((IkReal(1.91011997239877e-6))*(x383)))); -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 gconst1; -gconst1=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst2; -IkReal x410=((IkReal(13.9999955200021))*(cj5)); -gconst2=IKsign(((((IkReal(0.0105999966080016))*(cj5)*(sj3)))+(((IkReal(-13.2499957600020))*(cj3)*(cj5)))+(((IkReal(-1.00000000000000))*(x410)*((sj3)*(sj3))))+(((IkReal(-1.00000000000000))*(x410)*((cj3)*(cj3)))))); -IkReal x411=((IkReal(1320.75471698113))*(cj5)); -dummyeval[0]=((((IkReal(-1.00000000000000))*(x411)*((cj3)*(cj3))))+(((cj5)*(sj3)))+(((IkReal(-1.00000000000000))*(x411)*((sj3)*(sj3))))+(((IkReal(-1250.00000000000))*(cj3)*(cj5)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x412=(px)*(px); -IkReal x413=(py)*(py); -IkReal x414=(pz)*(pz); -IkReal x415=((IkReal(0.0237440000000000))*(cj3)); -IkReal x416=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x417=((cj0)*(r00)); -IkReal x418=((r01)*(sj0)); -IkReal x419=((IkReal(0.000169999997543500))*(sj1)); -IkReal x420=((IkReal(0.000339999995087000))*(py)); -IkReal x421=((pz)*(sj1)); -IkReal x422=((r02)*(sj0)); -IkReal x423=((cj0)*(px)); -IkReal x424=((cj1)*(r01)); -IkReal x425=((IkReal(0.000169999997543500))*(cj0)); -IkReal x426=((IkReal(3.60399994792220e-5))*(sj1)); -IkReal x427=((py)*(r01)); -IkReal x428=((r00)*(sj0)); -IkReal x429=((IkReal(0.000169999997543500))*(pz)); -IkReal x430=((IkReal(0.105999998468300))*(cj0)); -IkReal x431=((px)*(r00)); -IkReal x432=((IkReal(1.99999997110000))*(pz)); -IkReal x433=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x434=((r01)*(sj1)); -IkReal x435=((cj1)*(r02)); -IkReal x436=((r02)*(sj1)); -IkReal x437=((IkReal(0.999999985550000))*(py)); -IkReal x438=((IkReal(0.999999985550000))*(cj0)); -IkReal x439=((IkReal(0.211999996936600))*(px)); -IkReal x440=((IkReal(0.0112359998376398))*(cj0)); -IkReal x441=((cj0)*(r02)); -IkReal x442=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x443=((IkReal(0.211999996936600))*(py)); -IkReal x444=((cj1)*(px)); -IkReal x445=((IkReal(0.000339999995087000))*(pz)); -IkReal x446=((IkReal(0.999999985550000))*(cj1)); -IkReal x447=((IkReal(0.000169999997543500))*(cj1)); -IkReal x448=((cj0)*(py)); -IkReal x449=((pz)*(r02)); -IkReal x450=((IkReal(0.211999996936600))*(pz)); -IkReal x451=((cj1)*(pz)); -IkReal x452=((IkReal(0.999999985550000))*(sj1)); -IkReal x453=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x454=((cj0)*(r01)); -IkReal x455=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x456=((IkReal(0.999999985550000))*(x414)); -IkReal x457=((IkReal(0.000169999997543500))*(x413)); -IkReal x458=((IkReal(0.999999985550000))*(x412)); -IkReal x459=((IkReal(0.000169999997543500))*(x414)); -IkReal x460=((IkReal(1.99999997110000))*(px)*(py)); -IkReal x461=((IkReal(0.000169999997543500))*(x412)); -IkReal x462=((IkReal(2.00000000000000))*(x414)); -IkReal x463=((IkReal(0.999999985550000))*(x413)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(sj0)*(x437)))+(((py)*(x425)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((IkReal(-0.999999985550000))*(x423)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-0.000169999997543500))*(x428)))+(((r01)*(x425)))+(((IkReal(-0.999999985550000))*(x417)))+(((IkReal(-0.999999985550000))*(x418)))); -evalcond[3]=((IkReal(0.0125440000000000))+(((IkReal(-1.00000000000000))*(pp)))+(((sj0)*(sj1)*(x439)))+(x415)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x443)))+(((IkReal(-1.00000000000000))*(x423)*(x426)))+(((IkReal(0.212000000000000))*(x451)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x426)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x430)*(x434)))+(((IkReal(-1.00000000000000))*(x427)))+(((IkReal(-1.00000000000000))*(x431)))+(((IkReal(0.105999998468300))*(sj1)*(x428)))+(((IkReal(-1.00000000000000))*(x449)))+(((IkReal(0.106000000000000))*(x435)))+(((IkReal(-1.00000000000000))*(x417)*(x442)))+(((IkReal(-1.00000000000000))*(x418)*(x442)))); -evalcond[5]=((((x424)*(x438)))+(x436)+(((x418)*(x447)))+(((x417)*(x447)))+(((IkReal(-1.00000000000000))*(x428)*(x446)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x417)*(x419)))+(((IkReal(-1.00000000000000))*(x418)*(x419)))+(((x428)*(x452)))+(((IkReal(-1.00000000000000))*(x434)*(x438)))+(x435)); -evalcond[7]=((((x424)*(x430)))+(((IkReal(0.000169999997543500))*(py)*(x422)))+(((x418)*(x453)))+(((x417)*(x453)))+(((IkReal(0.106000000000000))*(x436)))+(((IkReal(-1.00000000000000))*(pz)*(r01)*(x438)))+(((IkReal(0.999999985550000))*(pz)*(x428)))+(((IkReal(-0.105999998468300))*(cj1)*(x428)))+(((IkReal(-1.00000000000000))*(x417)*(x429)))+(((IkReal(-1.00000000000000))*(x418)*(x429)))+(((IkReal(0.000169999997543500))*(r02)*(x423)))+(((IkReal(-0.999999985550000))*(px)*(x422)))+(((x437)*(x441)))); -evalcond[8]=((IkReal(-0.0237800000000000))+(((r01)*(x414)*(x425)))+(((IkReal(-1.00000000000000))*(cj1)*(x422)*(x443)))+(((IkReal(-1.91011997239877e-6))*(x428)))+(((IkReal(-0.211999996936600))*(x423)*(x435)))+(((IkReal(-1.00000000000000))*(cj0)*(x416)*(x424)))+(((px)*(x418)*(x420)))+(((IkReal(-1.00000000000000))*(pz)*(x420)*(x441)))+(((x428)*(x460)))+(((x428)*(x461)))+(((cj1)*(x417)*(x450)))+(((r01)*(x412)*(x425)))+(((IkReal(1.91011997239877e-6))*(x454)))+(((x418)*(x463)))+(((IkReal(1.99999997110000))*(x423)*(x427)))+(((IkReal(3.60399994792220e-5))*(x435)*(x448)))+(((x417)*(x458)))+(((IkReal(-1.00000000000000))*(px)*(x417)*(x420)))+(((IkReal(-1.00000000000000))*(x417)*(x456)))+(((IkReal(-1.00000000000000))*(x428)*(x457)))+(((IkReal(-1.00000000000000))*(x428)*(x459)))+(((cj1)*(x418)*(x450)))+(((IkReal(-1.00000000000000))*(r01)*(x413)*(x425)))+(((IkReal(-1.00000000000000))*(x417)*(x463)))+(((cj1)*(x416)*(x428)))+(((IkReal(-1.00000000000000))*(x418)*(x458)))+(((IkReal(-1.00000000000000))*(x418)*(x456)))+(((IkReal(-3.60399994792220e-5))*(x422)*(x444)))+(((IkReal(-0.0112359998376398))*(x418)))+(((IkReal(-0.0112359998376398))*(x417)))+(((IkReal(0.212000000000000))*(px)*(x434)))+(((px)*(x422)*(x445)))+(((IkReal(-0.212000000000000))*(py)*(r00)*(sj1)))+(((py)*(x422)*(x432)))+(((IkReal(-1.00000000000000))*(x415)))+(((r02)*(x423)*(x432)))); -evalcond[9]=((((x434)*(x440)))+(((px)*(sj1)*(x420)*(x428)))+(((IkReal(-1.00000000000000))*(x435)*(x462)))+(((x418)*(x455)))+(((x417)*(x455)))+(((IkReal(-1.00000000000000))*(x412)*(x428)*(x452)))+(((pp)*(x435)))+(((x412)*(x417)*(x419)))+(((IkReal(-1.00000000000000))*(x412)*(x418)*(x419)))+(((IkReal(-1.00000000000000))*(x412)*(x434)*(x438)))+(((IkReal(0.000339999995087000))*(r02)*(x421)*(x423)))+(((IkReal(-1.00000000000000))*(x414)*(x418)*(x419)))+(((x420)*(x421)*(x422)))+(((x413)*(x418)*(x419)))+(((IkReal(-0.0112359998376398))*(sj1)*(x428)))+(((IkReal(0.212000000000000))*(x427)))+(((IkReal(-1.00000000000000))*(x413)*(x417)*(x419)))+(((IkReal(-1.00000000000000))*(x414)*(x417)*(x419)))+(((x420)*(x423)*(x434)))+(((IkReal(0.212000000000000))*(x449)))+(((IkReal(-2.00000000000000))*(x431)*(x451)))+(((IkReal(0.212000000000000))*(x431)))+(((IkReal(-1.00000000000000))*(sj1)*(x418)*(x460)))+(((IkReal(1.99999997110000))*(py)*(x421)*(x441)))+(((IkReal(-2.00000000000000))*(py)*(pz)*(x424)))+(((IkReal(-0.0112360000000000))*(x435)))+(((sj1)*(x417)*(x460)))+(((x413)*(x428)*(x452)))+(((x414)*(x428)*(x452)))+(((IkReal(-1.99999997110000))*(px)*(x421)*(x422)))+(((IkReal(-1.00000000000000))*(x414)*(x434)*(x438)))+(((x413)*(x434)*(x438)))); -evalcond[10]=((((x428)*(x450)))+(((IkReal(1.99999997110000))*(py)*(x418)*(x444)))+(((x424)*(x440)))+(((IkReal(-1.99999997110000))*(py)*(x417)*(x444)))+(((IkReal(3.60399994792220e-5))*(py)*(x422)))+(((x422)*(x432)*(x444)))+(((x414)*(x417)*(x447)))+(((IkReal(-1.00000000000000))*(x413)*(x418)*(x447)))+(((IkReal(-1.00000000000000))*(x420)*(x428)*(x444)))+(((IkReal(-1.00000000000000))*(x420)*(x422)*(x451)))+(((pp)*(x436)))+(((x413)*(x417)*(x447)))+(((x412)*(x424)*(x438)))+(((IkReal(-1.00000000000000))*(x412)*(x417)*(x447)))+(((IkReal(-1.00000000000000))*(x423)*(x435)*(x445)))+(((IkReal(3.60399994792220e-5))*(r02)*(x423)))+(((IkReal(-2.00000000000000))*(x421)*(x427)))+(((IkReal(-1.00000000000000))*(x413)*(x424)*(x438)))+(((x412)*(x418)*(x447)))+(((x418)*(x433)))+(((x441)*(x443)))+(((IkReal(-1.00000000000000))*(x416)*(x418)))+(((IkReal(-1.00000000000000))*(x416)*(x417)))+(((IkReal(-1.00000000000000))*(x436)*(x462)))+(((IkReal(-2.00000000000000))*(x421)*(x431)))+(((IkReal(-1.00000000000000))*(x450)*(x454)))+(((IkReal(-1.00000000000000))*(x420)*(x423)*(x424)))+(((IkReal(-1.00000000000000))*(x432)*(x435)*(x448)))+(((IkReal(0.0112360000000000))*(x436)))+(((x412)*(x428)*(x446)))+(((IkReal(-1.00000000000000))*(x414)*(x428)*(x446)))+(((IkReal(-1.00000000000000))*(x422)*(x439)))+(((x414)*(x424)*(x438)))+(((x414)*(x418)*(x447)))+(((IkReal(-1.00000000000000))*(x413)*(x428)*(x446)))+(((IkReal(-0.0112359998376398))*(cj1)*(x428)))+(((x417)*(x433)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst3; -gconst3=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst4; -gconst4=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - } else { { IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x464=((py)*(r02)); -IkReal x465=((IkReal(1750.00000000000))*(cj1)); -IkReal x466=((px)*(sj3)); -IkReal x467=((cj3)*(pz)); -IkReal x468=((cj0)*(sj1)); -IkReal x469=((IkReal(1749.99997471250))*(r02)); -IkReal x470=((px)*(r01)); -IkReal x471=((r00)*(sj3)); -IkReal x472=((pz)*(sj3)); -IkReal x473=((pz)*(r01)); -IkReal x474=((IkReal(0.297499995701125))*(sj3)); -IkReal x475=((IkReal(1656.25000000000))*(cj1)); -IkReal x476=((IkReal(0.297499995701125))*(cj3)); -IkReal x477=((pz)*(r00)); -IkReal x478=((cj3)*(px)); -IkReal x479=((cj3)*(py)); -IkReal x480=((px)*(r02)); -IkReal x481=((IkReal(0.297499995701125))*(r01)); -IkReal x482=((sj0)*(sj1)); -IkReal x483=((IkReal(0.281562495931422))*(x468)); -IkReal x484=((IkReal(1656.24997606719))*(x482)); -IkReal x485=((IkReal(1749.99997471250))*(x482)); -IkReal x486=((py)*(x482)); -IkReal x487=((IkReal(0.281562495931422))*(x482)); -if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(py)*(r00)*(x475)))+(((IkReal(-1749.99997471250))*(r00)*(x467)*(x468)))+(((IkReal(-0.297499995701125))*(r00)*(x467)*(x482)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x468)))+(((x476)*(x480)*(x482)))+(((x470)*(x475)))+(((IkReal(-1.00000000000000))*(x464)*(x483)))+(((IkReal(-1.00000000000000))*(x474)*(x486)))+(((IkReal(-185.500000000000))*(sj3)))+(((x465)*(x472)))+(((x467)*(x468)*(x481)))+(((x468)*(x469)*(x478)))+(((x464)*(x484)))+(((IkReal(-1.00000000000000))*(r00)*(x465)*(x479)))+(((IkReal(-1656.24997606719))*(x468)*(x477)))+(((IkReal(1656.24997606719))*(x468)*(x480)))+(((x480)*(x487)))+(((IkReal(-1.00000000000000))*(x464)*(x468)*(x476)))+(((IkReal(-1.00000000000000))*(r01)*(x467)*(x485)))+(((IkReal(-1.00000000000000))*(x477)*(x487)))+(((IkReal(-0.297499995701125))*(x466)*(x468)))+(((cj3)*(x465)*(x470)))+(((x466)*(x485)))+(((cj3)*(x464)*(x485)))+(((x473)*(x483)))+(((IkReal(-1.00000000000000))*(x473)*(x484))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(175.562500000000))+(((x466)*(x468)*(x469)))+(((IkReal(-1.00000000000000))*(x465)*(x467)))+(((IkReal(-1.00000000000000))*(py)*(x465)*(x471)))+(((px)*(x468)*(x476)))+(((x476)*(x486)))+(((IkReal(1749.99997471250))*(x468)*(x479)))+(((IkReal(-0.297499995701125))*(pz)*(x471)*(x482)))+(((x468)*(x472)*(x481)))+(((IkReal(0.297499995701125))*(r02)*(x466)*(x482)))+(((IkReal(-1.00000000000000))*(pz)*(x475)))+(((px)*(x483)))+(((IkReal(-1.00000000000000))*(r01)*(x472)*(x485)))+(((IkReal(1656.24997606719))*(py)*(x468)))+(((IkReal(-1.00000000000000))*(px)*(x484)))+(((IkReal(-1749.99997471250))*(pz)*(x468)*(x471)))+(((IkReal(-1.00000000000000))*(x464)*(x468)*(x474)))+(((sj3)*(x464)*(x485)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(0.281562495931422))*(x486)))+(((r01)*(x465)*(x466)))+(((IkReal(-1.00000000000000))*(x478)*(x485))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(py)*(r00)*(x475)))+(((IkReal(-1749.99997471250))*(r00)*(x467)*(x468)))+(((IkReal(-0.297499995701125))*(r00)*(x467)*(x482)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x468)))+(((x476)*(x480)*(x482)))+(((x470)*(x475)))+(((IkReal(-1.00000000000000))*(x464)*(x483)))+(((IkReal(-1.00000000000000))*(x474)*(x486)))+(((IkReal(-185.500000000000))*(sj3)))+(((x465)*(x472)))+(((x467)*(x468)*(x481)))+(((x468)*(x469)*(x478)))+(((x464)*(x484)))+(((IkReal(-1.00000000000000))*(r00)*(x465)*(x479)))+(((IkReal(-1656.24997606719))*(x468)*(x477)))+(((IkReal(1656.24997606719))*(x468)*(x480)))+(((x480)*(x487)))+(((IkReal(-1.00000000000000))*(x464)*(x468)*(x476)))+(((IkReal(-1.00000000000000))*(r01)*(x467)*(x485)))+(((IkReal(-1.00000000000000))*(x477)*(x487)))+(((IkReal(-0.297499995701125))*(x466)*(x468)))+(((cj3)*(x465)*(x470)))+(((x466)*(x485)))+(((cj3)*(x464)*(x485)))+(((x473)*(x483)))+(((IkReal(-1.00000000000000))*(x473)*(x484)))))), ((gconst4)*(((IkReal(175.562500000000))+(((x466)*(x468)*(x469)))+(((IkReal(-1.00000000000000))*(x465)*(x467)))+(((IkReal(-1.00000000000000))*(py)*(x465)*(x471)))+(((px)*(x468)*(x476)))+(((x476)*(x486)))+(((IkReal(1749.99997471250))*(x468)*(x479)))+(((IkReal(-0.297499995701125))*(pz)*(x471)*(x482)))+(((x468)*(x472)*(x481)))+(((IkReal(0.297499995701125))*(r02)*(x466)*(x482)))+(((IkReal(-1.00000000000000))*(pz)*(x475)))+(((px)*(x483)))+(((IkReal(-1.00000000000000))*(r01)*(x472)*(x485)))+(((IkReal(1656.24997606719))*(py)*(x468)))+(((IkReal(-1.00000000000000))*(px)*(x484)))+(((IkReal(-1749.99997471250))*(pz)*(x468)*(x471)))+(((IkReal(-1.00000000000000))*(x464)*(x468)*(x474)))+(((sj3)*(x464)*(x485)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(0.281562495931422))*(x486)))+(((r01)*(x465)*(x466)))+(((IkReal(-1.00000000000000))*(x478)*(x485))))))); +IkReal x277=((1.1e-7)*cj1); +IkReal x278=(cj0*r01); +IkReal x279=(cj5*sj3); +IkReal x280=(cj3*cj5); +IkReal x281=(cj1*sj0); +IkReal x282=((0.112)*cj3); +IkReal x283=(r02*sj1); +IkReal x284=((0.112)*sj3); +IkReal x285=((1.1e-7)*x279); +IkReal x286=((1.0)*pz*sj1); +IkReal x287=((1.0)*cj0*cj1*py); +CheckValue x288 = IKatan2WithCheck(IkReal(((((-1.0)*r00*sj0*x277))+(((1.1e-7)*x283))+(((1.1e-7)*x280))+((r00*x281*x284))+(((-1.0)*x280*x286))+(((-1.0)*x280*x287))+((px*x280*x281))+((x277*x278))+(((-1.0)*cj1*x278*x284))+(((-1.0)*x283*x284)))),IkReal((((cj1*x278*x282))+(((0.106)*x283))+(((0.106)*cj1*x278))+((px*x279*x281))+(((-0.106)*r00*x281))+(((-1.0)*r00*x281*x282))+x285+(((-1.0)*x279*x287))+(((-1.0)*x279*x286))+((x282*x283)))),IKFAST_ATAN2_MAGTHRESH); +if(!x288.valid){ +continue; +} +CheckValue x289=IKPowWithIntegerCheck(IKsign(((((0.112)*cj5))+(((0.106)*x280))+(((-1.0)*x285)))),-1); +if(!x289.valid){ +continue; +} +j2array[0]=((-1.5707963267949)+(x288.value)+(((1.5707963267949)*(x289.value)))); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -1504,39 +1465,65 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES } j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { -IkReal evalcond[4]; -IkReal x488=IKsin(j2); -IkReal x489=IKcos(j2); -IkReal x490=((r00)*(sj0)); -IkReal x491=((r01)*(sj0)); -IkReal x492=((py)*(sj1)); -IkReal x493=((IkReal(0.999999985550000))*(r02)); -IkReal x494=((px)*(sj1)); -IkReal x495=((IkReal(0.999999985550000))*(sj0)); -IkReal x496=((IkReal(0.000169999997543500))*(cj0)); -IkReal x497=((IkReal(0.000169999997543500))*(sj0)); -IkReal x498=((cj1)*(px)); -IkReal x499=((pz)*(sj1)); -IkReal x500=((cj0)*(r00)); -IkReal x501=((cj1)*(pz)); -IkReal x502=((IkReal(1.00000000000000))*(r01)); -IkReal x503=((cj1)*(py)); -IkReal x504=((IkReal(0.999999985550000))*(cj0)); -IkReal x505=((IkReal(0.106000000000000))*(x488)); -IkReal x506=((IkReal(0.106000000000000))*(x489)); -IkReal x507=((IkReal(0.112000000000000))*(x489)); -IkReal x508=((IkReal(0.112000000000000))*(x488)); -IkReal x509=((cj3)*(x508)); -IkReal x510=((sj3)*(x507)); -IkReal x511=((sj3)*(x508)); -IkReal x512=((cj3)*(x507)); -IkReal x513=((x506)+(x512)); -IkReal x514=((x509)+(x505)+(x510)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x495)*(x498)))+(((x497)*(x503)))+(((x496)*(x498)))+(x499)+(x514)+(((x503)*(x504)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x494)*(x496)))+(((x494)*(x495)))+(x501)+(x511)+(((IkReal(-1.00000000000000))*(x492)*(x504)))+(((IkReal(-1.00000000000000))*(x513)))+(((IkReal(-1.00000000000000))*(x492)*(x497)))); -evalcond[2]=((((IkReal(0.999999985550000))*(x499)*(x500)))+(((IkReal(-1.00000000000000))*(sj0)*(x492)*(x493)))+(((IkReal(0.999999985550000))*(x491)*(x499)))+(((r02)*(x492)*(x496)))+(((IkReal(-1.00000000000000))*(cj0)*(x493)*(x494)))+(((IkReal(0.000169999997543500))*(x490)*(x499)))+(((r00)*(x503)))+(((IkReal(-1.00000000000000))*(r01)*(x496)*(x499)))+(((IkReal(-1.00000000000000))*(x514)))+(((IkReal(-1.00000000000000))*(x498)*(x502)))+(((IkReal(-1.00000000000000))*(r02)*(x494)*(x497)))); -evalcond[3]=((((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(1.80199997396110e-5))*(x490)))+(((sj0)*(x493)*(x503)))+(((IkReal(-0.999999985550000))*(x500)*(x501)))+(((IkReal(-1.00000000000000))*(x494)*(x502)))+(((IkReal(-0.999999985550000))*(x491)*(x501)))+(x511)+(((IkReal(0.105999998468300))*(x500)))+(((IkReal(-0.000169999997543500))*(x490)*(x501)))+(((r00)*(x492)))+(((IkReal(-1.00000000000000))*(r02)*(x496)*(x503)))+(((IkReal(-1.00000000000000))*(x513)))+(((r02)*(x497)*(x498)))+(((cj0)*(x493)*(x498)))+(((r01)*(x496)*(x501)))+(((IkReal(0.105999998468300))*(x491)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IkReal evalcond[8]; +IkReal x290=IKcos(j2); +IkReal x291=IKsin(j2); +IkReal x292=px*px; +IkReal x293=pz*pz; +IkReal x294=py*py; +IkReal x295=(py*sj1); +IkReal x296=((2.0)*r01); +IkReal x297=(px*sj0); +IkReal x298=(pz*r00); +IkReal x299=(cj0*sj1); +IkReal x300=((0.0237799999999879)*cj3); +IkReal x301=(cj0*r01); +IkReal x302=(px*r00); +IkReal x303=((0.212)*pz); +IkReal x304=((1.0)*px); +IkReal x305=((2.332e-8)*cj5); +IkReal x306=(cj1*r01); +IkReal x307=(cj1*r02); +IkReal x308=((2.2e-7)*r02); +IkReal x309=(r01*sj0); +IkReal x310=(r00*sj0); +IkReal x311=((1.0)*cj1); +IkReal x312=((2.0)*pz); +IkReal x313=(cj0*r00); +IkReal x314=((0.112)*cj3); +IkReal x315=(cj1*py); +IkReal x316=((1.0)*cj3); +IkReal x317=((2.0)*cj0); +IkReal x318=(cj0*py); +IkReal x319=(r02*sj1); +IkReal x320=((2.332e-8)*cj1); +IkReal x321=((1.0)*r01); +IkReal x322=(pz*sj1); +IkReal x323=((1.0)*sj0); +IkReal x324=(py*r01); +IkReal x325=((0.212)*r02); +IkReal x326=((0.0112359999999879)*cj1); +IkReal x327=((2.0)*px); +IkReal x328=(sj3*x290); +IkReal x329=(cj5*x290); +IkReal x330=(cj1*x292); +IkReal x331=((1.0)*x292); +IkReal x332=(cj5*x291); +IkReal x333=(sj1*x294); +IkReal x334=(sj5*x291); +IkReal x335=(sj3*x291); +IkReal x336=((0.106)*x290); +IkReal x337=((1.1e-7)*x290); +IkReal x338=((1.0)*x293); +evalcond[0]=(((cj1*x301))+(((-1.0)*x310*x311))+((sj3*x332))+x319+(((-1.0)*x316*x329))); +evalcond[1]=((((-1.0)*x299*x321))+(((-1.0)*x316*x332))+((sj1*x310))+x307+(((-1.0)*cj5*x328))); +evalcond[2]=((-1.1e-7)+(((-1.0)*x297*x311))+x322+(((0.112)*x328))+(((0.106)*x291))+((x291*x314))+(((-1.0)*x337))+((cj0*x315))); +evalcond[3]=((-0.106)+(((-1.0)*cj0*x295))+(((-1.0)*x290*x314))+((sj1*x297))+((cj1*pz))+(((0.112)*x335))+(((-1.1e-7)*x291))+(((-1.0)*x336))); +evalcond[4]=((((-1.0)*sj5*x290*x314))+(((-1.0)*sj5*x336))+(((-1.1e-7)*x334))+(((0.106)*x309))+((cj0*px*x307))+(((0.112)*sj3*x334))+(((-1.0)*cj0*x298*x311))+(((0.106)*x313))+((r00*x295))+(((-1.0)*pz*x306*x323))+(((-1.0)*r01*sj1*x304))+((py*sj0*x307))); +evalcond[5]=((((-1.0)*r02*x295*x323))+(((-1.1e-7)*x309))+((sj5*x337))+(((-0.106)*x334))+(((-1.1e-7)*x313))+((x298*x299))+((r00*x315))+((x309*x322))+(((-1.0)*r02*x299*x304))+(((-1.0)*x304*x306))+(((-1.0)*x314*x334))+(((-0.112)*sj5*x328))); +evalcond[6]=(((x294*x319))+((x318*x325))+((x301*x326))+(((-1.0)*x301*x303))+(((0.0112359999999879)*x319))+(((-2.464e-8)*x332))+(((-1.0)*x302*x315*x317))+(((-1.0)*x294*x310*x311))+(((-2.332e-8)*sj1*x310))+(((2.2e-7)*x324))+(((-1.0)*x294*x301*x311))+(((-1.0)*x319*x338))+(((-1.0)*cj3*x291*x305))+((x296*x297*x315))+(((-1.0)*pz*x295*x296))+((x305*x328))+(((-0.023744)*x329))+(((-1.0)*x307*x312*x318))+(((-1.0)*x300*x329))+((x297*x307*x312))+(((0.212)*sj0*x298))+(((0.0013080000000121)*sj3*x332))+((cj1*x293*x301))+(((-1.0)*x293*x310*x311))+(((2.2e-7)*x302))+(((2.332e-8)*r01*x299))+(((-1.0)*sj1*x298*x327))+(((-1.0)*x310*x326))+(((-1.0)*x297*x325))+((x292*x319))+(((-2.332e-8)*x307))+((x301*x330))+((x310*x330))+((pz*x308))); +evalcond[7]=((((-1.0)*x300*x332))+(((-1.0)*x295*x296*x297))+(((2.2e-7)*pz*x301))+((x310*x320))+((x292*x307))+(((-1.0)*sj1*x310*x331))+(((0.212)*x302))+(((-0.023744)*x332))+(((-2.332e-8)*x319))+((r02*x303))+((cj0*r02*x295*x312))+(((-1.0)*x293*x299*x321))+(((-1.0)*x308*x318))+((x294*x307))+((x305*x335))+((x295*x302*x317))+(((-1.0)*x301*x320))+((x297*x308))+(((-1.0)*x307*x338))+(((-0.0112359999999879)*sj1*x310))+(((0.0112359999999879)*r01*x299))+((sj1*x293*x310))+(((-0.0112359999999879)*x307))+((cj3*x290*x305))+(((-1.0)*pz*x296*x315))+(((0.212)*x324))+((r01*x294*x299))+(((-2.2e-7)*sj0*x298))+(((-1.0)*x292*x299*x321))+(((-1.0)*x297*x312*x319))+((x310*x333))+(((2.464e-8)*x329))+(((-0.0013080000000121)*cj5*x328))+(((-1.0)*cj1*x298*x327))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -1585,24 +1572,29 @@ solutions.AddSolution(vinfos,vfree); IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x515=((px)*(sj1)); -IkReal x516=((IkReal(0.281562495931422))*(cj0)); -IkReal x517=((cj1)*(sj0)); -IkReal x518=((IkReal(0.281562495931422))*(py)); -IkReal x519=((IkReal(1749.99997471250))*(sj3)); -IkReal x520=((IkReal(1749.99997471250))*(cj3)); -IkReal x521=((cj0)*(cj1)); -IkReal x522=((IkReal(0.297499995701125))*(sj3)); -IkReal x523=((IkReal(0.297499995701125))*(cj3)); -IkReal x524=((IkReal(1656.25000000000))*(pz)); -IkReal x525=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x526=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x527=((IkReal(1656.24997606719))*(cj0)*(py)); -IkReal x528=((py)*(sj0)*(sj1)); -IkReal x529=((cj0)*(py)*(sj1)); -if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(px)*(x517)*(x520)))+(((sj1)*(x525)))+(((sj1)*(x524)))+(((py)*(x517)*(x523)))+(((IkReal(1656.24997606719))*(py)*(x521)))+(((IkReal(-1656.24997606719))*(px)*(x517)))+(((IkReal(-1.00000000000000))*(cj0)*(x515)*(x522)))+(((sj0)*(x515)*(x519)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj1)*(px)*(x516)))+(((x517)*(x518)))+(((cj1)*(x526)))+(((IkReal(-1.00000000000000))*(x522)*(x528)))+(((py)*(x520)*(x521)))+(((px)*(x521)*(x523)))+(((IkReal(-1.00000000000000))*(x519)*(x529))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(175.562500000000))+(((sj0)*(sj1)*(x518)))+(((cj0)*(x515)*(x523)))+(((sj1)*(x527)))+(((sj1)*(x526)))+(((py)*(x517)*(x522)))+(((IkReal(-1.00000000000000))*(cj1)*(x525)))+(((IkReal(-1.00000000000000))*(cj1)*(x524)))+(((py)*(x519)*(x521)))+(((x520)*(x529)))+(((x515)*(x516)))+(((x523)*(x528)))+(((IkReal(-1656.24997606719))*(sj0)*(x515)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj0)*(x515)*(x520)))+(((px)*(x521)*(x522)))+(((IkReal(-1.00000000000000))*(px)*(x517)*(x519))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(px)*(x517)*(x520)))+(((sj1)*(x525)))+(((sj1)*(x524)))+(((py)*(x517)*(x523)))+(((IkReal(1656.24997606719))*(py)*(x521)))+(((IkReal(-1656.24997606719))*(px)*(x517)))+(((IkReal(-1.00000000000000))*(cj0)*(x515)*(x522)))+(((sj0)*(x515)*(x519)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj1)*(px)*(x516)))+(((x517)*(x518)))+(((cj1)*(x526)))+(((IkReal(-1.00000000000000))*(x522)*(x528)))+(((py)*(x520)*(x521)))+(((px)*(x521)*(x523)))+(((IkReal(-1.00000000000000))*(x519)*(x529)))))), ((gconst3)*(((IkReal(175.562500000000))+(((sj0)*(sj1)*(x518)))+(((cj0)*(x515)*(x523)))+(((sj1)*(x527)))+(((sj1)*(x526)))+(((py)*(x517)*(x522)))+(((IkReal(-1.00000000000000))*(cj1)*(x525)))+(((IkReal(-1.00000000000000))*(cj1)*(x524)))+(((py)*(x519)*(x521)))+(((x520)*(x529)))+(((x515)*(x516)))+(((x523)*(x528)))+(((IkReal(-1656.24997606719))*(sj0)*(x515)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj0)*(x515)*(x520)))+(((px)*(x521)*(x522)))+(((IkReal(-1.00000000000000))*(px)*(x517)*(x519))))))); +IkReal x339=(cj5*sj3); +IkReal x340=(pz*sj1); +IkReal x341=(cj3*cj5); +IkReal x342=((1.1e-7)*sj1); +IkReal x343=(cj0*r01); +IkReal x344=(cj1*r02); +IkReal x345=((0.112)*sj3); +IkReal x346=((0.112)*cj3); +IkReal x347=(r00*sj0); +IkReal x348=((0.106)*sj1); +IkReal x349=((1.1e-7)*x341); +IkReal x350=(cj1*px*sj0); +IkReal x351=(cj0*cj1*py); +IkReal x352=((0.112)*sj1*x347); +CheckValue x353=IKPowWithIntegerCheck(IKsign(((((-0.106)*x339))+(((-1.0)*x349)))),-1); +if(!x353.valid){ +continue; +} +CheckValue x354 = IKatan2WithCheck(IkReal(((((-1.1e-7)*x344))+(((-1.1e-7)*x339))+(((-1.0)*sj1*x343*x345))+((sj1*x345*x347))+((x344*x345))+(((-1.0)*x339*x350))+((x339*x340))+(((-1.0)*x342*x347))+((x339*x351))+((x342*x343)))),IkReal(((((-1.0)*x341*x351))+(((-0.106)*x344))+((x341*x350))+x349+(((-1.0)*x344*x346))+((sj1*x343*x346))+(((-1.0)*x340*x341))+(((-1.0)*x347*x348))+(((-1.0)*sj1*x346*x347))+((x343*x348)))),IKFAST_ATAN2_MAGTHRESH); +if(!x354.valid){ +continue; +} +j2array[0]=((-1.5707963267949)+(((1.5707963267949)*(x353.value)))+(x354.value)); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -1629,39 +1621,65 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES } j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { -IkReal evalcond[4]; -IkReal x530=IKsin(j2); -IkReal x531=IKcos(j2); -IkReal x532=((r00)*(sj0)); -IkReal x533=((r01)*(sj0)); -IkReal x534=((py)*(sj1)); -IkReal x535=((IkReal(0.999999985550000))*(r02)); -IkReal x536=((px)*(sj1)); -IkReal x537=((IkReal(0.999999985550000))*(sj0)); -IkReal x538=((IkReal(0.000169999997543500))*(cj0)); -IkReal x539=((IkReal(0.000169999997543500))*(sj0)); -IkReal x540=((cj1)*(px)); -IkReal x541=((pz)*(sj1)); -IkReal x542=((cj0)*(r00)); -IkReal x543=((cj1)*(pz)); -IkReal x544=((IkReal(1.00000000000000))*(r01)); -IkReal x545=((cj1)*(py)); -IkReal x546=((IkReal(0.999999985550000))*(cj0)); -IkReal x547=((IkReal(0.106000000000000))*(x530)); -IkReal x548=((IkReal(0.106000000000000))*(x531)); -IkReal x549=((IkReal(0.112000000000000))*(x531)); -IkReal x550=((IkReal(0.112000000000000))*(x530)); -IkReal x551=((cj3)*(x550)); -IkReal x552=((sj3)*(x549)); -IkReal x553=((sj3)*(x550)); -IkReal x554=((cj3)*(x549)); -IkReal x555=((x554)+(x548)); -IkReal x556=((x552)+(x551)+(x547)); -evalcond[0]=((((x539)*(x545)))+(((IkReal(-1.00000000000000))*(x537)*(x540)))+(x556)+(x541)+(((x538)*(x540)))+(((x545)*(x546)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x534)*(x546)))+(((IkReal(-1.00000000000000))*(x534)*(x539)))+(((IkReal(-1.00000000000000))*(x555)))+(((x536)*(x537)))+(x553)+(x543)+(((IkReal(-1.00000000000000))*(x536)*(x538)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x536)*(x539)))+(((IkReal(-1.00000000000000))*(sj0)*(x534)*(x535)))+(((IkReal(-1.00000000000000))*(x540)*(x544)))+(((IkReal(-1.00000000000000))*(x556)))+(((IkReal(0.000169999997543500))*(x532)*(x541)))+(((IkReal(-1.00000000000000))*(r01)*(x538)*(x541)))+(((IkReal(0.999999985550000))*(x541)*(x542)))+(((IkReal(0.999999985550000))*(x533)*(x541)))+(((r00)*(x545)))+(((IkReal(-1.00000000000000))*(cj0)*(x535)*(x536)))+(((r02)*(x534)*(x538)))); -evalcond[3]=((((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((r02)*(x539)*(x540)))+(((IkReal(-1.00000000000000))*(r02)*(x538)*(x545)))+(((r01)*(x538)*(x543)))+(((IkReal(-0.999999985550000))*(x533)*(x543)))+(((IkReal(-0.000169999997543500))*(x532)*(x543)))+(((IkReal(-1.00000000000000))*(x555)))+(((IkReal(-0.999999985550000))*(x542)*(x543)))+(x553)+(((IkReal(1.80199997396110e-5))*(x532)))+(((sj0)*(x535)*(x545)))+(((IkReal(0.105999998468300))*(x542)))+(((IkReal(-1.00000000000000))*(x536)*(x544)))+(((IkReal(0.105999998468300))*(x533)))+(((r00)*(x534)))+(((cj0)*(x535)*(x540)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IkReal evalcond[8]; +IkReal x355=IKcos(j2); +IkReal x356=IKsin(j2); +IkReal x357=px*px; +IkReal x358=pz*pz; +IkReal x359=py*py; +IkReal x360=(py*sj1); +IkReal x361=((2.0)*r01); +IkReal x362=(px*sj0); +IkReal x363=(pz*r00); +IkReal x364=(cj0*sj1); +IkReal x365=((0.0237799999999879)*cj3); +IkReal x366=(cj0*r01); +IkReal x367=(px*r00); +IkReal x368=((0.212)*pz); +IkReal x369=((1.0)*px); +IkReal x370=((2.332e-8)*cj5); +IkReal x371=(cj1*r01); +IkReal x372=(cj1*r02); +IkReal x373=((2.2e-7)*r02); +IkReal x374=(r01*sj0); +IkReal x375=(r00*sj0); +IkReal x376=((1.0)*cj1); +IkReal x377=((2.0)*pz); +IkReal x378=(cj0*r00); +IkReal x379=((0.112)*cj3); +IkReal x380=(cj1*py); +IkReal x381=((1.0)*cj3); +IkReal x382=((2.0)*cj0); +IkReal x383=(cj0*py); +IkReal x384=(r02*sj1); +IkReal x385=((2.332e-8)*cj1); +IkReal x386=((1.0)*r01); +IkReal x387=(pz*sj1); +IkReal x388=((1.0)*sj0); +IkReal x389=(py*r01); +IkReal x390=((0.212)*r02); +IkReal x391=((0.0112359999999879)*cj1); +IkReal x392=((2.0)*px); +IkReal x393=(sj3*x355); +IkReal x394=(cj5*x355); +IkReal x395=(cj1*x357); +IkReal x396=((1.0)*x357); +IkReal x397=(cj5*x356); +IkReal x398=(sj1*x359); +IkReal x399=(sj5*x356); +IkReal x400=(sj3*x356); +IkReal x401=((0.106)*x355); +IkReal x402=((1.1e-7)*x355); +IkReal x403=((1.0)*x358); +evalcond[0]=(x384+((sj3*x397))+((cj1*x366))+(((-1.0)*x381*x394))+(((-1.0)*x375*x376))); +evalcond[1]=(((sj1*x375))+(((-1.0)*x364*x386))+(((-1.0)*cj5*x393))+x372+(((-1.0)*x381*x397))); +evalcond[2]=((-1.1e-7)+(((-1.0)*x362*x376))+((x356*x379))+x387+((cj0*x380))+(((0.112)*x393))+(((-1.0)*x402))+(((0.106)*x356))); +evalcond[3]=((-0.106)+(((-1.0)*x355*x379))+((sj1*x362))+((cj1*pz))+(((-1.0)*cj0*x360))+(((0.112)*x400))+(((-1.1e-7)*x356))+(((-1.0)*x401))); +evalcond[4]=((((-1.1e-7)*x399))+(((-1.0)*r01*sj1*x369))+(((-1.0)*sj5*x401))+((cj0*px*x372))+((r00*x360))+(((-1.0)*sj5*x355*x379))+((py*sj0*x372))+(((-1.0)*pz*x371*x388))+(((0.106)*x378))+(((0.106)*x374))+(((0.112)*sj3*x399))+(((-1.0)*cj0*x363*x376))); +evalcond[5]=((((-0.106)*x399))+(((-1.0)*r02*x360*x388))+((x374*x387))+(((-1.0)*r02*x364*x369))+((sj5*x402))+(((-1.0)*x369*x371))+((r00*x380))+(((-1.0)*x379*x399))+((x363*x364))+(((-0.112)*sj5*x393))+(((-1.1e-7)*x374))+(((-1.1e-7)*x378))); +evalcond[6]=((((-0.023744)*x394))+((cj1*x358*x366))+(((2.2e-7)*x367))+((x359*x384))+(((-1.0)*x362*x390))+(((-2.332e-8)*x372))+(((-1.0)*x359*x366*x376))+((x375*x395))+((x366*x395))+((x366*x391))+((x357*x384))+(((0.0013080000000121)*sj3*x397))+(((-1.0)*x384*x403))+(((-1.0)*sj1*x363*x392))+(((-1.0)*x366*x368))+(((-2.464e-8)*x397))+(((-1.0)*x367*x380*x382))+(((-2.332e-8)*sj1*x375))+(((-1.0)*cj3*x356*x370))+(((2.332e-8)*r01*x364))+((pz*x373))+(((-1.0)*x372*x377*x383))+(((-1.0)*x358*x375*x376))+(((0.212)*sj0*x363))+(((-1.0)*x359*x375*x376))+(((0.0112359999999879)*x384))+(((-1.0)*pz*x360*x361))+(((-1.0)*x365*x394))+((x362*x372*x377))+(((2.2e-7)*x389))+((x370*x393))+(((-1.0)*x375*x391))+((x383*x390))+((x361*x362*x380))); +evalcond[7]=((((-0.023744)*x397))+(((-1.0)*x373*x383))+(((-1.0)*x372*x403))+(((-2.2e-7)*sj0*x363))+((r01*x359*x364))+(((-1.0)*sj1*x375*x396))+((sj1*x358*x375))+((x360*x367*x382))+(((-0.0112359999999879)*x372))+(((-1.0)*x358*x364*x386))+(((-0.0112359999999879)*sj1*x375))+((x370*x400))+((cj3*x355*x370))+((x375*x398))+(((0.212)*x389))+((x359*x372))+(((-2.332e-8)*x384))+(((-1.0)*x360*x361*x362))+(((2.464e-8)*x394))+(((-1.0)*pz*x361*x380))+(((0.212)*x367))+(((-1.0)*cj1*x363*x392))+(((-1.0)*x365*x397))+(((-0.0013080000000121)*cj5*x393))+(((-1.0)*x357*x364*x386))+((x357*x372))+((x375*x385))+(((0.0112359999999879)*r01*x364))+((cj0*r02*x360*x377))+(((-1.0)*x366*x385))+((x362*x373))+((r02*x368))+(((2.2e-7)*pz*x366))+(((-1.0)*x362*x377*x384))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -1704,2679 +1722,28 @@ solutions.AddSolution(vinfos,vfree); } -} else -{ -IkReal x557=(px)*(px); -IkReal x558=((IkReal(0.0237440000000000))*(cj3)); -IkReal x559=(py)*(py); -IkReal x560=(pz)*(pz); -IkReal x561=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x562=((cj0)*(r00)); -IkReal x563=((r01)*(sj0)); -IkReal x564=((IkReal(0.000169999997543500))*(sj1)); -IkReal x565=((IkReal(0.000339999995087000))*(py)); -IkReal x566=((pz)*(sj1)); -IkReal x567=((r02)*(sj0)); -IkReal x568=((cj0)*(px)); -IkReal x569=((cj1)*(r01)); -IkReal x570=((IkReal(0.000169999997543500))*(cj0)); -IkReal x571=((IkReal(3.60399994792220e-5))*(sj1)); -IkReal x572=((py)*(r01)); -IkReal x573=((r00)*(sj0)); -IkReal x574=((IkReal(0.000169999997543500))*(pz)); -IkReal x575=((IkReal(0.105999998468300))*(cj0)); -IkReal x576=((px)*(r00)); -IkReal x577=((IkReal(1.99999997110000))*(pz)); -IkReal x578=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x579=((r01)*(sj1)); -IkReal x580=((cj1)*(r02)); -IkReal x581=((r02)*(sj1)); -IkReal x582=((IkReal(0.999999985550000))*(py)); -IkReal x583=((IkReal(0.999999985550000))*(cj0)); -IkReal x584=((IkReal(0.211999996936600))*(px)); -IkReal x585=((IkReal(0.0112359998376398))*(cj0)); -IkReal x586=((cj0)*(r02)); -IkReal x587=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x588=((IkReal(0.211999996936600))*(py)); -IkReal x589=((cj1)*(px)); -IkReal x590=((IkReal(0.000339999995087000))*(pz)); -IkReal x591=((IkReal(0.999999985550000))*(cj1)); -IkReal x592=((IkReal(0.000169999997543500))*(cj1)); -IkReal x593=((cj0)*(py)); -IkReal x594=((pz)*(r02)); -IkReal x595=((IkReal(0.211999996936600))*(pz)); -IkReal x596=((cj1)*(pz)); -IkReal x597=((IkReal(0.999999985550000))*(sj1)); -IkReal x598=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x599=((cj0)*(r01)); -IkReal x600=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x601=((IkReal(0.999999985550000))*(x560)); -IkReal x602=((IkReal(0.000169999997543500))*(x559)); -IkReal x603=((IkReal(0.999999985550000))*(x557)); -IkReal x604=((IkReal(0.000169999997543500))*(x560)); -IkReal x605=((IkReal(1.99999997110000))*(px)*(py)); -IkReal x606=((IkReal(0.000169999997543500))*(x557)); -IkReal x607=((IkReal(2.00000000000000))*(x560)); -IkReal x608=((IkReal(0.999999985550000))*(x559)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((py)*(x570)))+(((IkReal(-0.999999985550000))*(x568)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((IkReal(-1.00000000000000))*(sj0)*(x582)))); -evalcond[2]=((IkReal(1.00000000000000))+(((r01)*(x570)))+(((IkReal(-0.999999985550000))*(x563)))+(((IkReal(-0.999999985550000))*(x562)))+(((IkReal(-0.000169999997543500))*(x573)))); -evalcond[3]=((IkReal(0.0125440000000000))+(((sj0)*(sj1)*(x584)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x588)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x571)))+(x558)+(((IkReal(-1.00000000000000))*(x568)*(x571)))+(((IkReal(0.212000000000000))*(x596)))); -evalcond[4]=((((IkReal(0.105999998468300))*(sj1)*(x573)))+(((IkReal(0.106000000000000))*(x580)))+(((IkReal(-1.00000000000000))*(x594)))+(((IkReal(-1.00000000000000))*(x563)*(x587)))+(((IkReal(-1.00000000000000))*(x572)))+(((IkReal(-1.00000000000000))*(x576)))+(((IkReal(-1.00000000000000))*(x575)*(x579)))+(((IkReal(-1.00000000000000))*(x562)*(x587)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x573)*(x591)))+(((x562)*(x592)))+(x581)+(((x569)*(x583)))+(((x563)*(x592)))); -evalcond[6]=((((x573)*(x597)))+(((IkReal(-1.00000000000000))*(x579)*(x583)))+(((IkReal(-1.00000000000000))*(x562)*(x564)))+(x580)+(((IkReal(-1.00000000000000))*(x563)*(x564)))); -evalcond[7]=((((IkReal(0.999999985550000))*(pz)*(x573)))+(((IkReal(0.000169999997543500))*(py)*(x567)))+(((x582)*(x586)))+(((IkReal(-0.999999985550000))*(px)*(x567)))+(((IkReal(-1.00000000000000))*(pz)*(r01)*(x583)))+(((IkReal(0.106000000000000))*(x581)))+(((IkReal(0.000169999997543500))*(r02)*(x568)))+(((x562)*(x598)))+(((IkReal(-1.00000000000000))*(x563)*(x574)))+(((x569)*(x575)))+(((IkReal(-0.105999998468300))*(cj1)*(x573)))+(((x563)*(x598)))+(((IkReal(-1.00000000000000))*(x562)*(x574)))); -evalcond[8]=((IkReal(0.0237800000000000))+(((IkReal(-1.00000000000000))*(px)*(x562)*(x565)))+(((x563)*(x608)))+(((IkReal(-1.00000000000000))*(r01)*(x559)*(x570)))+(((IkReal(-1.00000000000000))*(x573)*(x604)))+(((IkReal(-1.00000000000000))*(x573)*(x602)))+(((IkReal(3.60399994792220e-5))*(x580)*(x593)))+(((cj1)*(x561)*(x573)))+(((cj1)*(x563)*(x595)))+(((px)*(x567)*(x590)))+(((IkReal(-1.00000000000000))*(cj0)*(x561)*(x569)))+(((r01)*(x560)*(x570)))+(((x562)*(x603)))+(((cj1)*(x562)*(x595)))+(((r02)*(x568)*(x577)))+(((IkReal(-0.0112359998376398))*(x563)))+(((IkReal(-0.0112359998376398))*(x562)))+(((IkReal(-1.00000000000000))*(cj1)*(x567)*(x588)))+(((px)*(x563)*(x565)))+(((r01)*(x557)*(x570)))+(x558)+(((IkReal(-0.212000000000000))*(py)*(r00)*(sj1)))+(((IkReal(-1.00000000000000))*(x562)*(x608)))+(((IkReal(-1.00000000000000))*(x562)*(x601)))+(((IkReal(1.91011997239877e-6))*(x599)))+(((IkReal(-3.60399994792220e-5))*(x567)*(x589)))+(((IkReal(-0.211999996936600))*(x568)*(x580)))+(((py)*(x567)*(x577)))+(((IkReal(1.99999997110000))*(x568)*(x572)))+(((IkReal(-1.00000000000000))*(x563)*(x601)))+(((IkReal(-1.00000000000000))*(x563)*(x603)))+(((x573)*(x605)))+(((x573)*(x606)))+(((IkReal(-1.91011997239877e-6))*(x573)))+(((IkReal(0.212000000000000))*(px)*(x579)))+(((IkReal(-1.00000000000000))*(pz)*(x565)*(x586)))); -evalcond[9]=((((IkReal(-2.00000000000000))*(py)*(pz)*(x569)))+(((IkReal(-2.00000000000000))*(x576)*(x596)))+(((x563)*(x600)))+(((pp)*(x580)))+(((x579)*(x585)))+(((IkReal(-1.99999997110000))*(px)*(x566)*(x567)))+(((IkReal(0.212000000000000))*(x572)))+(((IkReal(0.212000000000000))*(x576)))+(((IkReal(0.000339999995087000))*(r02)*(x566)*(x568)))+(((IkReal(-0.0112360000000000))*(x580)))+(((IkReal(1.99999997110000))*(py)*(x566)*(x586)))+(((IkReal(-0.0112359998376398))*(sj1)*(x573)))+(((x562)*(x600)))+(((sj1)*(x562)*(x605)))+(((IkReal(-1.00000000000000))*(x560)*(x562)*(x564)))+(((IkReal(-1.00000000000000))*(x560)*(x579)*(x583)))+(((IkReal(-1.00000000000000))*(x557)*(x573)*(x597)))+(((IkReal(-1.00000000000000))*(x557)*(x563)*(x564)))+(((IkReal(-1.00000000000000))*(x557)*(x579)*(x583)))+(((x565)*(x568)*(x579)))+(((IkReal(-1.00000000000000))*(sj1)*(x563)*(x605)))+(((x557)*(x562)*(x564)))+(((IkReal(-1.00000000000000))*(x560)*(x563)*(x564)))+(((x560)*(x573)*(x597)))+(((x559)*(x579)*(x583)))+(((px)*(sj1)*(x565)*(x573)))+(((x559)*(x563)*(x564)))+(((IkReal(-1.00000000000000))*(x559)*(x562)*(x564)))+(((IkReal(-1.00000000000000))*(x580)*(x607)))+(((x565)*(x566)*(x567)))+(((IkReal(0.212000000000000))*(x594)))+(((x559)*(x573)*(x597)))); -evalcond[10]=((((IkReal(3.60399994792220e-5))*(r02)*(x568)))+(((IkReal(-1.00000000000000))*(x565)*(x567)*(x596)))+(((IkReal(1.99999997110000))*(py)*(x563)*(x589)))+(((IkReal(-1.00000000000000))*(x559)*(x573)*(x591)))+(((x573)*(x595)))+(((pp)*(x581)))+(((IkReal(-1.00000000000000))*(x577)*(x580)*(x593)))+(((IkReal(-1.00000000000000))*(x559)*(x563)*(x592)))+(((x563)*(x578)))+(((x560)*(x563)*(x592)))+(((IkReal(0.0112360000000000))*(x581)))+(((x557)*(x563)*(x592)))+(((x557)*(x573)*(x591)))+(((IkReal(-1.99999997110000))*(py)*(x562)*(x589)))+(((x557)*(x569)*(x583)))+(((IkReal(-0.0112359998376398))*(cj1)*(x573)))+(((x560)*(x569)*(x583)))+(((IkReal(-2.00000000000000))*(x566)*(x572)))+(((IkReal(-2.00000000000000))*(x566)*(x576)))+(((x586)*(x588)))+(((x562)*(x578)))+(((x560)*(x562)*(x592)))+(((IkReal(-1.00000000000000))*(x565)*(x573)*(x589)))+(((x559)*(x562)*(x592)))+(((IkReal(-1.00000000000000))*(x557)*(x562)*(x592)))+(((IkReal(-1.00000000000000))*(x559)*(x569)*(x583)))+(((IkReal(3.60399994792220e-5))*(py)*(x567)))+(((IkReal(-1.00000000000000))*(x595)*(x599)))+(((x569)*(x585)))+(((IkReal(-1.00000000000000))*(x560)*(x573)*(x591)))+(((x567)*(x577)*(x589)))+(((IkReal(-1.00000000000000))*(x581)*(x607)))+(((IkReal(-1.00000000000000))*(x568)*(x580)*(x590)))+(((IkReal(-1.00000000000000))*(x561)*(x563)))+(((IkReal(-1.00000000000000))*(x561)*(x562)))+(((IkReal(-1.00000000000000))*(x567)*(x584)))+(((IkReal(-1.00000000000000))*(x565)*(x568)*(x569)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst6; -gconst6=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - } else { { IkReal j2array[1], cj2array[1], sj2array[1]; bool j2valid[1]={false}; _nj2 = 1; -IkReal x609=((cj0)*(sj1)); -IkReal x610=((IkReal(1656.24997606719))*(pz)); -IkReal x611=((py)*(r02)); -IkReal x612=((IkReal(1750.00000000000))*(cj1)); -IkReal x613=((px)*(sj3)); -IkReal x614=((cj3)*(pz)); -IkReal x615=((sj0)*(sj1)); -IkReal x616=((IkReal(1749.99997471250))*(r02)); -IkReal x617=((px)*(r01)); -IkReal x618=((py)*(sj3)); -IkReal x619=((pz)*(sj3)); -IkReal x620=((IkReal(0.281562495931422))*(px)); -IkReal x621=((IkReal(0.297499995701125))*(cj3)); -IkReal x622=((IkReal(1749.99997471250))*(r01)); -IkReal x623=((IkReal(1749.99997471250))*(r00)); -IkReal x624=((IkReal(1656.25000000000))*(cj1)); -IkReal x625=((IkReal(0.281562495931422))*(pz)); -IkReal x626=((IkReal(1749.99997471250))*(cj3)); -IkReal x627=((IkReal(1656.24997606719))*(px)); -IkReal x628=((IkReal(0.297499995701125))*(r01)); -IkReal x629=((py)*(r00)); -IkReal x630=((IkReal(0.297499995701125))*(r00)); -if( IKabs(((gconst6)*(((((x609)*(x611)*(x621)))+(((IkReal(-1.00000000000000))*(x611)*(x615)*(x626)))+(((IkReal(-1656.24997606719))*(x611)*(x615)))+(((r00)*(x615)*(x625)))+(((IkReal(-1.00000000000000))*(r02)*(x615)*(x620)))+(((IkReal(0.281562495931422))*(x609)*(x611)))+(((IkReal(-1.00000000000000))*(cj3)*(px)*(x609)*(x616)))+(((IkReal(-1.00000000000000))*(cj3)*(x612)*(x617)))+(((IkReal(-0.297499995701125))*(x615)*(x618)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj3)*(x612)*(x629)))+(((x612)*(x619)))+(((IkReal(-1.00000000000000))*(x617)*(x624)))+(((r00)*(x609)*(x610)))+(((IkReal(-0.297499995701125))*(x609)*(x613)))+(((x614)*(x615)*(x622)))+(((IkReal(-1.00000000000000))*(x609)*(x614)*(x628)))+(((x614)*(x615)*(x630)))+(((IkReal(-1.00000000000000))*(r01)*(x609)*(x625)))+(((x624)*(x629)))+(((IkReal(1749.99997471250))*(x613)*(x615)))+(((IkReal(-1.00000000000000))*(r02)*(x609)*(x627)))+(((r01)*(x610)*(x615)))+(((x609)*(x614)*(x623)))+(((IkReal(-1749.99997471250))*(x609)*(x618)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x615)*(x621))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(x612)*(x614)))+(((IkReal(-1.00000000000000))*(pz)*(x624)))+(((IkReal(-1.00000000000000))*(x609)*(x613)*(x616)))+(((IkReal(-1.00000000000000))*(x609)*(x619)*(x628)))+(((py)*(x609)*(x626)))+(((x609)*(x619)*(x623)))+(((r00)*(x612)*(x618)))+(((IkReal(-1749.99997471250))*(sj3)*(x611)*(x615)))+(((x615)*(x619)*(x622)))+(((IkReal(-1.00000000000000))*(r01)*(x612)*(x613)))+(((IkReal(0.281562495931422))*(py)*(x615)))+(((x609)*(x620)))+(((x615)*(x619)*(x630)))+(((IkReal(-0.297499995701125))*(r02)*(x613)*(x615)))+(((IkReal(1656.24997606719))*(py)*(x609)))+(((IkReal(0.297499995701125))*(sj3)*(x609)*(x611)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x615)*(x626)))+(((px)*(x609)*(x621)))+(((py)*(x615)*(x621)))+(((IkReal(-1.00000000000000))*(x615)*(x627))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst6)*(((((x609)*(x611)*(x621)))+(((IkReal(-1.00000000000000))*(x611)*(x615)*(x626)))+(((IkReal(-1656.24997606719))*(x611)*(x615)))+(((r00)*(x615)*(x625)))+(((IkReal(-1.00000000000000))*(r02)*(x615)*(x620)))+(((IkReal(0.281562495931422))*(x609)*(x611)))+(((IkReal(-1.00000000000000))*(cj3)*(px)*(x609)*(x616)))+(((IkReal(-1.00000000000000))*(cj3)*(x612)*(x617)))+(((IkReal(-0.297499995701125))*(x615)*(x618)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj3)*(x612)*(x629)))+(((x612)*(x619)))+(((IkReal(-1.00000000000000))*(x617)*(x624)))+(((r00)*(x609)*(x610)))+(((IkReal(-0.297499995701125))*(x609)*(x613)))+(((x614)*(x615)*(x622)))+(((IkReal(-1.00000000000000))*(x609)*(x614)*(x628)))+(((x614)*(x615)*(x630)))+(((IkReal(-1.00000000000000))*(r01)*(x609)*(x625)))+(((x624)*(x629)))+(((IkReal(1749.99997471250))*(x613)*(x615)))+(((IkReal(-1.00000000000000))*(r02)*(x609)*(x627)))+(((r01)*(x610)*(x615)))+(((x609)*(x614)*(x623)))+(((IkReal(-1749.99997471250))*(x609)*(x618)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x615)*(x621)))))), ((gconst6)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(x612)*(x614)))+(((IkReal(-1.00000000000000))*(pz)*(x624)))+(((IkReal(-1.00000000000000))*(x609)*(x613)*(x616)))+(((IkReal(-1.00000000000000))*(x609)*(x619)*(x628)))+(((py)*(x609)*(x626)))+(((x609)*(x619)*(x623)))+(((r00)*(x612)*(x618)))+(((IkReal(-1749.99997471250))*(sj3)*(x611)*(x615)))+(((x615)*(x619)*(x622)))+(((IkReal(-1.00000000000000))*(r01)*(x612)*(x613)))+(((IkReal(0.281562495931422))*(py)*(x615)))+(((x609)*(x620)))+(((x615)*(x619)*(x630)))+(((IkReal(-0.297499995701125))*(r02)*(x613)*(x615)))+(((IkReal(1656.24997606719))*(py)*(x609)))+(((IkReal(0.297499995701125))*(sj3)*(x609)*(x611)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x615)*(x626)))+(((px)*(x609)*(x621)))+(((py)*(x615)*(x621)))+(((IkReal(-1.00000000000000))*(x615)*(x627))))))); -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[4]; -IkReal x631=IKsin(j2); -IkReal x632=IKcos(j2); -IkReal x633=((r00)*(sj0)); -IkReal x634=((r01)*(sj0)); -IkReal x635=((py)*(sj1)); -IkReal x636=((IkReal(0.999999985550000))*(r02)); -IkReal x637=((px)*(sj1)); -IkReal x638=((IkReal(0.999999985550000))*(sj0)); -IkReal x639=((IkReal(0.000169999997543500))*(cj0)); -IkReal x640=((IkReal(0.000169999997543500))*(sj0)); -IkReal x641=((cj1)*(px)); -IkReal x642=((pz)*(sj1)); -IkReal x643=((cj0)*(r00)); -IkReal x644=((cj1)*(pz)); -IkReal x645=((IkReal(1.00000000000000))*(r01)); -IkReal x646=((cj1)*(py)); -IkReal x647=((IkReal(0.999999985550000))*(cj0)); -IkReal x648=((IkReal(0.106000000000000))*(x631)); -IkReal x649=((IkReal(0.106000000000000))*(x632)); -IkReal x650=((IkReal(0.112000000000000))*(x632)); -IkReal x651=((IkReal(0.112000000000000))*(x631)); -IkReal x652=((cj3)*(x651)); -IkReal x653=((sj3)*(x650)); -IkReal x654=((cj3)*(x650)); -IkReal x655=((sj3)*(x651)); -IkReal x656=((x649)+(x654)); -IkReal x657=((x648)+(x652)+(x653)); -evalcond[0]=((((x639)*(x641)))+(((x640)*(x646)))+(x642)+(x657)+(((x646)*(x647)))+(((IkReal(-1.00000000000000))*(x638)*(x641)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x637)*(x639)))+(((IkReal(-1.00000000000000))*(x656)))+(((IkReal(-1.00000000000000))*(x635)*(x640)))+(((IkReal(-1.00000000000000))*(x635)*(x647)))+(x644)+(x655)+(((x637)*(x638)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(x636)*(x637)))+(((IkReal(0.999999985550000))*(x642)*(x643)))+(((IkReal(-1.00000000000000))*(x641)*(x645)))+(((IkReal(-1.00000000000000))*(sj0)*(x635)*(x636)))+(((IkReal(0.999999985550000))*(x634)*(x642)))+(((IkReal(-1.00000000000000))*(r01)*(x639)*(x642)))+(x657)+(((r00)*(x646)))+(((IkReal(-1.00000000000000))*(r02)*(x637)*(x640)))+(((r02)*(x635)*(x639)))+(((IkReal(0.000169999997543500))*(x633)*(x642)))); -evalcond[3]=((((IkReal(-0.999999985550000))*(x634)*(x644)))+(((r01)*(x639)*(x644)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-0.000169999997543500))*(x633)*(x644)))+(((IkReal(1.80199997396110e-5))*(x633)))+(((IkReal(0.105999998468300))*(x634)))+(((IkReal(-1.00000000000000))*(x655)))+(((sj0)*(x636)*(x646)))+(((IkReal(0.105999998468300))*(x643)))+(((r00)*(x635)))+(x656)+(((IkReal(-1.00000000000000))*(x637)*(x645)))+(((cj0)*(x636)*(x641)))+(((IkReal(-1.00000000000000))*(r02)*(x639)*(x646)))+(((IkReal(-0.999999985550000))*(x643)*(x644)))+(((r02)*(x640)*(x641)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x658=((cj0)*(sj1)); -IkReal x659=((IkReal(0.281562495931422))*(px)); -IkReal x660=((cj1)*(sj0)); -IkReal x661=((IkReal(0.281562495931422))*(py)); -IkReal x662=((IkReal(1656.24997606719))*(px)); -IkReal x663=((IkReal(1656.24997606719))*(py)); -IkReal x664=((cj0)*(cj1)); -IkReal x665=((IkReal(1749.99997471250))*(px)); -IkReal x666=((IkReal(0.297499995701125))*(px)); -IkReal x667=((IkReal(1749.99997471250))*(py)); -IkReal x668=((IkReal(0.297499995701125))*(py)); -IkReal x669=((sj0)*(sj1)); -IkReal x670=((IkReal(1656.25000000000))*(pz)); -IkReal x671=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x672=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x673=((sj3)*(x669)); -IkReal x674=((cj3)*(x669)); -if( IKabs(((gconst5)*(((((x665)*(x673)))+(((IkReal(-1.00000000000000))*(x660)*(x662)))+(((x663)*(x664)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x668)*(x673)))+(((cj3)*(x660)*(x668)))+(((x660)*(x661)))+(((x659)*(x664)))+(((sj1)*(x671)))+(((sj1)*(x670)))+(((IkReal(-1.00000000000000))*(cj3)*(x660)*(x665)))+(((cj3)*(x664)*(x666)))+(((cj3)*(x664)*(x667)))+(((cj1)*(x672)))+(((IkReal(-1.00000000000000))*(sj3)*(x658)*(x667)))+(((IkReal(-1.00000000000000))*(sj3)*(x658)*(x666))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(175.562500000000))+(((sj3)*(x660)*(x668)))+(((IkReal(-1.00000000000000))*(x662)*(x669)))+(((sj3)*(x664)*(x667)))+(((sj3)*(x664)*(x666)))+(((IkReal(-1.00000000000000))*(x665)*(x674)))+(((IkReal(-1.00000000000000))*(sj3)*(x660)*(x665)))+(((IkReal(-1.00000000000000))*(cj1)*(x671)))+(((IkReal(-1.00000000000000))*(cj1)*(x670)))+(((x668)*(x674)))+(((x661)*(x669)))+(((IkReal(185.500000000000))*(cj3)))+(((sj1)*(x672)))+(((x658)*(x663)))+(((x658)*(x659)))+(((cj3)*(x658)*(x667)))+(((cj3)*(x658)*(x666))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst5)*(((((x665)*(x673)))+(((IkReal(-1.00000000000000))*(x660)*(x662)))+(((x663)*(x664)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x668)*(x673)))+(((cj3)*(x660)*(x668)))+(((x660)*(x661)))+(((x659)*(x664)))+(((sj1)*(x671)))+(((sj1)*(x670)))+(((IkReal(-1.00000000000000))*(cj3)*(x660)*(x665)))+(((cj3)*(x664)*(x666)))+(((cj3)*(x664)*(x667)))+(((cj1)*(x672)))+(((IkReal(-1.00000000000000))*(sj3)*(x658)*(x667)))+(((IkReal(-1.00000000000000))*(sj3)*(x658)*(x666)))))), ((gconst5)*(((IkReal(175.562500000000))+(((sj3)*(x660)*(x668)))+(((IkReal(-1.00000000000000))*(x662)*(x669)))+(((sj3)*(x664)*(x667)))+(((sj3)*(x664)*(x666)))+(((IkReal(-1.00000000000000))*(x665)*(x674)))+(((IkReal(-1.00000000000000))*(sj3)*(x660)*(x665)))+(((IkReal(-1.00000000000000))*(cj1)*(x671)))+(((IkReal(-1.00000000000000))*(cj1)*(x670)))+(((x668)*(x674)))+(((x661)*(x669)))+(((IkReal(185.500000000000))*(cj3)))+(((sj1)*(x672)))+(((x658)*(x663)))+(((x658)*(x659)))+(((cj3)*(x658)*(x667)))+(((cj3)*(x658)*(x666))))))); -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[4]; -IkReal x675=IKsin(j2); -IkReal x676=IKcos(j2); -IkReal x677=((r00)*(sj0)); -IkReal x678=((r01)*(sj0)); -IkReal x679=((py)*(sj1)); -IkReal x680=((IkReal(0.999999985550000))*(r02)); -IkReal x681=((px)*(sj1)); -IkReal x682=((IkReal(0.999999985550000))*(sj0)); -IkReal x683=((IkReal(0.000169999997543500))*(cj0)); -IkReal x684=((IkReal(0.000169999997543500))*(sj0)); -IkReal x685=((cj1)*(px)); -IkReal x686=((pz)*(sj1)); -IkReal x687=((cj0)*(r00)); -IkReal x688=((cj1)*(pz)); -IkReal x689=((IkReal(1.00000000000000))*(r01)); -IkReal x690=((cj1)*(py)); -IkReal x691=((IkReal(0.999999985550000))*(cj0)); -IkReal x692=((IkReal(0.106000000000000))*(x675)); -IkReal x693=((IkReal(0.106000000000000))*(x676)); -IkReal x694=((IkReal(0.112000000000000))*(x676)); -IkReal x695=((IkReal(0.112000000000000))*(x675)); -IkReal x696=((cj3)*(x695)); -IkReal x697=((sj3)*(x694)); -IkReal x698=((cj3)*(x694)); -IkReal x699=((sj3)*(x695)); -IkReal x700=((x693)+(x698)); -IkReal x701=((x696)+(x697)+(x692)); -evalcond[0]=((((x690)*(x691)))+(((x683)*(x685)))+(((x684)*(x690)))+(x686)+(((IkReal(-1.00000000000000))*(x682)*(x685)))+(x701)); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x681)*(x683)))+(((IkReal(-1.00000000000000))*(x679)*(x684)))+(((IkReal(-1.00000000000000))*(x700)))+(((x681)*(x682)))+(x688)+(x699)+(((IkReal(-1.00000000000000))*(x679)*(x691)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(x680)*(x681)))+(((IkReal(-1.00000000000000))*(x685)*(x689)))+(((IkReal(0.000169999997543500))*(x677)*(x686)))+(((IkReal(-1.00000000000000))*(r02)*(x681)*(x684)))+(((IkReal(0.999999985550000))*(x686)*(x687)))+(((IkReal(0.999999985550000))*(x678)*(x686)))+(((IkReal(-1.00000000000000))*(sj0)*(x679)*(x680)))+(x701)+(((r00)*(x690)))+(((IkReal(-1.00000000000000))*(r01)*(x683)*(x686)))+(((r02)*(x679)*(x683)))); -evalcond[3]=((((IkReal(1.80199997396110e-5))*(x677)))+(((IkReal(0.105999998468300))*(x687)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(x681)*(x689)))+(((IkReal(0.105999998468300))*(x678)))+(((r00)*(x679)))+(((IkReal(-1.00000000000000))*(r02)*(x683)*(x690)))+(((IkReal(-0.000169999997543500))*(x677)*(x688)))+(((IkReal(-0.999999985550000))*(x687)*(x688)))+(((sj0)*(x680)*(x690)))+(((IkReal(-1.00000000000000))*(x699)))+(x700)+(((cj0)*(x680)*(x685)))+(((r01)*(x683)*(x688)))+(((r02)*(x684)*(x685)))+(((IkReal(-0.999999985550000))*(x678)*(x688)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x702=((cj0)*(py)); -IkReal x703=((r02)*(sj1)); -IkReal x704=((px)*(sj0)); -IkReal x705=((IkReal(13.9999997977000))*(r00)); -IkReal x706=((IkReal(1.69999943143527e-5))*(cj5)); -IkReal x707=((cj0)*(cj1)); -IkReal x708=((IkReal(0.00237999996560900))*(r01)); -IkReal x709=((cj3)*(px)); -IkReal x710=((IkReal(13.9999997977000))*(r01)); -IkReal x711=((cj1)*(sj0)); -IkReal x712=((px)*(sj3)); -IkReal x713=((IkReal(0.00237999996560900))*(r00)); -IkReal x714=((IkReal(0.0212499928929409))*(cj5)); -IkReal x715=((cj3)*(py)); -IkReal x716=((sj3)*(x711)); -IkReal x717=((IkReal(0.0999999665550159))*(cj1)*(cj5)*(sj3)); -IkReal x718=((cj5)*(pz)*(sj1)*(sj3)); -IkReal x719=((IkReal(0.0999999665550159))*(cj1)*(cj3)*(cj5)); -IkReal x720=((cj3)*(cj5)*(pz)*(sj1)); -IkReal x721=((IkReal(124.999958193770))*(cj1)*(cj5)*(sj3)); -IkReal x722=((IkReal(124.999958193770))*(cj1)*(cj3)*(cj5)); -if( IKabs(((gconst2)*(((((x702)*(x722)))+(((IkReal(-1.00000000000000))*(x706)*(x707)*(x712)))+(((sj3)*(x707)*(x713)))+(((sj3)*(x707)*(x710)))+(((IkReal(-1.00000000000000))*(x702)*(x717)))+(((x711)*(x714)*(x715)))+(((x707)*(x709)*(x714)))+(((IkReal(-1.00000000000000))*(x704)*(x722)))+(((IkReal(14.0000000000000))*(sj3)*(x703)))+(((IkReal(124.999960000019))*(x720)))+(((IkReal(-1.00000000000000))*(x705)*(x716)))+(((IkReal(-0.0999999680000154))*(x718)))+(((x704)*(x717)))+(((IkReal(-1.00000000000000))*(py)*(x706)*(x716)))+(((x708)*(x716))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((x702)*(x721)))+(((x706)*(x711)*(x715)))+(((IkReal(-14.0000000000000))*(cj3)*(x703)))+(((x706)*(x707)*(x709)))+(((IkReal(-0.00225249996745138))*(r00)*(x707)))+(((x702)*(x719)))+(((py)*(x714)*(x716)))+(((IkReal(124.999960000019))*(x718)))+(((IkReal(-1.00000000000000))*(x704)*(x721)))+(((IkReal(13.2499998085375))*(r00)*(x711)))+(((cj3)*(x705)*(x711)))+(((x707)*(x712)*(x714)))+(((IkReal(0.0999999680000154))*(x720)))+(((IkReal(-1.00000000000000))*(cj3)*(x707)*(x710)))+(((IkReal(-1.00000000000000))*(cj3)*(x707)*(x713)))+(((IkReal(-1.00000000000000))*(x704)*(x719)))+(((IkReal(-13.2499998085375))*(r01)*(x707)))+(((IkReal(-13.2500000000000))*(x703)))+(((IkReal(-1.00000000000000))*(cj3)*(x708)*(x711)))+(((IkReal(-0.00225249996745138))*(r01)*(x711))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst2)*(((((x702)*(x722)))+(((IkReal(-1.00000000000000))*(x706)*(x707)*(x712)))+(((sj3)*(x707)*(x713)))+(((sj3)*(x707)*(x710)))+(((IkReal(-1.00000000000000))*(x702)*(x717)))+(((x711)*(x714)*(x715)))+(((x707)*(x709)*(x714)))+(((IkReal(-1.00000000000000))*(x704)*(x722)))+(((IkReal(14.0000000000000))*(sj3)*(x703)))+(((IkReal(124.999960000019))*(x720)))+(((IkReal(-1.00000000000000))*(x705)*(x716)))+(((IkReal(-0.0999999680000154))*(x718)))+(((x704)*(x717)))+(((IkReal(-1.00000000000000))*(py)*(x706)*(x716)))+(((x708)*(x716)))))), ((gconst2)*(((((x702)*(x721)))+(((x706)*(x711)*(x715)))+(((IkReal(-14.0000000000000))*(cj3)*(x703)))+(((x706)*(x707)*(x709)))+(((IkReal(-0.00225249996745138))*(r00)*(x707)))+(((x702)*(x719)))+(((py)*(x714)*(x716)))+(((IkReal(124.999960000019))*(x718)))+(((IkReal(-1.00000000000000))*(x704)*(x721)))+(((IkReal(13.2499998085375))*(r00)*(x711)))+(((cj3)*(x705)*(x711)))+(((x707)*(x712)*(x714)))+(((IkReal(0.0999999680000154))*(x720)))+(((IkReal(-1.00000000000000))*(cj3)*(x707)*(x710)))+(((IkReal(-1.00000000000000))*(cj3)*(x707)*(x713)))+(((IkReal(-1.00000000000000))*(x704)*(x719)))+(((IkReal(-13.2499998085375))*(r01)*(x707)))+(((IkReal(-13.2500000000000))*(x703)))+(((IkReal(-1.00000000000000))*(cj3)*(x708)*(x711)))+(((IkReal(-0.00225249996745138))*(r01)*(x711))))))); -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[8]; -IkReal x723=IKsin(j2); -IkReal x724=IKcos(j2); -IkReal x725=(py)*(py); -IkReal x726=(pz)*(pz); -IkReal x727=(px)*(px); -IkReal x728=((cj0)*(r00)); -IkReal x729=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x730=((r01)*(sj0)); -IkReal x731=((cj3)*(cj5)); -IkReal x732=((sj0)*(sj1)); -IkReal x733=((IkReal(0.999999985550000))*(px)); -IkReal x734=((IkReal(0.000339999995087000))*(py)); -IkReal x735=((pz)*(r02)); -IkReal x736=((py)*(sj1)); -IkReal x737=((IkReal(0.000169999997543500))*(sj1)); -IkReal x738=((IkReal(0.999999680000154))*(cj5)); -IkReal x739=((px)*(r00)); -IkReal x740=((IkReal(0.000799999744000123))*(cj5)); -IkReal x741=((IkReal(0.000169999997543500))*(cj1)); -IkReal x742=((cj0)*(sj1)); -IkReal x743=((IkReal(1.99999997110000))*(px)); -IkReal x744=((IkReal(0.000169999997543500))*(pz)); -IkReal x745=((IkReal(0.112000000000000))*(sj5)); -IkReal x746=((cj1)*(r02)); -IkReal x747=((IkReal(0.106000000000000))*(sj5)); -IkReal x748=((IkReal(0.000339999995087000))*(px)); -IkReal x749=((IkReal(0.112000000000000))*(cj3)); -IkReal x750=((cj1)*(py)); -IkReal x751=((py)*(r02)); -IkReal x752=((cj0)*(px)); -IkReal x753=((px)*(r01)); -IkReal x754=((cj0)*(r01)); -IkReal x755=((IkReal(0.999999985550000))*(r01)); -IkReal x756=((IkReal(0.999999985550000))*(pz)); -IkReal x757=((r02)*(sj1)); -IkReal x758=((cj1)*(sj0)); -IkReal x759=((IkReal(1.99999997110000))*(cj0)); -IkReal x760=((IkReal(0.000169999997543500))*(r02)); -IkReal x761=((IkReal(1.04639966515216e-6))*(cj5)); -IkReal x762=((IkReal(0.999999985550000))*(cj0)); -IkReal x763=((IkReal(0.999999985550000))*(cj1)); -IkReal x764=((cj1)*(pz)); -IkReal x765=((py)*(sj0)); -IkReal x766=((r00)*(sj0)); -IkReal x767=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x768=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x769=((IkReal(0.00130799958144020))*(cj5)); -IkReal x770=((IkReal(0.211999996936600))*(pz)); -IkReal x771=((pz)*(sj1)); -IkReal x772=((sj3)*(x724)); -IkReal x773=((cj1)*(x753)); -IkReal x774=((r00)*(x758)); -IkReal x775=((IkReal(0.999999985550000))*(x726)); -IkReal x776=((IkReal(2.00000000000000))*(pz)*(r01)); -IkReal x777=((sj3)*(x723)); -IkReal x778=((cj5)*(x724)); -IkReal x779=((IkReal(0.999999985550000))*(x725)); -IkReal x780=((IkReal(2.00000000000000))*(x726)); -IkReal x781=((IkReal(0.999999985550000))*(x727)); -IkReal x782=((px)*(r02)*(sj0)); -IkReal x783=((cj5)*(x723)); -evalcond[0]=((((IkReal(0.112000000000000))*(x772)))+(((IkReal(0.106000000000000))*(x723)))+(((IkReal(-1.00000000000000))*(x733)*(x758)))+(((x741)*(x765)))+(((x723)*(x749)))+(((x741)*(x752)))+(x771)+(((x750)*(x762)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(0.112000000000000))*(x777)))+(((IkReal(-1.00000000000000))*(x736)*(x762)))+(((IkReal(-1.00000000000000))*(x737)*(x752)))+(((IkReal(-0.106000000000000))*(x724)))+(x764)+(((IkReal(-0.000169999997543500))*(py)*(x732)))+(((x732)*(x733)))+(((IkReal(-1.00000000000000))*(x724)*(x749)))); -evalcond[2]=((((IkReal(-0.999999680000154))*(x724)*(x731)))+(((x738)*(x777)))+(((IkReal(-0.999999985550000))*(x774)))+(((x730)*(x741)))+(((IkReal(0.000799999744000123))*(x723)*(x731)))+(((x740)*(x772)))+(x757)+(((x728)*(x741)))+(((x754)*(x763)))); -evalcond[3]=((((IkReal(-0.999999680000154))*(x723)*(x731)))+(((IkReal(-1.00000000000000))*(x742)*(x755)))+(((IkReal(-0.000799999744000123))*(x724)*(x731)))+(((x740)*(x777)))+(((IkReal(-1.00000000000000))*(x738)*(x772)))+(((IkReal(-1.00000000000000))*(x730)*(x737)))+(x746)+(((IkReal(-1.00000000000000))*(x728)*(x737)))+(((IkReal(0.999999985550000))*(r00)*(x732)))); -evalcond[4]=((((r00)*(x732)*(x744)))+(((IkReal(-1.00000000000000))*(x745)*(x772)))+(((r00)*(x750)))+(((IkReal(-1.00000000000000))*(px)*(x732)*(x760)))+(((IkReal(-1.00000000000000))*(x773)))+(((IkReal(-1.00000000000000))*(pz)*(x737)*(x754)))+(((IkReal(-1.00000000000000))*(r02)*(x733)*(x742)))+(((cj0)*(x736)*(x760)))+(((sj1)*(x728)*(x756)))+(((IkReal(-1.00000000000000))*(x723)*(x747)))+(((sj1)*(x730)*(x756)))+(((IkReal(-1.00000000000000))*(cj3)*(x723)*(x745)))+(((IkReal(-0.999999985550000))*(x732)*(x751)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(cj0)*(x741)*(x751)))+(((IkReal(-1.00000000000000))*(cj3)*(x724)*(x745)))+(((IkReal(-1.00000000000000))*(cj1)*(x730)*(x756)))+(((x745)*(x777)))+(((IkReal(-1.00000000000000))*(cj1)*(x728)*(x756)))+(((IkReal(1.80199997396110e-5))*(x766)))+(((IkReal(-1.00000000000000))*(pz)*(x741)*(x766)))+(((IkReal(-1.00000000000000))*(sj1)*(x753)))+(((x741)*(x782)))+(((r00)*(x736)))+(((IkReal(0.105999998468300))*(x730)))+(((IkReal(0.105999998468300))*(x728)))+(((IkReal(0.999999985550000))*(x746)*(x765)))+(((cj0)*(x733)*(x746)))+(((pz)*(x741)*(x754)))+(((IkReal(-1.80199997396110e-5))*(x754)))+(((IkReal(-1.00000000000000))*(x724)*(x747)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x726)*(x730)*(x737)))+(((IkReal(1.90239939123229e-5))*(x724)*(x731)))+(((r00)*(x732)*(x779)))+(((r00)*(x732)*(x775)))+(((IkReal(-1.00000000000000))*(x725)*(x728)*(x737)))+(((IkReal(-0.0237799923904037))*(x723)*(x731)))+(((x727)*(x728)*(x737)))+(((IkReal(-0.0112359998376398))*(r00)*(x732)))+(((x735)*(x742)*(x748)))+(((IkReal(-1.00000000000000))*(x750)*(x776)))+(((IkReal(-0.0112360000000000))*(x746)))+(((IkReal(-1.00000000000000))*(x746)*(x780)))+(((pp)*(x746)))+(((x728)*(x736)*(x743)))+(((x735)*(x736)*(x759)))+(((IkReal(0.212000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x727)*(x730)*(x737)))+(((IkReal(-0.0237439924019236))*(x783)))+(((x725)*(x730)*(x737)))+(((IkReal(-1.00000000000000))*(x769)*(x772)))+(((IkReal(-1.00000000000000))*(x730)*(x736)*(x743)))+(((IkReal(-1.00000000000000))*(x726)*(x728)*(x737)))+(((IkReal(-1.00000000000000))*(x732)*(x735)*(x743)))+(((x734)*(x742)*(x753)))+(((IkReal(-1.00000000000000))*(r00)*(x732)*(x781)))+(((IkReal(1.89951939215389e-5))*(x778)))+(((IkReal(0.212000000000000))*(x739)))+(((IkReal(0.212000000000000))*(x735)))+(((x732)*(x734)*(x735)))+(((x732)*(x734)*(x739)))+(((IkReal(-1.00000000000000))*(x726)*(x742)*(x755)))+(((x728)*(x768)))+(((IkReal(-1.00000000000000))*(x761)*(x777)))+(((IkReal(-2.00000000000000))*(x739)*(x764)))+(((x725)*(x742)*(x755)))+(((IkReal(-1.00000000000000))*(x727)*(x742)*(x755)))+(((IkReal(0.0112359998376398))*(r01)*(x742)))+(((x730)*(x768)))); -evalcond[7]=((((IkReal(-0.0237439924019236))*(x778)))+(((IkReal(3.60399994792220e-5))*(r02)*(x752)))+(((x725)*(x728)*(x741)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x735)*(x748)))+(((x735)*(x743)*(x758)))+(((x727)*(x730)*(x741)))+(((IkReal(-1.89951939215389e-5))*(x783)))+(((IkReal(-1.00000000000000))*(x754)*(x770)))+(((IkReal(-1.00000000000000))*(x774)*(x775)))+(((IkReal(-1.00000000000000))*(x774)*(x779)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x734)*(x752)))+(((x774)*(x781)))+(((IkReal(-2.00000000000000))*(x739)*(x771)))+(((pp)*(x757)))+(((x730)*(x743)*(x750)))+(((x727)*(x754)*(x763)))+(((IkReal(-1.00000000000000))*(x727)*(x728)*(x741)))+(((IkReal(0.0112359998376398))*(cj1)*(x754)))+(((x769)*(x777)))+(((IkReal(0.0112360000000000))*(x757)))+(((IkReal(-1.00000000000000))*(x735)*(x750)*(x759)))+(((IkReal(-0.211999996936600))*(x782)))+(((IkReal(0.211999996936600))*(cj0)*(x751)))+(((x766)*(x770)))+(((IkReal(-1.00000000000000))*(x728)*(x743)*(x750)))+(((x726)*(x754)*(x763)))+(((x726)*(x728)*(x741)))+(((x726)*(x730)*(x741)))+(((IkReal(-1.00000000000000))*(x757)*(x780)))+(((IkReal(-1.00000000000000))*(x734)*(x739)*(x758)))+(((IkReal(-1.00000000000000))*(x725)*(x730)*(x741)))+(((IkReal(-0.0112359998376398))*(x774)))+(((IkReal(-1.00000000000000))*(x734)*(x735)*(x758)))+(((IkReal(3.60399994792220e-5))*(sj0)*(x751)))+(((IkReal(-1.00000000000000))*(x736)*(x776)))+(((x728)*(x767)))+(((IkReal(-1.00000000000000))*(x761)*(x772)))+(((IkReal(-1.00000000000000))*(x725)*(x754)*(x763)))+(((IkReal(-0.0237799923904037))*(x724)*(x731)))+(((IkReal(-1.00000000000000))*(x729)*(x730)))+(((IkReal(-1.00000000000000))*(x728)*(x729)))+(((IkReal(-1.90239939123229e-5))*(x723)*(x731)))+(((x730)*(x767)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x784=((cj0)*(sj1)); -IkReal x785=((IkReal(0.281562495931422))*(px)); -IkReal x786=((IkReal(1750.00000000000))*(pz)); -IkReal x787=((sj1)*(sj3)); -IkReal x788=((cj1)*(cj3)); -IkReal x789=((IkReal(1656.24997606719))*(cj1)); -IkReal x790=((px)*(sj0)); -IkReal x791=((cj3)*(sj1)); -IkReal x792=((cj0)*(py)); -IkReal x793=((cj1)*(sj3)); -IkReal x794=((py)*(sj0)); -IkReal x795=((IkReal(0.297499995701125))*(px)); -IkReal x796=((IkReal(1749.99997471250))*(py)); -IkReal x797=((IkReal(1656.25000000000))*(pz)); -if( IKabs(((gconst1)*(((((IkReal(1749.99997471250))*(x787)*(x790)))+(((IkReal(-0.297499995701125))*(x787)*(x794)))+(((x786)*(x793)))+(((x786)*(x791)))+(((IkReal(0.281562495931422))*(cj1)*(x794)))+(((sj1)*(x797)))+(((IkReal(-1749.99997471250))*(x788)*(x790)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj0)*(x788)*(x795)))+(((IkReal(-1.00000000000000))*(sj3)*(x784)*(x795)))+(((IkReal(-1.00000000000000))*(sj3)*(x784)*(x796)))+(((cj0)*(cj1)*(x785)))+(((IkReal(-1.00000000000000))*(x789)*(x790)))+(((IkReal(0.297499995701125))*(x788)*(x794)))+(((IkReal(1749.99997471250))*(x788)*(x792)))+(((x789)*(x792))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((IkReal(175.562500000000))+(((x786)*(x787)))+(((IkReal(0.297499995701125))*(x793)*(x794)))+(((cj0)*(x793)*(x795)))+(((IkReal(1749.99997471250))*(x792)*(x793)))+(((x784)*(x785)))+(((IkReal(-1.00000000000000))*(cj1)*(x797)))+(((IkReal(1656.24997606719))*(py)*(x784)))+(((IkReal(-1.00000000000000))*(x786)*(x788)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1656.24997606719))*(sj1)*(x790)))+(((IkReal(-1749.99997471250))*(x790)*(x791)))+(((IkReal(-1749.99997471250))*(x790)*(x793)))+(((IkReal(0.297499995701125))*(x791)*(x794)))+(((cj3)*(x784)*(x796)))+(((cj3)*(x784)*(x795)))+(((IkReal(0.281562495931422))*(sj1)*(x794))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst1)*(((((IkReal(1749.99997471250))*(x787)*(x790)))+(((IkReal(-0.297499995701125))*(x787)*(x794)))+(((x786)*(x793)))+(((x786)*(x791)))+(((IkReal(0.281562495931422))*(cj1)*(x794)))+(((sj1)*(x797)))+(((IkReal(-1749.99997471250))*(x788)*(x790)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj0)*(x788)*(x795)))+(((IkReal(-1.00000000000000))*(sj3)*(x784)*(x795)))+(((IkReal(-1.00000000000000))*(sj3)*(x784)*(x796)))+(((cj0)*(cj1)*(x785)))+(((IkReal(-1.00000000000000))*(x789)*(x790)))+(((IkReal(0.297499995701125))*(x788)*(x794)))+(((IkReal(1749.99997471250))*(x788)*(x792)))+(((x789)*(x792)))))), ((gconst1)*(((IkReal(175.562500000000))+(((x786)*(x787)))+(((IkReal(0.297499995701125))*(x793)*(x794)))+(((cj0)*(x793)*(x795)))+(((IkReal(1749.99997471250))*(x792)*(x793)))+(((x784)*(x785)))+(((IkReal(-1.00000000000000))*(cj1)*(x797)))+(((IkReal(1656.24997606719))*(py)*(x784)))+(((IkReal(-1.00000000000000))*(x786)*(x788)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1656.24997606719))*(sj1)*(x790)))+(((IkReal(-1749.99997471250))*(x790)*(x791)))+(((IkReal(-1749.99997471250))*(x790)*(x793)))+(((IkReal(0.297499995701125))*(x791)*(x794)))+(((cj3)*(x784)*(x796)))+(((cj3)*(x784)*(x795)))+(((IkReal(0.281562495931422))*(sj1)*(x794))))))); -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[8]; -IkReal x798=IKsin(j2); -IkReal x799=IKcos(j2); -IkReal x800=(py)*(py); -IkReal x801=(pz)*(pz); -IkReal x802=(px)*(px); -IkReal x803=((cj0)*(r00)); -IkReal x804=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x805=((r01)*(sj0)); -IkReal x806=((cj3)*(cj5)); -IkReal x807=((sj0)*(sj1)); -IkReal x808=((IkReal(0.999999985550000))*(px)); -IkReal x809=((IkReal(0.000339999995087000))*(py)); -IkReal x810=((pz)*(r02)); -IkReal x811=((py)*(sj1)); -IkReal x812=((IkReal(0.000169999997543500))*(sj1)); -IkReal x813=((IkReal(0.999999680000154))*(cj5)); -IkReal x814=((px)*(r00)); -IkReal x815=((IkReal(0.000799999744000123))*(cj5)); -IkReal x816=((IkReal(0.000169999997543500))*(cj1)); -IkReal x817=((cj0)*(sj1)); -IkReal x818=((IkReal(1.99999997110000))*(px)); -IkReal x819=((IkReal(0.000169999997543500))*(pz)); -IkReal x820=((IkReal(0.112000000000000))*(sj5)); -IkReal x821=((cj1)*(r02)); -IkReal x822=((IkReal(0.106000000000000))*(sj5)); -IkReal x823=((IkReal(0.000339999995087000))*(px)); -IkReal x824=((IkReal(0.112000000000000))*(cj3)); -IkReal x825=((cj1)*(py)); -IkReal x826=((py)*(r02)); -IkReal x827=((cj0)*(px)); -IkReal x828=((px)*(r01)); -IkReal x829=((cj0)*(r01)); -IkReal x830=((IkReal(0.999999985550000))*(r01)); -IkReal x831=((IkReal(0.999999985550000))*(pz)); -IkReal x832=((r02)*(sj1)); -IkReal x833=((cj1)*(sj0)); -IkReal x834=((IkReal(1.99999997110000))*(cj0)); -IkReal x835=((IkReal(0.000169999997543500))*(r02)); -IkReal x836=((IkReal(1.04639966515216e-6))*(cj5)); -IkReal x837=((IkReal(0.999999985550000))*(cj0)); -IkReal x838=((IkReal(0.999999985550000))*(cj1)); -IkReal x839=((cj1)*(pz)); -IkReal x840=((py)*(sj0)); -IkReal x841=((r00)*(sj0)); -IkReal x842=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x843=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x844=((IkReal(0.00130799958144020))*(cj5)); -IkReal x845=((IkReal(0.211999996936600))*(pz)); -IkReal x846=((pz)*(sj1)); -IkReal x847=((sj3)*(x799)); -IkReal x848=((cj1)*(x828)); -IkReal x849=((r00)*(x833)); -IkReal x850=((IkReal(0.999999985550000))*(x801)); -IkReal x851=((IkReal(2.00000000000000))*(pz)*(r01)); -IkReal x852=((sj3)*(x798)); -IkReal x853=((cj5)*(x799)); -IkReal x854=((IkReal(0.999999985550000))*(x800)); -IkReal x855=((IkReal(2.00000000000000))*(x801)); -IkReal x856=((IkReal(0.999999985550000))*(x802)); -IkReal x857=((px)*(r02)*(sj0)); -IkReal x858=((cj5)*(x798)); -evalcond[0]=((((x798)*(x824)))+(((x816)*(x840)))+(((IkReal(-1.00000000000000))*(x808)*(x833)))+(((x816)*(x827)))+(((IkReal(0.106000000000000))*(x798)))+(x846)+(((IkReal(0.112000000000000))*(x847)))+(((x825)*(x837)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x812)*(x827)))+(((IkReal(0.112000000000000))*(x852)))+(((IkReal(-1.00000000000000))*(x799)*(x824)))+(((IkReal(-1.00000000000000))*(x811)*(x837)))+(((IkReal(-0.000169999997543500))*(py)*(x807)))+(((IkReal(-0.106000000000000))*(x799)))+(x839)+(((x807)*(x808)))); -evalcond[2]=((((x813)*(x852)))+(((x829)*(x838)))+(((IkReal(-0.999999985550000))*(x849)))+(x832)+(((x805)*(x816)))+(((x815)*(x847)))+(((IkReal(-0.999999680000154))*(x799)*(x806)))+(((x803)*(x816)))+(((IkReal(0.000799999744000123))*(x798)*(x806)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x803)*(x812)))+(((IkReal(-0.999999680000154))*(x798)*(x806)))+(((IkReal(-1.00000000000000))*(x813)*(x847)))+(((IkReal(-0.000799999744000123))*(x799)*(x806)))+(((x815)*(x852)))+(((IkReal(-1.00000000000000))*(x817)*(x830)))+(x821)+(((IkReal(-1.00000000000000))*(x805)*(x812)))+(((IkReal(0.999999985550000))*(r00)*(x807)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(px)*(x807)*(x835)))+(((IkReal(-1.00000000000000))*(r02)*(x808)*(x817)))+(((IkReal(-0.999999985550000))*(x807)*(x826)))+(((cj0)*(x811)*(x835)))+(((sj1)*(x805)*(x831)))+(((r00)*(x825)))+(((IkReal(-1.00000000000000))*(pz)*(x812)*(x829)))+(((IkReal(-1.00000000000000))*(x798)*(x822)))+(((IkReal(-1.00000000000000))*(x848)))+(((r00)*(x807)*(x819)))+(((sj1)*(x803)*(x831)))+(((IkReal(-1.00000000000000))*(cj3)*(x798)*(x820)))+(((IkReal(-1.00000000000000))*(x820)*(x847)))); -evalcond[5]=((((IkReal(0.105999998468300))*(x803)))+(((IkReal(0.105999998468300))*(x805)))+(((IkReal(-1.80199997396110e-5))*(x829)))+(((IkReal(-1.00000000000000))*(pz)*(x816)*(x841)))+(((IkReal(-1.00000000000000))*(cj0)*(x816)*(x826)))+(((IkReal(-1.00000000000000))*(sj1)*(x828)))+(((IkReal(1.80199997396110e-5))*(x841)))+(((r00)*(x811)))+(((IkReal(-1.00000000000000))*(cj1)*(x803)*(x831)))+(((IkReal(-1.00000000000000))*(cj1)*(x805)*(x831)))+(((IkReal(-1.00000000000000))*(x799)*(x822)))+(((x816)*(x857)))+(((x820)*(x852)))+(((IkReal(0.999999985550000))*(x821)*(x840)))+(((cj0)*(x808)*(x821)))+(((IkReal(-1.00000000000000))*(cj3)*(x799)*(x820)))+(((pz)*(x816)*(x829)))); -evalcond[6]=((((r00)*(x807)*(x854)))+(((r00)*(x807)*(x850)))+(((IkReal(-0.0237799923904037))*(x798)*(x806)))+(((x807)*(x809)*(x810)))+(((x807)*(x809)*(x814)))+(((IkReal(-1.00000000000000))*(x802)*(x817)*(x830)))+(((IkReal(-1.00000000000000))*(r00)*(x807)*(x856)))+(((IkReal(-1.00000000000000))*(x825)*(x851)))+(((IkReal(0.212000000000000))*(x814)))+(((IkReal(0.212000000000000))*(x810)))+(((IkReal(-0.0112360000000000))*(x821)))+(((IkReal(-1.00000000000000))*(x821)*(x855)))+(((x810)*(x811)*(x834)))+(((x800)*(x805)*(x812)))+(((x803)*(x811)*(x818)))+(((IkReal(-0.0112359998376398))*(r00)*(x807)))+(((IkReal(1.89951939215389e-5))*(x853)))+(((IkReal(0.212000000000000))*(py)*(r01)))+(((x800)*(x817)*(x830)))+(((IkReal(-1.00000000000000))*(x801)*(x805)*(x812)))+(((IkReal(-1.00000000000000))*(x801)*(x817)*(x830)))+(((x810)*(x817)*(x823)))+(((IkReal(-1.00000000000000))*(x802)*(x805)*(x812)))+(((IkReal(-1.00000000000000))*(x844)*(x847)))+(((x809)*(x817)*(x828)))+(((IkReal(-1.00000000000000))*(x836)*(x852)))+(((IkReal(-1.00000000000000))*(x807)*(x810)*(x818)))+(((x805)*(x843)))+(((x803)*(x843)))+(((pp)*(x821)))+(((IkReal(-2.00000000000000))*(x814)*(x839)))+(((IkReal(0.0112359998376398))*(r01)*(x817)))+(((x802)*(x803)*(x812)))+(((IkReal(-1.00000000000000))*(x800)*(x803)*(x812)))+(((IkReal(1.90239939123229e-5))*(x799)*(x806)))+(((IkReal(-0.0237439924019236))*(x858)))+(((IkReal(-1.00000000000000))*(x805)*(x811)*(x818)))+(((IkReal(-1.00000000000000))*(x801)*(x803)*(x812)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x811)*(x851)))+(((IkReal(-1.00000000000000))*(x809)*(x814)*(x833)))+(((IkReal(-1.00000000000000))*(x800)*(x805)*(x816)))+(((IkReal(-1.00000000000000))*(x804)*(x805)))+(((x801)*(x805)*(x816)))+(((x802)*(x829)*(x838)))+(((IkReal(0.211999996936600))*(cj0)*(x826)))+(((x802)*(x805)*(x816)))+(((IkReal(-1.00000000000000))*(x836)*(x847)))+(((IkReal(3.60399994792220e-5))*(sj0)*(x826)))+(((IkReal(-1.00000000000000))*(x810)*(x825)*(x834)))+(((IkReal(-1.00000000000000))*(x802)*(x803)*(x816)))+(((IkReal(-1.00000000000000))*(x849)*(x854)))+(((IkReal(-1.00000000000000))*(x849)*(x850)))+(((IkReal(-1.00000000000000))*(x803)*(x818)*(x825)))+(((IkReal(0.0112359998376398))*(cj1)*(x829)))+(((x844)*(x852)))+(((IkReal(3.60399994792220e-5))*(r02)*(x827)))+(((pp)*(x832)))+(((IkReal(-1.90239939123229e-5))*(x798)*(x806)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x809)*(x827)))+(((IkReal(-1.00000000000000))*(x832)*(x855)))+(((x800)*(x803)*(x816)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x810)*(x823)))+(((IkReal(-0.0237799923904037))*(x799)*(x806)))+(((IkReal(0.0112360000000000))*(x832)))+(((IkReal(-2.00000000000000))*(x814)*(x846)))+(((IkReal(-1.00000000000000))*(x803)*(x804)))+(((x805)*(x842)))+(((x803)*(x842)))+(((x801)*(x829)*(x838)))+(((IkReal(-0.211999996936600))*(x857)))+(((x801)*(x803)*(x816)))+(((x810)*(x818)*(x833)))+(((x805)*(x818)*(x825)))+(((IkReal(-1.00000000000000))*(x829)*(x845)))+(((x849)*(x856)))+(((IkReal(-1.00000000000000))*(x809)*(x810)*(x833)))+(((IkReal(-1.00000000000000))*(x800)*(x829)*(x838)))+(((IkReal(-0.0112359998376398))*(x849)))+(((x841)*(x845)))+(((IkReal(-1.89951939215389e-5))*(x858)))+(((IkReal(-0.0237439924019236))*(x853)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j3array[1], cj3array[1], sj3array[1]; -bool j3valid[1]={false}; -_nj3 = 1; -IkReal x859=((IkReal(26.4999996170750))*(cj5)); -IkReal x860=((sj0)*(sj1)); -IkReal x861=((cj0)*(sj1)); -IkReal x862=((IkReal(0.00450499993490275))*(cj5)); -IkReal x863=((cj1)*(pz)); -IkReal x864=((px)*(x860)); -if( IKabs(((IkReal(0.000269541778975741))*(((IKabs(cj5) != 0)?((IkReal)1/(cj5)):(IkReal)1.0e30))*(((((IkReal(3710.00113359029))*(r01)*(x861)))+(((IkReal(-1.00000000000000))*(py)*(x859)*(x861)))+(((IkReal(-1.00000000000000))*(py)*(x860)*(x862)))+(((IkReal(26.5000000000000))*(cj5)*(x863)))+(((IkReal(-1.56800000000000))*(cj5)))+(((IkReal(-125.000000000000))*(cj5)*(pp)))+(((IkReal(35000.0111999982))*(px)*(r00)))+(((x859)*(x864)))+(((IkReal(-1.00000000000000))*(px)*(x861)*(x862)))+(((IkReal(0.630700192710350))*(r01)*(x860)))+(((IkReal(-3710.00118719981))*(cj1)*(r02)))+(((IkReal(0.630700192710350))*(r00)*(x861)))+(((IkReal(-3710.00113359029))*(r00)*(x860)))+(((IkReal(35000.0111999982))*(pz)*(r02)))+(((IkReal(35000.0111999982))*(py)*(r01))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.528301886792453))+(((IkReal(0.00151785712092411))*(py)*(x860)))+(((IkReal(8.92857129955357))*(py)*(x861)))+(((IkReal(-8.92857129955357))*(x864)))+(((IkReal(0.00151785712092411))*(px)*(x861)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-8.92857142857143))*(x863))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.000269541778975741))*(((IKabs(cj5) != 0)?((IkReal)1/(cj5)):(IkReal)1.0e30))*(((((IkReal(3710.00113359029))*(r01)*(x861)))+(((IkReal(-1.00000000000000))*(py)*(x859)*(x861)))+(((IkReal(-1.00000000000000))*(py)*(x860)*(x862)))+(((IkReal(26.5000000000000))*(cj5)*(x863)))+(((IkReal(-1.56800000000000))*(cj5)))+(((IkReal(-125.000000000000))*(cj5)*(pp)))+(((IkReal(35000.0111999982))*(px)*(r00)))+(((x859)*(x864)))+(((IkReal(-1.00000000000000))*(px)*(x861)*(x862)))+(((IkReal(0.630700192710350))*(r01)*(x860)))+(((IkReal(-3710.00118719981))*(cj1)*(r02)))+(((IkReal(0.630700192710350))*(r00)*(x861)))+(((IkReal(-3710.00113359029))*(r00)*(x860)))+(((IkReal(35000.0111999982))*(pz)*(r02)))+(((IkReal(35000.0111999982))*(py)*(r01)))))))+IKsqr(((IkReal(-0.528301886792453))+(((IkReal(0.00151785712092411))*(py)*(x860)))+(((IkReal(8.92857129955357))*(py)*(x861)))+(((IkReal(-8.92857129955357))*(x864)))+(((IkReal(0.00151785712092411))*(px)*(x861)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-8.92857142857143))*(x863)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j3array[0]=IKatan2(((IkReal(0.000269541778975741))*(((IKabs(cj5) != 0)?((IkReal)1/(cj5)):(IkReal)1.0e30))*(((((IkReal(3710.00113359029))*(r01)*(x861)))+(((IkReal(-1.00000000000000))*(py)*(x859)*(x861)))+(((IkReal(-1.00000000000000))*(py)*(x860)*(x862)))+(((IkReal(26.5000000000000))*(cj5)*(x863)))+(((IkReal(-1.56800000000000))*(cj5)))+(((IkReal(-125.000000000000))*(cj5)*(pp)))+(((IkReal(35000.0111999982))*(px)*(r00)))+(((x859)*(x864)))+(((IkReal(-1.00000000000000))*(px)*(x861)*(x862)))+(((IkReal(0.630700192710350))*(r01)*(x860)))+(((IkReal(-3710.00118719981))*(cj1)*(r02)))+(((IkReal(0.630700192710350))*(r00)*(x861)))+(((IkReal(-3710.00113359029))*(r00)*(x860)))+(((IkReal(35000.0111999982))*(pz)*(r02)))+(((IkReal(35000.0111999982))*(py)*(r01)))))), ((IkReal(-0.528301886792453))+(((IkReal(0.00151785712092411))*(py)*(x860)))+(((IkReal(8.92857129955357))*(py)*(x861)))+(((IkReal(-8.92857129955357))*(x864)))+(((IkReal(0.00151785712092411))*(px)*(x861)))+(((IkReal(42.1159029649596))*(pp)))+(((IkReal(-8.92857142857143))*(x863))))); -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 x865=IKcos(j3); -IkReal x866=IKsin(j3); -IkReal x867=(px)*(px); -IkReal x868=(py)*(py); -IkReal x869=(pz)*(pz); -IkReal x870=((r00)*(sj0)); -IkReal x871=((cj0)*(r00)); -IkReal x872=((py)*(sj1)); -IkReal x873=((cj1)*(pz)); -IkReal x874=((cj0)*(r01)); -IkReal x875=((px)*(py)); -IkReal x876=((r01)*(sj0)); -IkReal x877=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x878=((cj0)*(r02)); -IkReal x879=((IkReal(3.60399994792220e-5))*(cj1)); -IkReal x880=((IkReal(0.105999966080016))*(cj5)); -IkReal x881=((IkReal(0.999999985550000))*(pz)); -IkReal x882=((px)*(sj1)); -IkReal x883=((IkReal(0.106000000000000))*(r02)); -IkReal x884=((IkReal(8.47999728640130e-5))*(cj5)); -IkReal x885=((IkReal(0.000169999997543500))*(pz)); -IkReal x886=((IkReal(0.211999996936600))*(cj1)); -IkReal x887=((IkReal(0.000339999995087000))*(pz)); -IkReal x888=((IkReal(0.105999998468300))*(cj1)); -IkReal x889=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x890=((IkReal(1.99999997110000))*(pz)); -IkReal x891=((IkReal(0.105999998468300))*(sj1)); -IkReal x892=((IkReal(0.999999985550000))*(x868)); -IkReal x893=((IkReal(0.999999985550000))*(x869)); -IkReal x894=((IkReal(0.000169999997543500))*(x867)); -IkReal x895=((IkReal(0.000169999997543500))*(x868)); -IkReal x896=((IkReal(0.000169999997543500))*(x869)); -IkReal x897=((px)*(r02)*(sj0)); -IkReal x898=((IkReal(0.999999985550000))*(x867)); -IkReal x899=((py)*(r02)*(sj0)); -IkReal x900=((IkReal(0.0237440000000000))*(x865)); -evalcond[0]=((IkReal(0.0125440000000000))+(((IkReal(-3.60399994792220e-5))*(cj0)*(x882)))+(((IkReal(-3.60399994792220e-5))*(sj0)*(x872)))+(((IkReal(0.211999996936600))*(sj0)*(x882)))+(((IkReal(0.212000000000000))*(x873)))+(((IkReal(-1.00000000000000))*(pp)))+(x900)+(((IkReal(-0.211999996936600))*(cj0)*(x872)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x876)*(x877)))+(((IkReal(-1.00000000000000))*(x871)*(x877)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((x865)*(x884)))+(((IkReal(8.95999713280138e-5))*(cj5)))+(((IkReal(-1.00000000000000))*(pz)*(r02)))+(((cj1)*(x883)))+(((x870)*(x891)))+(((IkReal(-1.00000000000000))*(py)*(r01)))+(((x866)*(x880)))+(((IkReal(-1.00000000000000))*(x874)*(x891)))); -evalcond[2]=((((sj1)*(x883)))+(((x874)*(x888)))+(((IkReal(-1.00000000000000))*(x870)*(x888)))+(((x876)*(x889)))+(((x865)*(x880)))+(((IkReal(-1.00000000000000))*(x876)*(x885)))+(((IkReal(0.000169999997543500))*(x899)))+(((IkReal(-0.999999985550000))*(x897)))+(((x870)*(x881)))+(((IkReal(0.999999985550000))*(py)*(x878)))+(((IkReal(-1.00000000000000))*(x871)*(x885)))+(((x871)*(x889)))+(((IkReal(0.000169999997543500))*(px)*(x878)))+(((IkReal(-1.00000000000000))*(x866)*(x884)))+(((IkReal(0.111999964160017))*(cj5)))+(((IkReal(-1.00000000000000))*(x874)*(x881)))); -evalcond[3]=((((x874)*(x894)))+(((x874)*(x896)))+(((IkReal(-0.212000000000000))*(r00)*(x872)))+(((IkReal(-1.00000000000000))*(py)*(x878)*(x887)))+(((IkReal(0.211999996936600))*(x871)*(x873)))+(((IkReal(-1.00000000000000))*(x870)*(x896)))+(((IkReal(-1.00000000000000))*(x870)*(x895)))+(((IkReal(-1.00000000000000))*(x879)*(x897)))+(((x876)*(x892)))+(((IkReal(3.60399994792220e-5))*(x870)*(x873)))+(((py)*(x878)*(x879)))+(((IkReal(1.99999997110000))*(x870)*(x875)))+(((IkReal(-0.0112359998376398))*(x876)))+(((IkReal(-0.0112359998376398))*(x871)))+(((IkReal(-1.00000000000000))*(px)*(x878)*(x886)))+(((IkReal(-1.00000000000000))*(x876)*(x893)))+(((IkReal(-1.00000000000000))*(x876)*(x898)))+(((IkReal(-0.0237800000000000))*(sj5)))+(((IkReal(-1.00000000000000))*(sj5)*(x900)))+(((IkReal(-3.60399994792220e-5))*(x873)*(x874)))+(((IkReal(0.211999996936600))*(x873)*(x876)))+(((px)*(x878)*(x890)))+(((IkReal(0.000339999995087000))*(x875)*(x876)))+(((x871)*(x898)))+(((IkReal(1.91011997239877e-6))*(x874)))+(((x870)*(x894)))+(((IkReal(-0.000339999995087000))*(x871)*(x875)))+(((IkReal(-1.91011997239877e-6))*(x870)))+(((IkReal(0.212000000000000))*(r01)*(x882)))+(((x890)*(x899)))+(((IkReal(-1.00000000000000))*(x871)*(x892)))+(((IkReal(-1.00000000000000))*(x871)*(x893)))+(((IkReal(1.99999997110000))*(x874)*(x875)))+(((IkReal(-1.00000000000000))*(x874)*(x895)))+(((x887)*(x897)))+(((IkReal(-1.00000000000000))*(x886)*(x899)))); -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 gconst1; -gconst1=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst2; -IkReal x901=((IkReal(13.9999955200021))*(cj5)); -gconst2=IKsign(((((IkReal(-1.00000000000000))*(x901)*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*(x901)*((sj3)*(sj3))))+(((IkReal(0.0105999966080016))*(cj5)*(sj3)))+(((IkReal(-13.2499957600020))*(cj3)*(cj5))))); -IkReal x902=((IkReal(1320.75471698113))*(cj5)); -dummyeval[0]=((((cj5)*(sj3)))+(((IkReal(-1.00000000000000))*(x902)*((sj3)*(sj3))))+(((IkReal(-1.00000000000000))*(x902)*((cj3)*(cj3))))+(((IkReal(-1250.00000000000))*(cj3)*(cj5)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x903=(px)*(px); -IkReal x904=(py)*(py); -IkReal x905=(pz)*(pz); -IkReal x906=((IkReal(0.0237440000000000))*(cj3)); -IkReal x907=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x908=((cj0)*(r00)); -IkReal x909=((r01)*(sj0)); -IkReal x910=((IkReal(0.000169999997543500))*(sj1)); -IkReal x911=((IkReal(0.000339999995087000))*(py)); -IkReal x912=((pz)*(sj1)); -IkReal x913=((r02)*(sj0)); -IkReal x914=((cj0)*(px)); -IkReal x915=((cj1)*(r01)); -IkReal x916=((IkReal(0.000169999997543500))*(cj0)); -IkReal x917=((IkReal(3.60399994792220e-5))*(sj1)); -IkReal x918=((py)*(r01)); -IkReal x919=((r00)*(sj0)); -IkReal x920=((IkReal(0.000169999997543500))*(pz)); -IkReal x921=((IkReal(0.105999998468300))*(cj0)); -IkReal x922=((px)*(r00)); -IkReal x923=((IkReal(1.99999997110000))*(pz)); -IkReal x924=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x925=((r01)*(sj1)); -IkReal x926=((cj1)*(r02)); -IkReal x927=((r02)*(sj1)); -IkReal x928=((IkReal(0.999999985550000))*(py)); -IkReal x929=((IkReal(0.999999985550000))*(cj0)); -IkReal x930=((IkReal(0.211999996936600))*(px)); -IkReal x931=((IkReal(0.0112359998376398))*(cj0)); -IkReal x932=((cj0)*(r02)); -IkReal x933=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x934=((IkReal(0.211999996936600))*(py)); -IkReal x935=((cj1)*(px)); -IkReal x936=((IkReal(0.000339999995087000))*(pz)); -IkReal x937=((IkReal(0.999999985550000))*(cj1)); -IkReal x938=((IkReal(0.000169999997543500))*(cj1)); -IkReal x939=((cj0)*(py)); -IkReal x940=((pz)*(r02)); -IkReal x941=((IkReal(0.211999996936600))*(pz)); -IkReal x942=((cj1)*(pz)); -IkReal x943=((IkReal(0.999999985550000))*(sj1)); -IkReal x944=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x945=((cj0)*(r01)); -IkReal x946=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x947=((IkReal(0.999999985550000))*(x905)); -IkReal x948=((IkReal(0.000169999997543500))*(x904)); -IkReal x949=((IkReal(0.999999985550000))*(x903)); -IkReal x950=((IkReal(0.000169999997543500))*(x905)); -IkReal x951=((IkReal(1.99999997110000))*(px)*(py)); -IkReal x952=((IkReal(0.000169999997543500))*(x903)); -IkReal x953=((IkReal(2.00000000000000))*(x905)); -IkReal x954=((IkReal(0.999999985550000))*(x904)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((IkReal(-0.999999985550000))*(x914)))+(((IkReal(-1.00000000000000))*(sj0)*(x928)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((py)*(x916)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-0.999999985550000))*(x909)))+(((IkReal(-0.999999985550000))*(x908)))+(((IkReal(-0.000169999997543500))*(x919)))+(((r01)*(x916)))); -evalcond[3]=((IkReal(0.0125440000000000))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x917)))+(((IkReal(-1.00000000000000))*(pp)))+(((sj0)*(sj1)*(x930)))+(((IkReal(-1.00000000000000))*(x914)*(x917)))+(((IkReal(0.212000000000000))*(x942)))+(x906)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x934)))); -evalcond[4]=((((IkReal(0.106000000000000))*(x926)))+(((IkReal(-1.00000000000000))*(x909)*(x933)))+(((IkReal(-1.00000000000000))*(x940)))+(((IkReal(-1.00000000000000))*(x922)))+(((IkReal(-1.00000000000000))*(x918)))+(((IkReal(-1.00000000000000))*(x921)*(x925)))+(((IkReal(-1.00000000000000))*(x908)*(x933)))+(((IkReal(0.105999998468300))*(sj1)*(x919)))); -evalcond[5]=((((x915)*(x929)))+(((x908)*(x938)))+(((x909)*(x938)))+(((IkReal(-1.00000000000000))*(x919)*(x937)))+(x927)); -evalcond[6]=((((x919)*(x943)))+(((IkReal(-1.00000000000000))*(x925)*(x929)))+(((IkReal(-1.00000000000000))*(x908)*(x910)))+(x926)+(((IkReal(-1.00000000000000))*(x909)*(x910)))); -evalcond[7]=((((IkReal(0.106000000000000))*(x927)))+(((IkReal(-0.105999998468300))*(cj1)*(x919)))+(((x915)*(x921)))+(((IkReal(0.999999985550000))*(pz)*(x919)))+(((x928)*(x932)))+(((IkReal(-0.999999985550000))*(px)*(x913)))+(((IkReal(0.000169999997543500))*(py)*(x913)))+(((x909)*(x944)))+(((IkReal(-1.00000000000000))*(x909)*(x920)))+(((IkReal(-1.00000000000000))*(x908)*(x920)))+(((IkReal(-1.00000000000000))*(pz)*(r01)*(x929)))+(((IkReal(0.000169999997543500))*(r02)*(x914)))+(((x908)*(x944)))); -evalcond[8]=((IkReal(-0.0237800000000000))+(((x919)*(x951)))+(((x919)*(x952)))+(((IkReal(-1.00000000000000))*(cj0)*(x907)*(x915)))+(((IkReal(1.91011997239877e-6))*(x945)))+(((IkReal(-1.00000000000000))*(cj1)*(x913)*(x934)))+(((IkReal(-0.211999996936600))*(x914)*(x926)))+(((IkReal(3.60399994792220e-5))*(x926)*(x939)))+(((IkReal(-1.00000000000000))*(x908)*(x954)))+(((IkReal(-1.00000000000000))*(x906)))+(((IkReal(-1.00000000000000))*(x909)*(x947)))+(((IkReal(-1.00000000000000))*(x909)*(x949)))+(((IkReal(0.212000000000000))*(px)*(x925)))+(((IkReal(-1.91011997239877e-6))*(x919)))+(((cj1)*(x907)*(x919)))+(((IkReal(1.99999997110000))*(x914)*(x918)))+(((py)*(x913)*(x923)))+(((cj1)*(x909)*(x941)))+(((IkReal(-1.00000000000000))*(r01)*(x904)*(x916)))+(((IkReal(-3.60399994792220e-5))*(x913)*(x935)))+(((IkReal(-1.00000000000000))*(px)*(x908)*(x911)))+(((IkReal(-0.212000000000000))*(py)*(r00)*(sj1)))+(((IkReal(-1.00000000000000))*(x919)*(x948)))+(((r02)*(x914)*(x923)))+(((IkReal(-1.00000000000000))*(x908)*(x947)))+(((x909)*(x954)))+(((IkReal(-1.00000000000000))*(x919)*(x950)))+(((IkReal(-1.00000000000000))*(pz)*(x911)*(x932)))+(((px)*(x909)*(x911)))+(((px)*(x913)*(x936)))+(((IkReal(-0.0112359998376398))*(x909)))+(((IkReal(-0.0112359998376398))*(x908)))+(((r01)*(x905)*(x916)))+(((x908)*(x949)))+(((cj1)*(x908)*(x941)))+(((r01)*(x903)*(x916)))); -evalcond[9]=((((IkReal(-1.00000000000000))*(x905)*(x925)*(x929)))+(((IkReal(-1.00000000000000))*(sj1)*(x909)*(x951)))+(((x904)*(x925)*(x929)))+(((IkReal(-1.00000000000000))*(x903)*(x925)*(x929)))+(((x911)*(x914)*(x925)))+(((IkReal(-1.00000000000000))*(x904)*(x908)*(x910)))+(((IkReal(-1.00000000000000))*(x905)*(x908)*(x910)))+(((sj1)*(x908)*(x951)))+(((IkReal(-0.0112360000000000))*(x926)))+(((IkReal(0.000339999995087000))*(r02)*(x912)*(x914)))+(((x909)*(x946)))+(((x903)*(x908)*(x910)))+(((x904)*(x919)*(x943)))+(((x904)*(x909)*(x910)))+(((pp)*(x926)))+(((IkReal(-1.00000000000000))*(x903)*(x919)*(x943)))+(((IkReal(-1.00000000000000))*(x926)*(x953)))+(((IkReal(1.99999997110000))*(py)*(x912)*(x932)))+(((IkReal(-2.00000000000000))*(py)*(pz)*(x915)))+(((x905)*(x919)*(x943)))+(((IkReal(-1.00000000000000))*(x903)*(x909)*(x910)))+(((IkReal(-2.00000000000000))*(x922)*(x942)))+(((x925)*(x931)))+(((IkReal(0.212000000000000))*(x940)))+(((px)*(sj1)*(x911)*(x919)))+(((IkReal(0.212000000000000))*(x922)))+(((IkReal(-0.0112359998376398))*(sj1)*(x919)))+(((IkReal(-1.00000000000000))*(x905)*(x909)*(x910)))+(((x911)*(x912)*(x913)))+(((x908)*(x946)))+(((IkReal(-1.99999997110000))*(px)*(x912)*(x913)))+(((IkReal(0.212000000000000))*(x918)))); -evalcond[10]=((((IkReal(-1.00000000000000))*(x904)*(x919)*(x937)))+(((x919)*(x941)))+(((IkReal(-1.00000000000000))*(x911)*(x913)*(x942)))+(((x915)*(x931)))+(((x905)*(x915)*(x929)))+(((IkReal(-1.00000000000000))*(x904)*(x915)*(x929)))+(((x908)*(x924)))+(((x904)*(x908)*(x938)))+(((IkReal(-1.00000000000000))*(x914)*(x926)*(x936)))+(((IkReal(-1.99999997110000))*(py)*(x908)*(x935)))+(((IkReal(3.60399994792220e-5))*(py)*(x913)))+(((IkReal(-1.00000000000000))*(x911)*(x919)*(x935)))+(((IkReal(-2.00000000000000))*(x912)*(x922)))+(((x903)*(x919)*(x937)))+(((IkReal(-1.00000000000000))*(x911)*(x914)*(x915)))+(((x905)*(x909)*(x938)))+(((x913)*(x923)*(x935)))+(((IkReal(-1.00000000000000))*(x927)*(x953)))+(((pp)*(x927)))+(((IkReal(-1.00000000000000))*(x941)*(x945)))+(((x905)*(x908)*(x938)))+(((x909)*(x924)))+(((IkReal(-0.0112359998376398))*(cj1)*(x919)))+(((IkReal(-1.00000000000000))*(x903)*(x908)*(x938)))+(((IkReal(0.0112360000000000))*(x927)))+(((IkReal(-1.00000000000000))*(x913)*(x930)))+(((IkReal(-2.00000000000000))*(x912)*(x918)))+(((IkReal(-1.00000000000000))*(x905)*(x919)*(x937)))+(((IkReal(1.99999997110000))*(py)*(x909)*(x935)))+(((x903)*(x909)*(x938)))+(((IkReal(-1.00000000000000))*(x904)*(x909)*(x938)))+(((x903)*(x915)*(x929)))+(((IkReal(3.60399994792220e-5))*(r02)*(x914)))+(((IkReal(-1.00000000000000))*(x923)*(x926)*(x939)))+(((IkReal(-1.00000000000000))*(x907)*(x909)))+(((IkReal(-1.00000000000000))*(x907)*(x908)))+(((x932)*(x934)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst3; -gconst3=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst4; -gconst4=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x955=((py)*(r02)); -IkReal x956=((IkReal(1750.00000000000))*(cj1)); -IkReal x957=((px)*(sj3)); -IkReal x958=((cj3)*(pz)); -IkReal x959=((cj0)*(sj1)); -IkReal x960=((IkReal(1749.99997471250))*(r02)); -IkReal x961=((px)*(r01)); -IkReal x962=((r00)*(sj3)); -IkReal x963=((pz)*(sj3)); -IkReal x964=((pz)*(r01)); -IkReal x965=((IkReal(0.297499995701125))*(sj3)); -IkReal x966=((IkReal(1656.25000000000))*(cj1)); -IkReal x967=((IkReal(0.297499995701125))*(cj3)); -IkReal x968=((pz)*(r00)); -IkReal x969=((cj3)*(px)); -IkReal x970=((cj3)*(py)); -IkReal x971=((px)*(r02)); -IkReal x972=((IkReal(0.297499995701125))*(r01)); -IkReal x973=((sj0)*(sj1)); -IkReal x974=((IkReal(0.281562495931422))*(x959)); -IkReal x975=((IkReal(1656.24997606719))*(x973)); -IkReal x976=((IkReal(1749.99997471250))*(x973)); -IkReal x977=((py)*(x973)); -IkReal x978=((IkReal(0.281562495931422))*(x973)); -if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(r00)*(x956)*(x970)))+(((IkReal(-0.297499995701125))*(r00)*(x958)*(x973)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x966)))+(((IkReal(-1.00000000000000))*(x955)*(x974)))+(((IkReal(-1.00000000000000))*(x965)*(x977)))+(((x958)*(x959)*(x972)))+(((x961)*(x966)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r01)*(x958)*(x976)))+(((IkReal(-1749.99997471250))*(r00)*(x958)*(x959)))+(((IkReal(-1.00000000000000))*(x964)*(x975)))+(((IkReal(-1656.24997606719))*(x959)*(x968)))+(((IkReal(1656.24997606719))*(x959)*(x971)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x967)))+(((cj3)*(x956)*(x961)))+(((x956)*(x963)))+(((x971)*(x978)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x959)))+(((x967)*(x971)*(x973)))+(((x964)*(x974)))+(((x957)*(x976)))+(((x959)*(x960)*(x969)))+(((cj3)*(x955)*(x976)))+(((x955)*(x975)))+(((IkReal(-1.00000000000000))*(x968)*(x978)))+(((IkReal(-0.297499995701125))*(x957)*(x959))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(px)*(x975)))+(((IkReal(-0.297499995701125))*(pz)*(x962)*(x973)))+(((IkReal(-1.00000000000000))*(pz)*(x966)))+(((IkReal(-1.00000000000000))*(py)*(x956)*(x962)))+(((IkReal(1749.99997471250))*(x959)*(x970)))+(((IkReal(-1.00000000000000))*(x969)*(x976)))+(((x959)*(x963)*(x972)))+(((IkReal(-1749.99997471250))*(pz)*(x959)*(x962)))+(((IkReal(1656.24997606719))*(py)*(x959)))+(((x957)*(x959)*(x960)))+(((px)*(x959)*(x967)))+(((IkReal(0.281562495931422))*(x977)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x965)))+(((IkReal(-1.00000000000000))*(x956)*(x958)))+(((IkReal(-1.00000000000000))*(r01)*(x963)*(x976)))+(((r01)*(x956)*(x957)))+(((sj3)*(x955)*(x976)))+(((IkReal(185.500000000000))*(cj3)))+(((x967)*(x977)))+(((IkReal(0.297499995701125))*(r02)*(x957)*(x973)))+(((px)*(x974))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(r00)*(x956)*(x970)))+(((IkReal(-0.297499995701125))*(r00)*(x958)*(x973)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x966)))+(((IkReal(-1.00000000000000))*(x955)*(x974)))+(((IkReal(-1.00000000000000))*(x965)*(x977)))+(((x958)*(x959)*(x972)))+(((x961)*(x966)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r01)*(x958)*(x976)))+(((IkReal(-1749.99997471250))*(r00)*(x958)*(x959)))+(((IkReal(-1.00000000000000))*(x964)*(x975)))+(((IkReal(-1656.24997606719))*(x959)*(x968)))+(((IkReal(1656.24997606719))*(x959)*(x971)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x967)))+(((cj3)*(x956)*(x961)))+(((x956)*(x963)))+(((x971)*(x978)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x959)))+(((x967)*(x971)*(x973)))+(((x964)*(x974)))+(((x957)*(x976)))+(((x959)*(x960)*(x969)))+(((cj3)*(x955)*(x976)))+(((x955)*(x975)))+(((IkReal(-1.00000000000000))*(x968)*(x978)))+(((IkReal(-0.297499995701125))*(x957)*(x959)))))), ((gconst4)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(px)*(x975)))+(((IkReal(-0.297499995701125))*(pz)*(x962)*(x973)))+(((IkReal(-1.00000000000000))*(pz)*(x966)))+(((IkReal(-1.00000000000000))*(py)*(x956)*(x962)))+(((IkReal(1749.99997471250))*(x959)*(x970)))+(((IkReal(-1.00000000000000))*(x969)*(x976)))+(((x959)*(x963)*(x972)))+(((IkReal(-1749.99997471250))*(pz)*(x959)*(x962)))+(((IkReal(1656.24997606719))*(py)*(x959)))+(((x957)*(x959)*(x960)))+(((px)*(x959)*(x967)))+(((IkReal(0.281562495931422))*(x977)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x965)))+(((IkReal(-1.00000000000000))*(x956)*(x958)))+(((IkReal(-1.00000000000000))*(r01)*(x963)*(x976)))+(((r01)*(x956)*(x957)))+(((sj3)*(x955)*(x976)))+(((IkReal(185.500000000000))*(cj3)))+(((x967)*(x977)))+(((IkReal(0.297499995701125))*(r02)*(x957)*(x973)))+(((px)*(x974))))))); -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[4]; -IkReal x979=IKsin(j2); -IkReal x980=IKcos(j2); -IkReal x981=((r00)*(sj0)); -IkReal x982=((r01)*(sj0)); -IkReal x983=((py)*(sj1)); -IkReal x984=((IkReal(0.999999985550000))*(r02)); -IkReal x985=((px)*(sj1)); -IkReal x986=((IkReal(0.999999985550000))*(sj0)); -IkReal x987=((IkReal(0.000169999997543500))*(cj0)); -IkReal x988=((IkReal(0.000169999997543500))*(sj0)); -IkReal x989=((cj1)*(px)); -IkReal x990=((pz)*(sj1)); -IkReal x991=((cj0)*(r00)); -IkReal x992=((cj1)*(pz)); -IkReal x993=((IkReal(1.00000000000000))*(r01)); -IkReal x994=((cj1)*(py)); -IkReal x995=((IkReal(0.999999985550000))*(cj0)); -IkReal x996=((IkReal(0.106000000000000))*(x979)); -IkReal x997=((IkReal(0.106000000000000))*(x980)); -IkReal x998=((IkReal(0.112000000000000))*(x980)); -IkReal x999=((IkReal(0.112000000000000))*(x979)); -IkReal x1000=((cj3)*(x999)); -IkReal x1001=((sj3)*(x998)); -IkReal x1002=((sj3)*(x999)); -IkReal x1003=((cj3)*(x998)); -IkReal x1004=((x1003)+(x997)); -IkReal x1005=((x1001)+(x1000)+(x996)); -evalcond[0]=((((x994)*(x995)))+(x1005)+(((x988)*(x994)))+(((IkReal(-1.00000000000000))*(x986)*(x989)))+(((x987)*(x989)))+(x990)); -evalcond[1]=((IkReal(-0.106000000000000))+(x1002)+(((IkReal(-1.00000000000000))*(x1004)))+(((IkReal(-1.00000000000000))*(x983)*(x995)))+(((x985)*(x986)))+(x992)+(((IkReal(-1.00000000000000))*(x985)*(x987)))+(((IkReal(-1.00000000000000))*(x983)*(x988)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x987)*(x990)))+(((IkReal(-1.00000000000000))*(x1005)))+(((IkReal(0.999999985550000))*(x982)*(x990)))+(((IkReal(-1.00000000000000))*(sj0)*(x983)*(x984)))+(((r00)*(x994)))+(((IkReal(-1.00000000000000))*(cj0)*(x984)*(x985)))+(((r02)*(x983)*(x987)))+(((IkReal(0.000169999997543500))*(x981)*(x990)))+(((IkReal(0.999999985550000))*(x990)*(x991)))+(((IkReal(-1.00000000000000))*(x989)*(x993)))+(((IkReal(-1.00000000000000))*(r02)*(x985)*(x988)))); -evalcond[3]=((((IkReal(0.105999998468300))*(x991)))+(((IkReal(-0.999999985550000))*(x991)*(x992)))+(((IkReal(-0.000169999997543500))*(x981)*(x992)))+(((r01)*(x987)*(x992)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(x1002)+(((IkReal(-1.00000000000000))*(x1004)))+(((sj0)*(x984)*(x994)))+(((IkReal(-1.00000000000000))*(r02)*(x987)*(x994)))+(((IkReal(1.80199997396110e-5))*(x981)))+(((IkReal(0.105999998468300))*(x982)))+(((r00)*(x983)))+(((IkReal(-1.00000000000000))*(x985)*(x993)))+(((cj0)*(x984)*(x989)))+(((r02)*(x988)*(x989)))+(((IkReal(-0.999999985550000))*(x982)*(x992)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1006=((px)*(sj1)); -IkReal x1007=((IkReal(0.281562495931422))*(cj0)); -IkReal x1008=((cj1)*(sj0)); -IkReal x1009=((IkReal(0.281562495931422))*(py)); -IkReal x1010=((IkReal(1749.99997471250))*(sj3)); -IkReal x1011=((IkReal(1749.99997471250))*(cj3)); -IkReal x1012=((cj0)*(cj1)); -IkReal x1013=((IkReal(0.297499995701125))*(sj3)); -IkReal x1014=((IkReal(0.297499995701125))*(cj3)); -IkReal x1015=((IkReal(1656.25000000000))*(pz)); -IkReal x1016=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x1017=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x1018=((IkReal(1656.24997606719))*(cj0)*(py)); -IkReal x1019=((py)*(sj0)*(sj1)); -IkReal x1020=((cj0)*(py)*(sj1)); -if( IKabs(((gconst3)*(((((py)*(x1011)*(x1012)))+(((x1008)*(x1009)))+(((IkReal(-1656.24997606719))*(px)*(x1008)))+(((px)*(x1012)*(x1014)))+(((sj1)*(x1016)))+(((sj1)*(x1015)))+(((IkReal(-185.500000000000))*(sj3)))+(((py)*(x1008)*(x1014)))+(((IkReal(-1.00000000000000))*(cj0)*(x1006)*(x1013)))+(((sj0)*(x1006)*(x1010)))+(((IkReal(-1.00000000000000))*(x1013)*(x1019)))+(((cj1)*(x1017)))+(((IkReal(1656.24997606719))*(py)*(x1012)))+(((IkReal(-1.00000000000000))*(px)*(x1008)*(x1011)))+(((IkReal(-1.00000000000000))*(x1010)*(x1020)))+(((cj1)*(px)*(x1007))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(175.562500000000))+(((x1014)*(x1019)))+(((x1011)*(x1020)))+(((py)*(x1010)*(x1012)))+(((px)*(x1012)*(x1013)))+(((sj0)*(sj1)*(x1009)))+(((sj1)*(x1017)))+(((sj1)*(x1018)))+(((cj0)*(x1006)*(x1014)))+(((py)*(x1008)*(x1013)))+(((IkReal(-1.00000000000000))*(sj0)*(x1006)*(x1011)))+(((IkReal(-1.00000000000000))*(cj1)*(x1016)))+(((IkReal(-1.00000000000000))*(cj1)*(x1015)))+(((x1006)*(x1007)))+(((IkReal(-1656.24997606719))*(sj0)*(x1006)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x1008)*(x1010))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst3)*(((((py)*(x1011)*(x1012)))+(((x1008)*(x1009)))+(((IkReal(-1656.24997606719))*(px)*(x1008)))+(((px)*(x1012)*(x1014)))+(((sj1)*(x1016)))+(((sj1)*(x1015)))+(((IkReal(-185.500000000000))*(sj3)))+(((py)*(x1008)*(x1014)))+(((IkReal(-1.00000000000000))*(cj0)*(x1006)*(x1013)))+(((sj0)*(x1006)*(x1010)))+(((IkReal(-1.00000000000000))*(x1013)*(x1019)))+(((cj1)*(x1017)))+(((IkReal(1656.24997606719))*(py)*(x1012)))+(((IkReal(-1.00000000000000))*(px)*(x1008)*(x1011)))+(((IkReal(-1.00000000000000))*(x1010)*(x1020)))+(((cj1)*(px)*(x1007)))))), ((gconst3)*(((IkReal(175.562500000000))+(((x1014)*(x1019)))+(((x1011)*(x1020)))+(((py)*(x1010)*(x1012)))+(((px)*(x1012)*(x1013)))+(((sj0)*(sj1)*(x1009)))+(((sj1)*(x1017)))+(((sj1)*(x1018)))+(((cj0)*(x1006)*(x1014)))+(((py)*(x1008)*(x1013)))+(((IkReal(-1.00000000000000))*(sj0)*(x1006)*(x1011)))+(((IkReal(-1.00000000000000))*(cj1)*(x1016)))+(((IkReal(-1.00000000000000))*(cj1)*(x1015)))+(((x1006)*(x1007)))+(((IkReal(-1656.24997606719))*(sj0)*(x1006)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x1008)*(x1010))))))); -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[4]; -IkReal x1021=IKsin(j2); -IkReal x1022=IKcos(j2); -IkReal x1023=((r00)*(sj0)); -IkReal x1024=((r01)*(sj0)); -IkReal x1025=((py)*(sj1)); -IkReal x1026=((IkReal(0.999999985550000))*(r02)); -IkReal x1027=((px)*(sj1)); -IkReal x1028=((IkReal(0.999999985550000))*(sj0)); -IkReal x1029=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1030=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1031=((cj1)*(px)); -IkReal x1032=((pz)*(sj1)); -IkReal x1033=((cj0)*(r00)); -IkReal x1034=((cj1)*(pz)); -IkReal x1035=((IkReal(1.00000000000000))*(r01)); -IkReal x1036=((cj1)*(py)); -IkReal x1037=((IkReal(0.999999985550000))*(cj0)); -IkReal x1038=((IkReal(0.106000000000000))*(x1021)); -IkReal x1039=((IkReal(0.106000000000000))*(x1022)); -IkReal x1040=((IkReal(0.112000000000000))*(x1022)); -IkReal x1041=((IkReal(0.112000000000000))*(x1021)); -IkReal x1042=((cj3)*(x1041)); -IkReal x1043=((sj3)*(x1040)); -IkReal x1044=((sj3)*(x1041)); -IkReal x1045=((cj3)*(x1040)); -IkReal x1046=((x1045)+(x1039)); -IkReal x1047=((x1043)+(x1042)+(x1038)); -evalcond[0]=((((x1036)*(x1037)))+(((x1030)*(x1036)))+(((IkReal(-1.00000000000000))*(x1028)*(x1031)))+(x1047)+(x1032)+(((x1029)*(x1031)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x1025)*(x1030)))+(((IkReal(-1.00000000000000))*(x1025)*(x1037)))+(x1044)+(x1034)+(((x1027)*(x1028)))+(((IkReal(-1.00000000000000))*(x1027)*(x1029)))+(((IkReal(-1.00000000000000))*(x1046)))); -evalcond[2]=((((r00)*(x1036)))+(((IkReal(-1.00000000000000))*(r01)*(x1029)*(x1032)))+(((r02)*(x1025)*(x1029)))+(((IkReal(0.999999985550000))*(x1032)*(x1033)))+(((IkReal(0.000169999997543500))*(x1023)*(x1032)))+(((IkReal(-1.00000000000000))*(x1031)*(x1035)))+(((IkReal(0.999999985550000))*(x1024)*(x1032)))+(((IkReal(-1.00000000000000))*(cj0)*(x1026)*(x1027)))+(((IkReal(-1.00000000000000))*(r02)*(x1027)*(x1030)))+(((IkReal(-1.00000000000000))*(sj0)*(x1025)*(x1026)))+(((IkReal(-1.00000000000000))*(x1047)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x1027)*(x1035)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(r02)*(x1029)*(x1036)))+(x1044)+(((sj0)*(x1026)*(x1036)))+(((IkReal(-0.999999985550000))*(x1033)*(x1034)))+(((IkReal(0.105999998468300))*(x1033)))+(((r01)*(x1029)*(x1034)))+(((IkReal(-0.999999985550000))*(x1024)*(x1034)))+(((IkReal(0.105999998468300))*(x1024)))+(((r02)*(x1030)*(x1031)))+(((IkReal(-0.000169999997543500))*(x1023)*(x1034)))+(((IkReal(1.80199997396110e-5))*(x1023)))+(((cj0)*(x1026)*(x1031)))+(((IkReal(-1.00000000000000))*(x1046)))+(((r00)*(x1025)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -IkReal x1048=(px)*(px); -IkReal x1049=((IkReal(0.0237440000000000))*(cj3)); -IkReal x1050=(py)*(py); -IkReal x1051=(pz)*(pz); -IkReal x1052=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1053=((cj0)*(r00)); -IkReal x1054=((r01)*(sj0)); -IkReal x1055=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1056=((IkReal(0.000339999995087000))*(py)); -IkReal x1057=((pz)*(sj1)); -IkReal x1058=((r02)*(sj0)); -IkReal x1059=((cj0)*(px)); -IkReal x1060=((cj1)*(r01)); -IkReal x1061=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1062=((IkReal(3.60399994792220e-5))*(sj1)); -IkReal x1063=((py)*(r01)); -IkReal x1064=((r00)*(sj0)); -IkReal x1065=((IkReal(0.000169999997543500))*(pz)); -IkReal x1066=((IkReal(0.105999998468300))*(cj0)); -IkReal x1067=((px)*(r00)); -IkReal x1068=((IkReal(1.99999997110000))*(pz)); -IkReal x1069=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1070=((r01)*(sj1)); -IkReal x1071=((cj1)*(r02)); -IkReal x1072=((r02)*(sj1)); -IkReal x1073=((IkReal(0.999999985550000))*(py)); -IkReal x1074=((IkReal(0.999999985550000))*(cj0)); -IkReal x1075=((IkReal(0.211999996936600))*(px)); -IkReal x1076=((IkReal(0.0112359998376398))*(cj0)); -IkReal x1077=((cj0)*(r02)); -IkReal x1078=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x1079=((IkReal(0.211999996936600))*(py)); -IkReal x1080=((cj1)*(px)); -IkReal x1081=((IkReal(0.000339999995087000))*(pz)); -IkReal x1082=((IkReal(0.999999985550000))*(cj1)); -IkReal x1083=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1084=((cj0)*(py)); -IkReal x1085=((pz)*(r02)); -IkReal x1086=((IkReal(0.211999996936600))*(pz)); -IkReal x1087=((cj1)*(pz)); -IkReal x1088=((IkReal(0.999999985550000))*(sj1)); -IkReal x1089=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x1090=((cj0)*(r01)); -IkReal x1091=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1092=((IkReal(0.999999985550000))*(x1051)); -IkReal x1093=((IkReal(0.000169999997543500))*(x1050)); -IkReal x1094=((IkReal(0.999999985550000))*(x1048)); -IkReal x1095=((IkReal(0.000169999997543500))*(x1051)); -IkReal x1096=((IkReal(1.99999997110000))*(px)*(py)); -IkReal x1097=((IkReal(0.000169999997543500))*(x1048)); -IkReal x1098=((IkReal(2.00000000000000))*(x1051)); -IkReal x1099=((IkReal(0.999999985550000))*(x1050)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((py)*(x1061)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((IkReal(-1.00000000000000))*(sj0)*(x1073)))+(((IkReal(-0.999999985550000))*(x1059)))); -evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-0.000169999997543500))*(x1064)))+(((r01)*(x1061)))+(((IkReal(-0.999999985550000))*(x1054)))+(((IkReal(-0.999999985550000))*(x1053)))); -evalcond[3]=((IkReal(0.0125440000000000))+(x1049)+(((IkReal(-1.00000000000000))*(x1059)*(x1062)))+(((IkReal(0.212000000000000))*(x1087)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x1079)))+(((sj0)*(sj1)*(x1075)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x1062)))); -evalcond[4]=((((IkReal(0.106000000000000))*(x1071)))+(((IkReal(0.105999998468300))*(sj1)*(x1064)))+(((IkReal(-1.00000000000000))*(x1085)))+(((IkReal(-1.00000000000000))*(x1054)*(x1078)))+(((IkReal(-1.00000000000000))*(x1066)*(x1070)))+(((IkReal(-1.00000000000000))*(x1067)))+(((IkReal(-1.00000000000000))*(x1063)))+(((IkReal(-1.00000000000000))*(x1053)*(x1078)))); -evalcond[5]=((((x1053)*(x1083)))+(x1072)+(((IkReal(-1.00000000000000))*(x1064)*(x1082)))+(((x1060)*(x1074)))+(((x1054)*(x1083)))); -evalcond[6]=((x1071)+(((IkReal(-1.00000000000000))*(x1070)*(x1074)))+(((IkReal(-1.00000000000000))*(x1054)*(x1055)))+(((IkReal(-1.00000000000000))*(x1053)*(x1055)))+(((x1064)*(x1088)))); -evalcond[7]=((((IkReal(0.106000000000000))*(x1072)))+(((x1053)*(x1089)))+(((IkReal(-0.105999998468300))*(cj1)*(x1064)))+(((x1060)*(x1066)))+(((IkReal(0.999999985550000))*(pz)*(x1064)))+(((IkReal(0.000169999997543500))*(r02)*(x1059)))+(((IkReal(0.000169999997543500))*(py)*(x1058)))+(((IkReal(-0.999999985550000))*(px)*(x1058)))+(((x1073)*(x1077)))+(((IkReal(-1.00000000000000))*(x1054)*(x1065)))+(((x1054)*(x1089)))+(((IkReal(-1.00000000000000))*(pz)*(r01)*(x1074)))+(((IkReal(-1.00000000000000))*(x1053)*(x1065)))); -evalcond[8]=((IkReal(0.0237800000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x1052)*(x1060)))+(((r01)*(x1051)*(x1061)))+(x1049)+(((IkReal(-1.00000000000000))*(cj1)*(x1058)*(x1079)))+(((IkReal(3.60399994792220e-5))*(x1071)*(x1084)))+(((cj1)*(x1054)*(x1086)))+(((IkReal(0.212000000000000))*(px)*(x1070)))+(((IkReal(-1.00000000000000))*(px)*(x1053)*(x1056)))+(((IkReal(-3.60399994792220e-5))*(x1058)*(x1080)))+(((x1064)*(x1097)))+(((x1064)*(x1096)))+(((IkReal(-1.00000000000000))*(x1054)*(x1094)))+(((IkReal(-1.00000000000000))*(x1054)*(x1092)))+(((IkReal(-1.00000000000000))*(x1064)*(x1095)))+(((IkReal(-1.00000000000000))*(x1064)*(x1093)))+(((cj1)*(x1052)*(x1064)))+(((r01)*(x1048)*(x1061)))+(((IkReal(-1.00000000000000))*(r01)*(x1050)*(x1061)))+(((x1054)*(x1099)))+(((IkReal(1.99999997110000))*(x1059)*(x1063)))+(((px)*(x1054)*(x1056)))+(((IkReal(-1.00000000000000))*(x1053)*(x1092)))+(((IkReal(-1.00000000000000))*(x1053)*(x1099)))+(((IkReal(-0.212000000000000))*(py)*(r00)*(sj1)))+(((py)*(x1058)*(x1068)))+(((px)*(x1058)*(x1081)))+(((cj1)*(x1053)*(x1086)))+(((IkReal(-0.0112359998376398))*(x1054)))+(((IkReal(-0.0112359998376398))*(x1053)))+(((IkReal(-0.211999996936600))*(x1059)*(x1071)))+(((IkReal(-1.91011997239877e-6))*(x1064)))+(((IkReal(1.91011997239877e-6))*(x1090)))+(((IkReal(-1.00000000000000))*(pz)*(x1056)*(x1077)))+(((x1053)*(x1094)))+(((r02)*(x1059)*(x1068)))); -evalcond[9]=((((IkReal(-1.00000000000000))*(x1071)*(x1098)))+(((IkReal(-1.00000000000000))*(x1051)*(x1054)*(x1055)))+(((IkReal(-1.00000000000000))*(x1048)*(x1064)*(x1088)))+(((x1070)*(x1076)))+(((x1050)*(x1070)*(x1074)))+(((IkReal(0.212000000000000))*(x1085)))+(((IkReal(-2.00000000000000))*(py)*(pz)*(x1060)))+(((IkReal(-1.00000000000000))*(x1048)*(x1070)*(x1074)))+(((IkReal(-1.00000000000000))*(x1050)*(x1053)*(x1055)))+(((IkReal(-1.99999997110000))*(px)*(x1057)*(x1058)))+(((IkReal(-1.00000000000000))*(x1051)*(x1070)*(x1074)))+(((x1056)*(x1057)*(x1058)))+(((IkReal(0.212000000000000))*(x1063)))+(((IkReal(0.212000000000000))*(x1067)))+(((IkReal(-1.00000000000000))*(x1051)*(x1053)*(x1055)))+(((IkReal(-1.00000000000000))*(x1048)*(x1054)*(x1055)))+(((px)*(sj1)*(x1056)*(x1064)))+(((x1056)*(x1059)*(x1070)))+(((x1048)*(x1053)*(x1055)))+(((x1050)*(x1064)*(x1088)))+(((IkReal(-0.0112359998376398))*(sj1)*(x1064)))+(((x1054)*(x1091)))+(((IkReal(0.000339999995087000))*(r02)*(x1057)*(x1059)))+(((IkReal(-1.00000000000000))*(sj1)*(x1054)*(x1096)))+(((x1050)*(x1054)*(x1055)))+(((IkReal(-2.00000000000000))*(x1067)*(x1087)))+(((pp)*(x1071)))+(((x1051)*(x1064)*(x1088)))+(((IkReal(1.99999997110000))*(py)*(x1057)*(x1077)))+(((IkReal(-0.0112360000000000))*(x1071)))+(((x1053)*(x1091)))+(((sj1)*(x1053)*(x1096)))); -evalcond[10]=((((IkReal(-1.00000000000000))*(x1086)*(x1090)))+(((x1077)*(x1079)))+(((x1048)*(x1064)*(x1082)))+(((IkReal(-1.00000000000000))*(x1052)*(x1053)))+(((IkReal(-1.00000000000000))*(x1052)*(x1054)))+(((IkReal(-1.00000000000000))*(x1056)*(x1059)*(x1060)))+(((IkReal(-1.00000000000000))*(x1050)*(x1054)*(x1083)))+(((x1054)*(x1069)))+(((IkReal(-1.00000000000000))*(x1050)*(x1060)*(x1074)))+(((x1060)*(x1076)))+(((IkReal(0.0112360000000000))*(x1072)))+(((IkReal(-1.00000000000000))*(x1056)*(x1058)*(x1087)))+(((IkReal(-1.00000000000000))*(x1051)*(x1064)*(x1082)))+(((x1058)*(x1068)*(x1080)))+(((IkReal(-1.00000000000000))*(x1059)*(x1071)*(x1081)))+(((IkReal(-1.00000000000000))*(x1058)*(x1075)))+(((IkReal(1.99999997110000))*(py)*(x1054)*(x1080)))+(((x1048)*(x1060)*(x1074)))+(((x1053)*(x1069)))+(((IkReal(3.60399994792220e-5))*(py)*(x1058)))+(((IkReal(-1.00000000000000))*(x1050)*(x1064)*(x1082)))+(((x1051)*(x1060)*(x1074)))+(((IkReal(-1.99999997110000))*(py)*(x1053)*(x1080)))+(((IkReal(-0.0112359998376398))*(cj1)*(x1064)))+(((IkReal(-1.00000000000000))*(x1072)*(x1098)))+(((IkReal(-1.00000000000000))*(x1048)*(x1053)*(x1083)))+(((IkReal(-1.00000000000000))*(x1068)*(x1071)*(x1084)))+(((x1051)*(x1053)*(x1083)))+(((x1048)*(x1054)*(x1083)))+(((IkReal(-1.00000000000000))*(x1056)*(x1064)*(x1080)))+(((pp)*(x1072)))+(((x1051)*(x1054)*(x1083)))+(((IkReal(-2.00000000000000))*(x1057)*(x1067)))+(((IkReal(-2.00000000000000))*(x1057)*(x1063)))+(((x1050)*(x1053)*(x1083)))+(((x1064)*(x1086)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1059)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst6; -gconst6=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1100=((cj0)*(sj1)); -IkReal x1101=((IkReal(1656.24997606719))*(pz)); -IkReal x1102=((py)*(r02)); -IkReal x1103=((IkReal(1750.00000000000))*(cj1)); -IkReal x1104=((px)*(sj3)); -IkReal x1105=((cj3)*(pz)); -IkReal x1106=((sj0)*(sj1)); -IkReal x1107=((IkReal(1749.99997471250))*(r02)); -IkReal x1108=((px)*(r01)); -IkReal x1109=((py)*(sj3)); -IkReal x1110=((pz)*(sj3)); -IkReal x1111=((IkReal(0.281562495931422))*(px)); -IkReal x1112=((IkReal(0.297499995701125))*(cj3)); -IkReal x1113=((IkReal(1749.99997471250))*(r01)); -IkReal x1114=((IkReal(1749.99997471250))*(r00)); -IkReal x1115=((IkReal(1656.25000000000))*(cj1)); -IkReal x1116=((IkReal(0.281562495931422))*(pz)); -IkReal x1117=((IkReal(1749.99997471250))*(cj3)); -IkReal x1118=((IkReal(1656.24997606719))*(px)); -IkReal x1119=((IkReal(0.297499995701125))*(r01)); -IkReal x1120=((py)*(r00)); -IkReal x1121=((IkReal(0.297499995701125))*(r00)); -if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(r02)*(x1100)*(x1118)))+(((IkReal(1749.99997471250))*(x1104)*(x1106)))+(((IkReal(-1.00000000000000))*(cj3)*(px)*(x1100)*(x1107)))+(((IkReal(-1.00000000000000))*(x1100)*(x1105)*(x1119)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x1106)*(x1112)))+(((r01)*(x1101)*(x1106)))+(((IkReal(-1.00000000000000))*(r02)*(x1106)*(x1111)))+(((r00)*(x1100)*(x1101)))+(((IkReal(-1.00000000000000))*(cj3)*(x1103)*(x1108)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-0.297499995701125))*(x1106)*(x1109)))+(((IkReal(-1749.99997471250))*(x1100)*(x1109)))+(((IkReal(-1.00000000000000))*(x1102)*(x1106)*(x1117)))+(((IkReal(-1656.24997606719))*(x1102)*(x1106)))+(((x1105)*(x1106)*(x1113)))+(((IkReal(-1.00000000000000))*(x1108)*(x1115)))+(((IkReal(-1.00000000000000))*(r01)*(x1100)*(x1116)))+(((IkReal(-0.297499995701125))*(x1100)*(x1104)))+(((cj3)*(x1103)*(x1120)))+(((x1105)*(x1106)*(x1121)))+(((x1103)*(x1110)))+(((r00)*(x1106)*(x1116)))+(((x1100)*(x1102)*(x1112)))+(((IkReal(0.281562495931422))*(x1100)*(x1102)))+(((x1100)*(x1105)*(x1114)))+(((x1115)*(x1120))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(x1103)*(x1105)))+(((IkReal(-1.00000000000000))*(x1100)*(x1104)*(x1107)))+(((IkReal(-1.00000000000000))*(x1100)*(x1110)*(x1119)))+(((px)*(x1100)*(x1112)))+(((IkReal(-1.00000000000000))*(r01)*(x1103)*(x1104)))+(((x1100)*(x1110)*(x1114)))+(((x1106)*(x1110)*(x1113)))+(((x1106)*(x1110)*(x1121)))+(((IkReal(-0.297499995701125))*(r02)*(x1104)*(x1106)))+(((IkReal(1656.24997606719))*(py)*(x1100)))+(((IkReal(-1.00000000000000))*(pz)*(x1115)))+(((py)*(x1106)*(x1112)))+(((IkReal(-1749.99997471250))*(sj3)*(x1102)*(x1106)))+(((IkReal(-1.00000000000000))*(x1106)*(x1118)))+(((r00)*(x1103)*(x1109)))+(((py)*(x1100)*(x1117)))+(((x1100)*(x1111)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x1106)*(x1117)))+(((IkReal(0.297499995701125))*(sj3)*(x1100)*(x1102)))+(((IkReal(0.281562495931422))*(py)*(x1106))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(r02)*(x1100)*(x1118)))+(((IkReal(1749.99997471250))*(x1104)*(x1106)))+(((IkReal(-1.00000000000000))*(cj3)*(px)*(x1100)*(x1107)))+(((IkReal(-1.00000000000000))*(x1100)*(x1105)*(x1119)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x1106)*(x1112)))+(((r01)*(x1101)*(x1106)))+(((IkReal(-1.00000000000000))*(r02)*(x1106)*(x1111)))+(((r00)*(x1100)*(x1101)))+(((IkReal(-1.00000000000000))*(cj3)*(x1103)*(x1108)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-0.297499995701125))*(x1106)*(x1109)))+(((IkReal(-1749.99997471250))*(x1100)*(x1109)))+(((IkReal(-1.00000000000000))*(x1102)*(x1106)*(x1117)))+(((IkReal(-1656.24997606719))*(x1102)*(x1106)))+(((x1105)*(x1106)*(x1113)))+(((IkReal(-1.00000000000000))*(x1108)*(x1115)))+(((IkReal(-1.00000000000000))*(r01)*(x1100)*(x1116)))+(((IkReal(-0.297499995701125))*(x1100)*(x1104)))+(((cj3)*(x1103)*(x1120)))+(((x1105)*(x1106)*(x1121)))+(((x1103)*(x1110)))+(((r00)*(x1106)*(x1116)))+(((x1100)*(x1102)*(x1112)))+(((IkReal(0.281562495931422))*(x1100)*(x1102)))+(((x1100)*(x1105)*(x1114)))+(((x1115)*(x1120)))))), ((gconst6)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(x1103)*(x1105)))+(((IkReal(-1.00000000000000))*(x1100)*(x1104)*(x1107)))+(((IkReal(-1.00000000000000))*(x1100)*(x1110)*(x1119)))+(((px)*(x1100)*(x1112)))+(((IkReal(-1.00000000000000))*(r01)*(x1103)*(x1104)))+(((x1100)*(x1110)*(x1114)))+(((x1106)*(x1110)*(x1113)))+(((x1106)*(x1110)*(x1121)))+(((IkReal(-0.297499995701125))*(r02)*(x1104)*(x1106)))+(((IkReal(1656.24997606719))*(py)*(x1100)))+(((IkReal(-1.00000000000000))*(pz)*(x1115)))+(((py)*(x1106)*(x1112)))+(((IkReal(-1749.99997471250))*(sj3)*(x1102)*(x1106)))+(((IkReal(-1.00000000000000))*(x1106)*(x1118)))+(((r00)*(x1103)*(x1109)))+(((py)*(x1100)*(x1117)))+(((x1100)*(x1111)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x1106)*(x1117)))+(((IkReal(0.297499995701125))*(sj3)*(x1100)*(x1102)))+(((IkReal(0.281562495931422))*(py)*(x1106))))))); -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[4]; -IkReal x1122=IKsin(j2); -IkReal x1123=IKcos(j2); -IkReal x1124=((r00)*(sj0)); -IkReal x1125=((r01)*(sj0)); -IkReal x1126=((py)*(sj1)); -IkReal x1127=((IkReal(0.999999985550000))*(r02)); -IkReal x1128=((px)*(sj1)); -IkReal x1129=((IkReal(0.999999985550000))*(sj0)); -IkReal x1130=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1131=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1132=((cj1)*(px)); -IkReal x1133=((pz)*(sj1)); -IkReal x1134=((cj0)*(r00)); -IkReal x1135=((cj1)*(pz)); -IkReal x1136=((IkReal(1.00000000000000))*(r01)); -IkReal x1137=((cj1)*(py)); -IkReal x1138=((IkReal(0.999999985550000))*(cj0)); -IkReal x1139=((IkReal(0.106000000000000))*(x1122)); -IkReal x1140=((IkReal(0.106000000000000))*(x1123)); -IkReal x1141=((IkReal(0.112000000000000))*(x1123)); -IkReal x1142=((IkReal(0.112000000000000))*(x1122)); -IkReal x1143=((cj3)*(x1142)); -IkReal x1144=((sj3)*(x1141)); -IkReal x1145=((cj3)*(x1141)); -IkReal x1146=((sj3)*(x1142)); -IkReal x1147=((x1140)+(x1145)); -IkReal x1148=((x1143)+(x1144)+(x1139)); -evalcond[0]=((x1148)+(x1133)+(((x1130)*(x1132)))+(((x1131)*(x1137)))+(((x1137)*(x1138)))+(((IkReal(-1.00000000000000))*(x1129)*(x1132)))); -evalcond[1]=((IkReal(-0.106000000000000))+(x1146)+(x1135)+(((IkReal(-1.00000000000000))*(x1147)))+(((IkReal(-1.00000000000000))*(x1126)*(x1131)))+(((IkReal(-1.00000000000000))*(x1126)*(x1138)))+(((x1128)*(x1129)))+(((IkReal(-1.00000000000000))*(x1128)*(x1130)))); -evalcond[2]=((((r02)*(x1126)*(x1130)))+(x1148)+(((r00)*(x1137)))+(((IkReal(-1.00000000000000))*(r01)*(x1130)*(x1133)))+(((IkReal(0.000169999997543500))*(x1124)*(x1133)))+(((IkReal(-1.00000000000000))*(r02)*(x1128)*(x1131)))+(((IkReal(0.999999985550000))*(x1125)*(x1133)))+(((IkReal(-1.00000000000000))*(sj0)*(x1126)*(x1127)))+(((IkReal(-1.00000000000000))*(x1132)*(x1136)))+(((IkReal(0.999999985550000))*(x1133)*(x1134)))+(((IkReal(-1.00000000000000))*(cj0)*(x1127)*(x1128)))); -evalcond[3]=((x1147)+(((r00)*(x1126)))+(((IkReal(-0.999999985550000))*(x1134)*(x1135)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(1.80199997396110e-5))*(x1124)))+(((r02)*(x1131)*(x1132)))+(((cj0)*(x1127)*(x1132)))+(((IkReal(0.105999998468300))*(x1125)))+(((IkReal(0.105999998468300))*(x1134)))+(((r01)*(x1130)*(x1135)))+(((IkReal(-1.00000000000000))*(x1146)))+(((IkReal(-1.00000000000000))*(r02)*(x1130)*(x1137)))+(((IkReal(-0.000169999997543500))*(x1124)*(x1135)))+(((IkReal(-1.00000000000000))*(x1128)*(x1136)))+(((IkReal(-0.999999985550000))*(x1125)*(x1135)))+(((sj0)*(x1127)*(x1137)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1149=((cj0)*(sj1)); -IkReal x1150=((IkReal(0.281562495931422))*(px)); -IkReal x1151=((cj1)*(sj0)); -IkReal x1152=((IkReal(0.281562495931422))*(py)); -IkReal x1153=((IkReal(1656.24997606719))*(px)); -IkReal x1154=((IkReal(1656.24997606719))*(py)); -IkReal x1155=((cj0)*(cj1)); -IkReal x1156=((IkReal(1749.99997471250))*(px)); -IkReal x1157=((IkReal(0.297499995701125))*(px)); -IkReal x1158=((IkReal(1749.99997471250))*(py)); -IkReal x1159=((IkReal(0.297499995701125))*(py)); -IkReal x1160=((sj0)*(sj1)); -IkReal x1161=((IkReal(1656.25000000000))*(pz)); -IkReal x1162=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x1163=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x1164=((sj3)*(x1160)); -IkReal x1165=((cj3)*(x1160)); -if( IKabs(((gconst5)*(((((cj1)*(x1163)))+(((sj1)*(x1162)))+(((sj1)*(x1161)))+(((IkReal(-1.00000000000000))*(x1159)*(x1164)))+(((IkReal(-185.500000000000))*(sj3)))+(((x1156)*(x1164)))+(((x1150)*(x1155)))+(((x1151)*(x1152)))+(((cj3)*(x1151)*(x1159)))+(((IkReal(-1.00000000000000))*(sj3)*(x1149)*(x1157)))+(((IkReal(-1.00000000000000))*(sj3)*(x1149)*(x1158)))+(((cj3)*(x1155)*(x1157)))+(((cj3)*(x1155)*(x1158)))+(((IkReal(-1.00000000000000))*(x1151)*(x1153)))+(((x1154)*(x1155)))+(((IkReal(-1.00000000000000))*(cj3)*(x1151)*(x1156))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(x1156)*(x1165)))+(((x1159)*(x1165)))+(((x1149)*(x1154)))+(((x1149)*(x1150)))+(((sj1)*(x1163)))+(((cj3)*(x1149)*(x1158)))+(((cj3)*(x1149)*(x1157)))+(((sj3)*(x1155)*(x1157)))+(((sj3)*(x1155)*(x1158)))+(((sj3)*(x1151)*(x1159)))+(((IkReal(-1.00000000000000))*(x1153)*(x1160)))+(((IkReal(-1.00000000000000))*(cj1)*(x1161)))+(((IkReal(-1.00000000000000))*(cj1)*(x1162)))+(((x1152)*(x1160)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x1151)*(x1156))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst5)*(((((cj1)*(x1163)))+(((sj1)*(x1162)))+(((sj1)*(x1161)))+(((IkReal(-1.00000000000000))*(x1159)*(x1164)))+(((IkReal(-185.500000000000))*(sj3)))+(((x1156)*(x1164)))+(((x1150)*(x1155)))+(((x1151)*(x1152)))+(((cj3)*(x1151)*(x1159)))+(((IkReal(-1.00000000000000))*(sj3)*(x1149)*(x1157)))+(((IkReal(-1.00000000000000))*(sj3)*(x1149)*(x1158)))+(((cj3)*(x1155)*(x1157)))+(((cj3)*(x1155)*(x1158)))+(((IkReal(-1.00000000000000))*(x1151)*(x1153)))+(((x1154)*(x1155)))+(((IkReal(-1.00000000000000))*(cj3)*(x1151)*(x1156)))))), ((gconst5)*(((IkReal(175.562500000000))+(((IkReal(-1.00000000000000))*(x1156)*(x1165)))+(((x1159)*(x1165)))+(((x1149)*(x1154)))+(((x1149)*(x1150)))+(((sj1)*(x1163)))+(((cj3)*(x1149)*(x1158)))+(((cj3)*(x1149)*(x1157)))+(((sj3)*(x1155)*(x1157)))+(((sj3)*(x1155)*(x1158)))+(((sj3)*(x1151)*(x1159)))+(((IkReal(-1.00000000000000))*(x1153)*(x1160)))+(((IkReal(-1.00000000000000))*(cj1)*(x1161)))+(((IkReal(-1.00000000000000))*(cj1)*(x1162)))+(((x1152)*(x1160)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x1151)*(x1156))))))); -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[4]; -IkReal x1166=IKsin(j2); -IkReal x1167=IKcos(j2); -IkReal x1168=((r00)*(sj0)); -IkReal x1169=((r01)*(sj0)); -IkReal x1170=((py)*(sj1)); -IkReal x1171=((IkReal(0.999999985550000))*(r02)); -IkReal x1172=((px)*(sj1)); -IkReal x1173=((IkReal(0.999999985550000))*(sj0)); -IkReal x1174=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1175=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1176=((cj1)*(px)); -IkReal x1177=((pz)*(sj1)); -IkReal x1178=((cj0)*(r00)); -IkReal x1179=((cj1)*(pz)); -IkReal x1180=((IkReal(1.00000000000000))*(r01)); -IkReal x1181=((cj1)*(py)); -IkReal x1182=((IkReal(0.999999985550000))*(cj0)); -IkReal x1183=((IkReal(0.106000000000000))*(x1166)); -IkReal x1184=((IkReal(0.106000000000000))*(x1167)); -IkReal x1185=((IkReal(0.112000000000000))*(x1167)); -IkReal x1186=((IkReal(0.112000000000000))*(x1166)); -IkReal x1187=((cj3)*(x1186)); -IkReal x1188=((sj3)*(x1185)); -IkReal x1189=((cj3)*(x1185)); -IkReal x1190=((sj3)*(x1186)); -IkReal x1191=((x1189)+(x1184)); -IkReal x1192=((x1188)+(x1187)+(x1183)); -evalcond[0]=((x1192)+(x1177)+(((IkReal(-1.00000000000000))*(x1173)*(x1176)))+(((x1174)*(x1176)))+(((x1175)*(x1181)))+(((x1181)*(x1182)))); -evalcond[1]=((IkReal(-0.106000000000000))+(x1190)+(x1179)+(((IkReal(-1.00000000000000))*(x1172)*(x1174)))+(((IkReal(-1.00000000000000))*(x1191)))+(((x1172)*(x1173)))+(((IkReal(-1.00000000000000))*(x1170)*(x1175)))+(((IkReal(-1.00000000000000))*(x1170)*(x1182)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x1176)*(x1180)))+(x1192)+(((IkReal(-1.00000000000000))*(r02)*(x1172)*(x1175)))+(((r00)*(x1181)))+(((IkReal(-1.00000000000000))*(r01)*(x1174)*(x1177)))+(((IkReal(0.000169999997543500))*(x1168)*(x1177)))+(((r02)*(x1170)*(x1174)))+(((IkReal(-1.00000000000000))*(sj0)*(x1170)*(x1171)))+(((IkReal(0.999999985550000))*(x1177)*(x1178)))+(((IkReal(-1.00000000000000))*(cj0)*(x1171)*(x1172)))+(((IkReal(0.999999985550000))*(x1169)*(x1177)))); -evalcond[3]=((((IkReal(-0.999999985550000))*(x1169)*(x1179)))+(x1191)+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(r02)*(x1174)*(x1181)))+(((r00)*(x1170)))+(((IkReal(1.80199997396110e-5))*(x1168)))+(((IkReal(-1.00000000000000))*(x1190)))+(((sj0)*(x1171)*(x1181)))+(((IkReal(0.105999998468300))*(x1169)))+(((IkReal(-1.00000000000000))*(x1172)*(x1180)))+(((IkReal(0.105999998468300))*(x1178)))+(((cj0)*(x1171)*(x1176)))+(((IkReal(-0.999999985550000))*(x1178)*(x1179)))+(((IkReal(-0.000169999997543500))*(x1168)*(x1179)))+(((r01)*(x1174)*(x1179)))+(((r02)*(x1175)*(x1176)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1193=((cj0)*(py)); -IkReal x1194=((r02)*(sj1)); -IkReal x1195=((px)*(sj0)); -IkReal x1196=((IkReal(13.9999997977000))*(r00)); -IkReal x1197=((IkReal(1.69999943143527e-5))*(cj5)); -IkReal x1198=((cj0)*(cj1)); -IkReal x1199=((IkReal(0.00237999996560900))*(r01)); -IkReal x1200=((cj3)*(px)); -IkReal x1201=((IkReal(13.9999997977000))*(r01)); -IkReal x1202=((cj1)*(sj0)); -IkReal x1203=((px)*(sj3)); -IkReal x1204=((IkReal(0.00237999996560900))*(r00)); -IkReal x1205=((IkReal(0.0212499928929409))*(cj5)); -IkReal x1206=((cj3)*(py)); -IkReal x1207=((sj3)*(x1202)); -IkReal x1208=((IkReal(0.0999999665550159))*(cj1)*(cj5)*(sj3)); -IkReal x1209=((cj5)*(pz)*(sj1)*(sj3)); -IkReal x1210=((IkReal(0.0999999665550159))*(cj1)*(cj3)*(cj5)); -IkReal x1211=((cj3)*(cj5)*(pz)*(sj1)); -IkReal x1212=((IkReal(124.999958193770))*(cj1)*(cj5)*(sj3)); -IkReal x1213=((IkReal(124.999958193770))*(cj1)*(cj3)*(cj5)); -if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x1197)*(x1198)*(x1203)))+(((IkReal(14.0000000000000))*(sj3)*(x1194)))+(((sj3)*(x1198)*(x1201)))+(((sj3)*(x1198)*(x1204)))+(((IkReal(-0.0999999680000154))*(x1209)))+(((IkReal(-1.00000000000000))*(x1196)*(x1207)))+(((x1202)*(x1205)*(x1206)))+(((x1193)*(x1213)))+(((x1199)*(x1207)))+(((IkReal(-1.00000000000000))*(py)*(x1197)*(x1207)))+(((x1195)*(x1208)))+(((x1198)*(x1200)*(x1205)))+(((IkReal(-1.00000000000000))*(x1195)*(x1213)))+(((IkReal(124.999960000019))*(x1211)))+(((IkReal(-1.00000000000000))*(x1193)*(x1208))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-14.0000000000000))*(cj3)*(x1194)))+(((IkReal(124.999960000019))*(x1209)))+(((x1193)*(x1210)))+(((x1193)*(x1212)))+(((IkReal(-1.00000000000000))*(cj3)*(x1199)*(x1202)))+(((py)*(x1205)*(x1207)))+(((IkReal(0.0999999680000154))*(x1211)))+(((x1197)*(x1198)*(x1200)))+(((x1198)*(x1203)*(x1205)))+(((IkReal(13.2499998085375))*(r00)*(x1202)))+(((IkReal(-13.2499998085375))*(r01)*(x1198)))+(((x1197)*(x1202)*(x1206)))+(((IkReal(-0.00225249996745138))*(r00)*(x1198)))+(((IkReal(-0.00225249996745138))*(r01)*(x1202)))+(((IkReal(-1.00000000000000))*(x1195)*(x1212)))+(((IkReal(-1.00000000000000))*(x1195)*(x1210)))+(((cj3)*(x1196)*(x1202)))+(((IkReal(-13.2500000000000))*(x1194)))+(((IkReal(-1.00000000000000))*(cj3)*(x1198)*(x1201)))+(((IkReal(-1.00000000000000))*(cj3)*(x1198)*(x1204))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(x1197)*(x1198)*(x1203)))+(((IkReal(14.0000000000000))*(sj3)*(x1194)))+(((sj3)*(x1198)*(x1201)))+(((sj3)*(x1198)*(x1204)))+(((IkReal(-0.0999999680000154))*(x1209)))+(((IkReal(-1.00000000000000))*(x1196)*(x1207)))+(((x1202)*(x1205)*(x1206)))+(((x1193)*(x1213)))+(((x1199)*(x1207)))+(((IkReal(-1.00000000000000))*(py)*(x1197)*(x1207)))+(((x1195)*(x1208)))+(((x1198)*(x1200)*(x1205)))+(((IkReal(-1.00000000000000))*(x1195)*(x1213)))+(((IkReal(124.999960000019))*(x1211)))+(((IkReal(-1.00000000000000))*(x1193)*(x1208)))))), ((gconst2)*(((((IkReal(-14.0000000000000))*(cj3)*(x1194)))+(((IkReal(124.999960000019))*(x1209)))+(((x1193)*(x1210)))+(((x1193)*(x1212)))+(((IkReal(-1.00000000000000))*(cj3)*(x1199)*(x1202)))+(((py)*(x1205)*(x1207)))+(((IkReal(0.0999999680000154))*(x1211)))+(((x1197)*(x1198)*(x1200)))+(((x1198)*(x1203)*(x1205)))+(((IkReal(13.2499998085375))*(r00)*(x1202)))+(((IkReal(-13.2499998085375))*(r01)*(x1198)))+(((x1197)*(x1202)*(x1206)))+(((IkReal(-0.00225249996745138))*(r00)*(x1198)))+(((IkReal(-0.00225249996745138))*(r01)*(x1202)))+(((IkReal(-1.00000000000000))*(x1195)*(x1212)))+(((IkReal(-1.00000000000000))*(x1195)*(x1210)))+(((cj3)*(x1196)*(x1202)))+(((IkReal(-13.2500000000000))*(x1194)))+(((IkReal(-1.00000000000000))*(cj3)*(x1198)*(x1201)))+(((IkReal(-1.00000000000000))*(cj3)*(x1198)*(x1204))))))); -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[8]; -IkReal x1214=IKsin(j2); -IkReal x1215=IKcos(j2); -IkReal x1216=(py)*(py); -IkReal x1217=(pz)*(pz); -IkReal x1218=(px)*(px); -IkReal x1219=((cj0)*(r00)); -IkReal x1220=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1221=((r01)*(sj0)); -IkReal x1222=((cj3)*(cj5)); -IkReal x1223=((sj0)*(sj1)); -IkReal x1224=((IkReal(0.999999985550000))*(px)); -IkReal x1225=((IkReal(0.000339999995087000))*(py)); -IkReal x1226=((pz)*(r02)); -IkReal x1227=((py)*(sj1)); -IkReal x1228=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1229=((IkReal(0.999999680000154))*(cj5)); -IkReal x1230=((px)*(r00)); -IkReal x1231=((IkReal(0.000799999744000123))*(cj5)); -IkReal x1232=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1233=((cj0)*(sj1)); -IkReal x1234=((IkReal(1.99999997110000))*(px)); -IkReal x1235=((IkReal(0.000169999997543500))*(pz)); -IkReal x1236=((IkReal(0.112000000000000))*(sj5)); -IkReal x1237=((cj1)*(r02)); -IkReal x1238=((IkReal(0.106000000000000))*(sj5)); -IkReal x1239=((IkReal(0.000339999995087000))*(px)); -IkReal x1240=((IkReal(0.112000000000000))*(cj3)); -IkReal x1241=((cj1)*(py)); -IkReal x1242=((py)*(r02)); -IkReal x1243=((cj0)*(px)); -IkReal x1244=((px)*(r01)); -IkReal x1245=((cj0)*(r01)); -IkReal x1246=((IkReal(0.999999985550000))*(r01)); -IkReal x1247=((IkReal(0.999999985550000))*(pz)); -IkReal x1248=((r02)*(sj1)); -IkReal x1249=((cj1)*(sj0)); -IkReal x1250=((IkReal(1.99999997110000))*(cj0)); -IkReal x1251=((IkReal(0.000169999997543500))*(r02)); -IkReal x1252=((IkReal(1.04639966515216e-6))*(cj5)); -IkReal x1253=((IkReal(0.999999985550000))*(cj0)); -IkReal x1254=((IkReal(0.999999985550000))*(cj1)); -IkReal x1255=((cj1)*(pz)); -IkReal x1256=((py)*(sj0)); -IkReal x1257=((r00)*(sj0)); -IkReal x1258=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1259=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1260=((IkReal(0.00130799958144020))*(cj5)); -IkReal x1261=((IkReal(0.211999996936600))*(pz)); -IkReal x1262=((pz)*(sj1)); -IkReal x1263=((sj3)*(x1215)); -IkReal x1264=((cj1)*(x1244)); -IkReal x1265=((r00)*(x1249)); -IkReal x1266=((IkReal(0.999999985550000))*(x1217)); -IkReal x1267=((IkReal(2.00000000000000))*(pz)*(r01)); -IkReal x1268=((sj3)*(x1214)); -IkReal x1269=((cj5)*(x1215)); -IkReal x1270=((IkReal(0.999999985550000))*(x1216)); -IkReal x1271=((IkReal(2.00000000000000))*(x1217)); -IkReal x1272=((IkReal(0.999999985550000))*(x1218)); -IkReal x1273=((px)*(r02)*(sj0)); -IkReal x1274=((cj5)*(x1214)); -evalcond[0]=((((x1232)*(x1256)))+(x1262)+(((x1241)*(x1253)))+(((IkReal(0.112000000000000))*(x1263)))+(((x1232)*(x1243)))+(((x1214)*(x1240)))+(((IkReal(0.106000000000000))*(x1214)))+(((IkReal(-1.00000000000000))*(x1224)*(x1249)))); -evalcond[1]=((IkReal(-0.106000000000000))+(x1255)+(((x1223)*(x1224)))+(((IkReal(-1.00000000000000))*(x1227)*(x1253)))+(((IkReal(-1.00000000000000))*(x1228)*(x1243)))+(((IkReal(-0.000169999997543500))*(py)*(x1223)))+(((IkReal(-0.106000000000000))*(x1215)))+(((IkReal(0.112000000000000))*(x1268)))+(((IkReal(-1.00000000000000))*(x1215)*(x1240)))); -evalcond[2]=((((x1231)*(x1263)))+(x1248)+(((x1219)*(x1232)))+(((x1245)*(x1254)))+(((IkReal(-0.999999680000154))*(x1215)*(x1222)))+(((IkReal(-0.999999985550000))*(x1265)))+(((IkReal(0.000799999744000123))*(x1214)*(x1222)))+(((x1229)*(x1268)))+(((x1221)*(x1232)))); -evalcond[3]=((((x1231)*(x1268)))+(((IkReal(-0.999999680000154))*(x1214)*(x1222)))+(((IkReal(-1.00000000000000))*(x1229)*(x1263)))+(x1237)+(((IkReal(0.999999985550000))*(r00)*(x1223)))+(((IkReal(-1.00000000000000))*(x1233)*(x1246)))+(((IkReal(-1.00000000000000))*(x1221)*(x1228)))+(((IkReal(-0.000799999744000123))*(x1215)*(x1222)))+(((IkReal(-1.00000000000000))*(x1219)*(x1228)))); -evalcond[4]=((((IkReal(-0.999999985550000))*(x1223)*(x1242)))+(((IkReal(-1.00000000000000))*(x1214)*(x1238)))+(((IkReal(-1.00000000000000))*(px)*(x1223)*(x1251)))+(((r00)*(x1223)*(x1235)))+(((sj1)*(x1219)*(x1247)))+(((IkReal(-1.00000000000000))*(r02)*(x1224)*(x1233)))+(((r00)*(x1241)))+(((IkReal(-1.00000000000000))*(cj3)*(x1214)*(x1236)))+(((IkReal(-1.00000000000000))*(pz)*(x1228)*(x1245)))+(((IkReal(-1.00000000000000))*(x1264)))+(((cj0)*(x1227)*(x1251)))+(((sj1)*(x1221)*(x1247)))+(((IkReal(-1.00000000000000))*(x1236)*(x1263)))); -evalcond[5]=((((IkReal(1.80199997396110e-5))*(x1257)))+(((IkReal(-1.80199997396110e-5))*(x1245)))+(((IkReal(-1.00000000000000))*(cj0)*(x1232)*(x1242)))+(((r00)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(x1221)*(x1247)))+(((IkReal(-1.00000000000000))*(cj1)*(x1219)*(x1247)))+(((IkReal(-1.00000000000000))*(pz)*(x1232)*(x1257)))+(((IkReal(-1.00000000000000))*(x1215)*(x1238)))+(((IkReal(-1.00000000000000))*(cj3)*(x1215)*(x1236)))+(((IkReal(0.105999998468300))*(x1219)))+(((x1236)*(x1268)))+(((cj0)*(x1224)*(x1237)))+(((IkReal(0.999999985550000))*(x1237)*(x1256)))+(((x1232)*(x1273)))+(((pz)*(x1232)*(x1245)))+(((IkReal(-1.00000000000000))*(sj1)*(x1244)))+(((IkReal(0.105999998468300))*(x1221)))); -evalcond[6]=((((x1218)*(x1219)*(x1228)))+(((IkReal(1.89951939215389e-5))*(x1269)))+(((IkReal(-0.0237799923904037))*(x1214)*(x1222)))+(((IkReal(-1.00000000000000))*(x1216)*(x1219)*(x1228)))+(((IkReal(-1.00000000000000))*(x1218)*(x1221)*(x1228)))+(((IkReal(-0.0237439924019236))*(x1274)))+(((x1216)*(x1233)*(x1246)))+(((IkReal(-1.00000000000000))*(x1260)*(x1263)))+(((IkReal(-1.00000000000000))*(x1217)*(x1233)*(x1246)))+(((IkReal(-0.0112360000000000))*(x1237)))+(((x1219)*(x1227)*(x1234)))+(((IkReal(-1.00000000000000))*(x1241)*(x1267)))+(((IkReal(-1.00000000000000))*(x1218)*(x1233)*(x1246)))+(((x1221)*(x1259)))+(((IkReal(-0.0112359998376398))*(r00)*(x1223)))+(((r00)*(x1223)*(x1270)))+(((IkReal(-2.00000000000000))*(x1230)*(x1255)))+(((IkReal(-1.00000000000000))*(x1252)*(x1268)))+(((x1225)*(x1233)*(x1244)))+(((IkReal(-1.00000000000000))*(r00)*(x1223)*(x1272)))+(((IkReal(0.0112359998376398))*(r01)*(x1233)))+(((IkReal(-1.00000000000000))*(x1221)*(x1227)*(x1234)))+(((x1223)*(x1225)*(x1226)))+(((r00)*(x1223)*(x1266)))+(((IkReal(0.212000000000000))*(py)*(r01)))+(((IkReal(0.212000000000000))*(x1226)))+(((x1223)*(x1225)*(x1230)))+(((IkReal(-1.00000000000000))*(x1217)*(x1221)*(x1228)))+(((IkReal(1.90239939123229e-5))*(x1215)*(x1222)))+(((pp)*(x1237)))+(((x1226)*(x1227)*(x1250)))+(((IkReal(0.212000000000000))*(x1230)))+(((x1216)*(x1221)*(x1228)))+(((IkReal(-1.00000000000000))*(x1217)*(x1219)*(x1228)))+(((x1226)*(x1233)*(x1239)))+(((IkReal(-1.00000000000000))*(x1223)*(x1226)*(x1234)))+(((x1219)*(x1259)))+(((IkReal(-1.00000000000000))*(x1237)*(x1271)))); -evalcond[7]=((((IkReal(-1.89951939215389e-5))*(x1274)))+(((x1221)*(x1234)*(x1241)))+(((IkReal(-0.0237439924019236))*(x1269)))+(((x1265)*(x1272)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1225)*(x1243)))+(((IkReal(3.60399994792220e-5))*(sj0)*(x1242)))+(((IkReal(-1.00000000000000))*(x1225)*(x1226)*(x1249)))+(((IkReal(-1.00000000000000))*(x1226)*(x1241)*(x1250)))+(((IkReal(-1.00000000000000))*(x1227)*(x1267)))+(((IkReal(-1.00000000000000))*(x1248)*(x1271)))+(((IkReal(-1.00000000000000))*(x1216)*(x1245)*(x1254)))+(((x1217)*(x1221)*(x1232)))+(((IkReal(-1.00000000000000))*(x1220)*(x1221)))+(((IkReal(-1.00000000000000))*(x1219)*(x1234)*(x1241)))+(((IkReal(-2.00000000000000))*(x1230)*(x1262)))+(((x1221)*(x1258)))+(((IkReal(0.211999996936600))*(cj0)*(x1242)))+(((pp)*(x1248)))+(((IkReal(-1.00000000000000))*(x1252)*(x1263)))+(((x1218)*(x1245)*(x1254)))+(((IkReal(-0.0112359998376398))*(x1265)))+(((x1217)*(x1245)*(x1254)))+(((x1216)*(x1219)*(x1232)))+(((IkReal(-1.00000000000000))*(x1245)*(x1261)))+(((x1260)*(x1268)))+(((IkReal(0.0112360000000000))*(x1248)))+(((IkReal(-1.00000000000000))*(x1216)*(x1221)*(x1232)))+(((x1217)*(x1219)*(x1232)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1243)))+(((IkReal(-1.00000000000000))*(x1265)*(x1266)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x1226)*(x1239)))+(((IkReal(-0.211999996936600))*(x1273)))+(((IkReal(-0.0237799923904037))*(x1215)*(x1222)))+(((x1257)*(x1261)))+(((IkReal(-1.90239939123229e-5))*(x1214)*(x1222)))+(((IkReal(-1.00000000000000))*(x1265)*(x1270)))+(((IkReal(-1.00000000000000))*(x1218)*(x1219)*(x1232)))+(((IkReal(-1.00000000000000))*(x1219)*(x1220)))+(((IkReal(-1.00000000000000))*(x1225)*(x1230)*(x1249)))+(((x1219)*(x1258)))+(((x1218)*(x1221)*(x1232)))+(((IkReal(0.0112359998376398))*(cj1)*(x1245)))+(((x1226)*(x1234)*(x1249)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1275=((cj0)*(sj1)); -IkReal x1276=((IkReal(0.281562495931422))*(px)); -IkReal x1277=((IkReal(1750.00000000000))*(pz)); -IkReal x1278=((sj1)*(sj3)); -IkReal x1279=((cj1)*(cj3)); -IkReal x1280=((IkReal(1656.24997606719))*(cj1)); -IkReal x1281=((px)*(sj0)); -IkReal x1282=((cj3)*(sj1)); -IkReal x1283=((cj0)*(py)); -IkReal x1284=((cj1)*(sj3)); -IkReal x1285=((py)*(sj0)); -IkReal x1286=((IkReal(0.297499995701125))*(px)); -IkReal x1287=((IkReal(1749.99997471250))*(py)); -IkReal x1288=((IkReal(1656.25000000000))*(pz)); -if( IKabs(((gconst1)*(((((IkReal(1749.99997471250))*(x1278)*(x1281)))+(((IkReal(1749.99997471250))*(x1279)*(x1283)))+(((IkReal(-1749.99997471250))*(x1279)*(x1281)))+(((cj0)*(x1279)*(x1286)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x1275)*(x1287)))+(((IkReal(-1.00000000000000))*(sj3)*(x1275)*(x1286)))+(((cj0)*(cj1)*(x1276)))+(((IkReal(0.297499995701125))*(x1279)*(x1285)))+(((x1277)*(x1284)))+(((x1277)*(x1282)))+(((IkReal(-0.297499995701125))*(x1278)*(x1285)))+(((sj1)*(x1288)))+(((x1280)*(x1283)))+(((IkReal(0.281562495931422))*(cj1)*(x1285)))+(((IkReal(-1.00000000000000))*(x1280)*(x1281))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((IkReal(175.562500000000))+(((x1275)*(x1276)))+(((x1277)*(x1278)))+(((IkReal(0.297499995701125))*(x1282)*(x1285)))+(((cj3)*(x1275)*(x1286)))+(((cj3)*(x1275)*(x1287)))+(((IkReal(0.281562495931422))*(sj1)*(x1285)))+(((IkReal(-1656.24997606719))*(sj1)*(x1281)))+(((IkReal(-1.00000000000000))*(cj1)*(x1288)))+(((IkReal(-1.00000000000000))*(x1277)*(x1279)))+(((IkReal(0.297499995701125))*(x1284)*(x1285)))+(((IkReal(1656.24997606719))*(py)*(x1275)))+(((IkReal(-1749.99997471250))*(x1281)*(x1282)))+(((IkReal(-1749.99997471250))*(x1281)*(x1284)))+(((IkReal(1749.99997471250))*(x1283)*(x1284)))+(((IkReal(185.500000000000))*(cj3)))+(((cj0)*(x1284)*(x1286))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst1)*(((((IkReal(1749.99997471250))*(x1278)*(x1281)))+(((IkReal(1749.99997471250))*(x1279)*(x1283)))+(((IkReal(-1749.99997471250))*(x1279)*(x1281)))+(((cj0)*(x1279)*(x1286)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x1275)*(x1287)))+(((IkReal(-1.00000000000000))*(sj3)*(x1275)*(x1286)))+(((cj0)*(cj1)*(x1276)))+(((IkReal(0.297499995701125))*(x1279)*(x1285)))+(((x1277)*(x1284)))+(((x1277)*(x1282)))+(((IkReal(-0.297499995701125))*(x1278)*(x1285)))+(((sj1)*(x1288)))+(((x1280)*(x1283)))+(((IkReal(0.281562495931422))*(cj1)*(x1285)))+(((IkReal(-1.00000000000000))*(x1280)*(x1281)))))), ((gconst1)*(((IkReal(175.562500000000))+(((x1275)*(x1276)))+(((x1277)*(x1278)))+(((IkReal(0.297499995701125))*(x1282)*(x1285)))+(((cj3)*(x1275)*(x1286)))+(((cj3)*(x1275)*(x1287)))+(((IkReal(0.281562495931422))*(sj1)*(x1285)))+(((IkReal(-1656.24997606719))*(sj1)*(x1281)))+(((IkReal(-1.00000000000000))*(cj1)*(x1288)))+(((IkReal(-1.00000000000000))*(x1277)*(x1279)))+(((IkReal(0.297499995701125))*(x1284)*(x1285)))+(((IkReal(1656.24997606719))*(py)*(x1275)))+(((IkReal(-1749.99997471250))*(x1281)*(x1282)))+(((IkReal(-1749.99997471250))*(x1281)*(x1284)))+(((IkReal(1749.99997471250))*(x1283)*(x1284)))+(((IkReal(185.500000000000))*(cj3)))+(((cj0)*(x1284)*(x1286))))))); -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[8]; -IkReal x1289=IKsin(j2); -IkReal x1290=IKcos(j2); -IkReal x1291=(py)*(py); -IkReal x1292=(pz)*(pz); -IkReal x1293=(px)*(px); -IkReal x1294=((cj0)*(r00)); -IkReal x1295=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1296=((r01)*(sj0)); -IkReal x1297=((cj3)*(cj5)); -IkReal x1298=((sj0)*(sj1)); -IkReal x1299=((IkReal(0.999999985550000))*(px)); -IkReal x1300=((IkReal(0.000339999995087000))*(py)); -IkReal x1301=((pz)*(r02)); -IkReal x1302=((py)*(sj1)); -IkReal x1303=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1304=((IkReal(0.999999680000154))*(cj5)); -IkReal x1305=((px)*(r00)); -IkReal x1306=((IkReal(0.000799999744000123))*(cj5)); -IkReal x1307=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1308=((cj0)*(sj1)); -IkReal x1309=((IkReal(1.99999997110000))*(px)); -IkReal x1310=((IkReal(0.000169999997543500))*(pz)); -IkReal x1311=((IkReal(0.112000000000000))*(sj5)); -IkReal x1312=((cj1)*(r02)); -IkReal x1313=((IkReal(0.106000000000000))*(sj5)); -IkReal x1314=((IkReal(0.000339999995087000))*(px)); -IkReal x1315=((IkReal(0.112000000000000))*(cj3)); -IkReal x1316=((cj1)*(py)); -IkReal x1317=((py)*(r02)); -IkReal x1318=((cj0)*(px)); -IkReal x1319=((px)*(r01)); -IkReal x1320=((cj0)*(r01)); -IkReal x1321=((IkReal(0.999999985550000))*(r01)); -IkReal x1322=((IkReal(0.999999985550000))*(pz)); -IkReal x1323=((r02)*(sj1)); -IkReal x1324=((cj1)*(sj0)); -IkReal x1325=((IkReal(1.99999997110000))*(cj0)); -IkReal x1326=((IkReal(0.000169999997543500))*(r02)); -IkReal x1327=((IkReal(1.04639966515216e-6))*(cj5)); -IkReal x1328=((IkReal(0.999999985550000))*(cj0)); -IkReal x1329=((IkReal(0.999999985550000))*(cj1)); -IkReal x1330=((cj1)*(pz)); -IkReal x1331=((py)*(sj0)); -IkReal x1332=((r00)*(sj0)); -IkReal x1333=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1334=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1335=((IkReal(0.00130799958144020))*(cj5)); -IkReal x1336=((IkReal(0.211999996936600))*(pz)); -IkReal x1337=((pz)*(sj1)); -IkReal x1338=((sj3)*(x1290)); -IkReal x1339=((cj1)*(x1319)); -IkReal x1340=((r00)*(x1324)); -IkReal x1341=((IkReal(0.999999985550000))*(x1292)); -IkReal x1342=((IkReal(2.00000000000000))*(pz)*(r01)); -IkReal x1343=((sj3)*(x1289)); -IkReal x1344=((cj5)*(x1290)); -IkReal x1345=((IkReal(0.999999985550000))*(x1291)); -IkReal x1346=((IkReal(2.00000000000000))*(x1292)); -IkReal x1347=((IkReal(0.999999985550000))*(x1293)); -IkReal x1348=((px)*(r02)*(sj0)); -IkReal x1349=((cj5)*(x1289)); -evalcond[0]=((x1337)+(((x1316)*(x1328)))+(((IkReal(0.112000000000000))*(x1338)))+(((x1289)*(x1315)))+(((x1307)*(x1318)))+(((x1307)*(x1331)))+(((IkReal(-1.00000000000000))*(x1299)*(x1324)))+(((IkReal(0.106000000000000))*(x1289)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-0.106000000000000))*(x1290)))+(x1330)+(((IkReal(-0.000169999997543500))*(py)*(x1298)))+(((IkReal(0.112000000000000))*(x1343)))+(((IkReal(-1.00000000000000))*(x1303)*(x1318)))+(((IkReal(-1.00000000000000))*(x1290)*(x1315)))+(((IkReal(-1.00000000000000))*(x1302)*(x1328)))+(((x1298)*(x1299)))); -evalcond[2]=((x1323)+(((x1320)*(x1329)))+(((x1306)*(x1338)))+(((IkReal(-0.999999680000154))*(x1290)*(x1297)))+(((IkReal(-0.999999985550000))*(x1340)))+(((x1294)*(x1307)))+(((IkReal(0.000799999744000123))*(x1289)*(x1297)))+(((x1296)*(x1307)))+(((x1304)*(x1343)))); -evalcond[3]=((x1312)+(((IkReal(-0.999999680000154))*(x1289)*(x1297)))+(((IkReal(-1.00000000000000))*(x1308)*(x1321)))+(((IkReal(-0.000799999744000123))*(x1290)*(x1297)))+(((IkReal(0.999999985550000))*(r00)*(x1298)))+(((IkReal(-1.00000000000000))*(x1296)*(x1303)))+(((IkReal(-1.00000000000000))*(x1294)*(x1303)))+(((x1306)*(x1343)))+(((IkReal(-1.00000000000000))*(x1304)*(x1338)))); -evalcond[4]=((((r00)*(x1316)))+(((IkReal(-1.00000000000000))*(cj3)*(x1289)*(x1311)))+(((r00)*(x1298)*(x1310)))+(((IkReal(-1.00000000000000))*(x1339)))+(((IkReal(-1.00000000000000))*(r02)*(x1299)*(x1308)))+(((sj1)*(x1296)*(x1322)))+(((sj1)*(x1294)*(x1322)))+(((cj0)*(x1302)*(x1326)))+(((IkReal(-1.00000000000000))*(px)*(x1298)*(x1326)))+(((IkReal(-0.999999985550000))*(x1298)*(x1317)))+(((IkReal(-1.00000000000000))*(pz)*(x1303)*(x1320)))+(((IkReal(-1.00000000000000))*(x1289)*(x1313)))+(((IkReal(-1.00000000000000))*(x1311)*(x1338)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(cj0)*(x1307)*(x1317)))+(((IkReal(-1.00000000000000))*(pz)*(x1307)*(x1332)))+(((cj0)*(x1299)*(x1312)))+(((IkReal(-1.00000000000000))*(cj3)*(x1290)*(x1311)))+(((IkReal(0.105999998468300))*(x1296)))+(((IkReal(0.105999998468300))*(x1294)))+(((IkReal(-1.00000000000000))*(sj1)*(x1319)))+(((IkReal(-1.00000000000000))*(cj1)*(x1294)*(x1322)))+(((pz)*(x1307)*(x1320)))+(((x1311)*(x1343)))+(((IkReal(-1.00000000000000))*(cj1)*(x1296)*(x1322)))+(((IkReal(1.80199997396110e-5))*(x1332)))+(((IkReal(-1.80199997396110e-5))*(x1320)))+(((x1307)*(x1348)))+(((IkReal(0.999999985550000))*(x1312)*(x1331)))+(((IkReal(-1.00000000000000))*(x1290)*(x1313)))+(((r00)*(x1302)))); -evalcond[6]=((((x1296)*(x1334)))+(((IkReal(-1.00000000000000))*(x1335)*(x1338)))+(((IkReal(-1.00000000000000))*(x1298)*(x1301)*(x1309)))+(((IkReal(-1.00000000000000))*(x1292)*(x1296)*(x1303)))+(((r00)*(x1298)*(x1341)))+(((r00)*(x1298)*(x1345)))+(((IkReal(-1.00000000000000))*(x1296)*(x1302)*(x1309)))+(((IkReal(1.90239939123229e-5))*(x1290)*(x1297)))+(((IkReal(-1.00000000000000))*(x1291)*(x1294)*(x1303)))+(((x1301)*(x1302)*(x1325)))+(((IkReal(-1.00000000000000))*(x1312)*(x1346)))+(((x1294)*(x1334)))+(((IkReal(-0.0237439924019236))*(x1349)))+(((IkReal(-1.00000000000000))*(x1293)*(x1296)*(x1303)))+(((IkReal(-0.0237799923904037))*(x1289)*(x1297)))+(((pp)*(x1312)))+(((x1298)*(x1300)*(x1301)))+(((x1298)*(x1300)*(x1305)))+(((x1294)*(x1302)*(x1309)))+(((IkReal(-1.00000000000000))*(x1316)*(x1342)))+(((IkReal(-1.00000000000000))*(x1293)*(x1308)*(x1321)))+(((IkReal(0.212000000000000))*(x1305)))+(((IkReal(0.212000000000000))*(x1301)))+(((IkReal(-1.00000000000000))*(r00)*(x1298)*(x1347)))+(((x1300)*(x1308)*(x1319)))+(((x1301)*(x1308)*(x1314)))+(((IkReal(0.212000000000000))*(py)*(r01)))+(((IkReal(-0.0112359998376398))*(r00)*(x1298)))+(((IkReal(1.89951939215389e-5))*(x1344)))+(((IkReal(-0.0112360000000000))*(x1312)))+(((x1291)*(x1308)*(x1321)))+(((IkReal(0.0112359998376398))*(r01)*(x1308)))+(((IkReal(-1.00000000000000))*(x1327)*(x1343)))+(((IkReal(-1.00000000000000))*(x1292)*(x1294)*(x1303)))+(((x1291)*(x1296)*(x1303)))+(((IkReal(-2.00000000000000))*(x1305)*(x1330)))+(((IkReal(-1.00000000000000))*(x1292)*(x1308)*(x1321)))+(((x1293)*(x1294)*(x1303)))); -evalcond[7]=((((x1296)*(x1333)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1318)))+(((x1292)*(x1296)*(x1307)))+(((x1291)*(x1294)*(x1307)))+(((IkReal(-1.00000000000000))*(x1300)*(x1305)*(x1324)))+(((IkReal(-1.00000000000000))*(x1327)*(x1338)))+(((x1292)*(x1294)*(x1307)))+(((IkReal(-1.00000000000000))*(x1340)*(x1341)))+(((IkReal(-1.00000000000000))*(x1340)*(x1345)))+(((IkReal(-0.0237799923904037))*(x1290)*(x1297)))+(((x1294)*(x1333)))+(((IkReal(-0.0237439924019236))*(x1344)))+(((IkReal(0.211999996936600))*(cj0)*(x1317)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1300)*(x1318)))+(((IkReal(-1.00000000000000))*(x1294)*(x1309)*(x1316)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x1301)*(x1314)))+(((IkReal(-1.00000000000000))*(x1293)*(x1294)*(x1307)))+(((x1340)*(x1347)))+(((pp)*(x1323)))+(((IkReal(-1.90239939123229e-5))*(x1289)*(x1297)))+(((IkReal(-1.00000000000000))*(x1300)*(x1301)*(x1324)))+(((IkReal(-1.00000000000000))*(x1320)*(x1336)))+(((IkReal(-0.0112359998376398))*(x1340)))+(((IkReal(-1.00000000000000))*(x1291)*(x1296)*(x1307)))+(((IkReal(-1.00000000000000))*(x1295)*(x1296)))+(((IkReal(-1.00000000000000))*(x1291)*(x1320)*(x1329)))+(((IkReal(-1.89951939215389e-5))*(x1349)))+(((IkReal(3.60399994792220e-5))*(sj0)*(x1317)))+(((x1292)*(x1320)*(x1329)))+(((IkReal(-1.00000000000000))*(x1301)*(x1316)*(x1325)))+(((IkReal(-1.00000000000000))*(x1302)*(x1342)))+(((x1335)*(x1343)))+(((x1293)*(x1296)*(x1307)))+(((IkReal(0.0112359998376398))*(cj1)*(x1320)))+(((IkReal(-1.00000000000000))*(x1294)*(x1295)))+(((x1301)*(x1309)*(x1324)))+(((IkReal(-0.211999996936600))*(x1348)))+(((IkReal(-1.00000000000000))*(x1323)*(x1346)))+(((x1293)*(x1320)*(x1329)))+(((IkReal(-2.00000000000000))*(x1305)*(x1337)))+(((IkReal(0.0112360000000000))*(x1323)))+(((x1332)*(x1336)))+(((x1296)*(x1309)*(x1316)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j3array[1], cj3array[1], sj3array[1]; -bool j3valid[1]={false}; -_nj3 = 1; -IkReal x1350=((IkReal(66250.0202426838))*(sj1)); -IkReal x1351=((r00)*(sj0)); -IkReal x1352=((cj0)*(cj1)); -IkReal x1353=((IkReal(14078.1293015703))*(r00)); -IkReal x1354=((cj0)*(r01)); -IkReal x1355=((py)*(r01)); -IkReal x1356=((r01)*(sj0)); -IkReal x1357=((IkReal(82812525.3033548))*(sj1)); -IkReal x1358=((cj1)*(r02)); -IkReal x1359=((py)*(r02)); -IkReal x1360=((pz)*(r02)); -IkReal x1361=((px)*(r00)); -IkReal x1362=((IkReal(11.2625034412562))*(sj1)); -IkReal x1363=((r02)*(sj1)); -IkReal x1364=((px)*(r02)); -IkReal x1365=((IkReal(781250238.710894))*(pz)); -IkReal x1366=((IkReal(625000.190968715))*(pz)); -IkReal x1367=((cj0)*(pz)*(r00)); -if( IKabs(((gconst0)*(((((IkReal(781250249.999960))*(x1355)))+(((IkReal(-625000.190968715))*(sj0)*(x1364)))+(((IkReal(781250249.999960))*(x1360)))+(((IkReal(781250249.999960))*(x1361)))+(((IkReal(106.250032464682))*(cj0)*(x1364)))+(((IkReal(14078.1293015703))*(sj1)*(x1356)))+(((IkReal(-1.00000000000000))*(x1354)*(x1366)))+(((IkReal(-106.250032464682))*(pz)*(x1356)))+(((x1354)*(x1357)))+(((IkReal(11.2625034412562))*(cj1)*(x1356)))+(((IkReal(11.2625034412562))*(r00)*(x1352)))+(((IkReal(66250.0202426838))*(r01)*(x1352)))+(((IkReal(-106.250032464682))*(x1367)))+(((IkReal(625000.190968715))*(cj0)*(x1359)))+(((x1351)*(x1366)))+(((IkReal(-82812526.4999958))*(x1358)))+(((cj0)*(sj1)*(x1353)))+(((IkReal(106.250032464682))*(sj0)*(x1359)))+(((IkReal(-1.00000000000000))*(x1351)*(x1357)))+(((IkReal(-66250.0202426838))*(cj1)*(x1351)))+(((IkReal(66250.0211999966))*(x1363))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((x1356)*(x1362)))+(((IkReal(-1.00000000000000))*(x1352)*(x1353)))+(((IkReal(-82812525.3033548))*(r01)*(x1352)))+(((IkReal(-87500056.0000000))*(cj5)))+(((cj0)*(r00)*(x1362)))+(((IkReal(-14078.1293015703))*(cj1)*(x1356)))+(((x1354)*(x1365)))+(((IkReal(-1.00000000000000))*(x1350)*(x1351)))+(((IkReal(-132812.540580852))*(sj0)*(x1359)))+(((IkReal(-132812.540580852))*(cj0)*(x1364)))+(((IkReal(-66250.0211999966))*(x1358)))+(((IkReal(781250238.710894))*(sj0)*(x1364)))+(((x1350)*(x1354)))+(((IkReal(132812.540580852))*(pz)*(x1356)))+(((IkReal(625000.199999968))*(x1361)))+(((IkReal(625000.199999968))*(x1360)))+(((IkReal(82812525.3033548))*(cj1)*(x1351)))+(((IkReal(132812.540580852))*(x1367)))+(((IkReal(625000.199999968))*(x1355)))+(((IkReal(-82812526.4999958))*(x1363)))+(((IkReal(-781250238.710894))*(cj0)*(x1359)))+(((IkReal(-1.00000000000000))*(x1351)*(x1365))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j3array[0]=IKatan2(((gconst0)*(((((IkReal(781250249.999960))*(x1355)))+(((IkReal(-625000.190968715))*(sj0)*(x1364)))+(((IkReal(781250249.999960))*(x1360)))+(((IkReal(781250249.999960))*(x1361)))+(((IkReal(106.250032464682))*(cj0)*(x1364)))+(((IkReal(14078.1293015703))*(sj1)*(x1356)))+(((IkReal(-1.00000000000000))*(x1354)*(x1366)))+(((IkReal(-106.250032464682))*(pz)*(x1356)))+(((x1354)*(x1357)))+(((IkReal(11.2625034412562))*(cj1)*(x1356)))+(((IkReal(11.2625034412562))*(r00)*(x1352)))+(((IkReal(66250.0202426838))*(r01)*(x1352)))+(((IkReal(-106.250032464682))*(x1367)))+(((IkReal(625000.190968715))*(cj0)*(x1359)))+(((x1351)*(x1366)))+(((IkReal(-82812526.4999958))*(x1358)))+(((cj0)*(sj1)*(x1353)))+(((IkReal(106.250032464682))*(sj0)*(x1359)))+(((IkReal(-1.00000000000000))*(x1351)*(x1357)))+(((IkReal(-66250.0202426838))*(cj1)*(x1351)))+(((IkReal(66250.0211999966))*(x1363)))))), ((gconst0)*(((((x1356)*(x1362)))+(((IkReal(-1.00000000000000))*(x1352)*(x1353)))+(((IkReal(-82812525.3033548))*(r01)*(x1352)))+(((IkReal(-87500056.0000000))*(cj5)))+(((cj0)*(r00)*(x1362)))+(((IkReal(-14078.1293015703))*(cj1)*(x1356)))+(((x1354)*(x1365)))+(((IkReal(-1.00000000000000))*(x1350)*(x1351)))+(((IkReal(-132812.540580852))*(sj0)*(x1359)))+(((IkReal(-132812.540580852))*(cj0)*(x1364)))+(((IkReal(-66250.0211999966))*(x1358)))+(((IkReal(781250238.710894))*(sj0)*(x1364)))+(((x1350)*(x1354)))+(((IkReal(132812.540580852))*(pz)*(x1356)))+(((IkReal(625000.199999968))*(x1361)))+(((IkReal(625000.199999968))*(x1360)))+(((IkReal(82812525.3033548))*(cj1)*(x1351)))+(((IkReal(132812.540580852))*(x1367)))+(((IkReal(625000.199999968))*(x1355)))+(((IkReal(-82812526.4999958))*(x1363)))+(((IkReal(-781250238.710894))*(cj0)*(x1359)))+(((IkReal(-1.00000000000000))*(x1351)*(x1365))))))); -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 x1368=IKcos(j3); -IkReal x1369=IKsin(j3); -IkReal x1370=(px)*(px); -IkReal x1371=(py)*(py); -IkReal x1372=(pz)*(pz); -IkReal x1373=((r00)*(sj0)); -IkReal x1374=((cj0)*(r00)); -IkReal x1375=((py)*(sj1)); -IkReal x1376=((cj1)*(pz)); -IkReal x1377=((cj0)*(r01)); -IkReal x1378=((px)*(py)); -IkReal x1379=((r01)*(sj0)); -IkReal x1380=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x1381=((cj0)*(r02)); -IkReal x1382=((IkReal(3.60399994792220e-5))*(cj1)); -IkReal x1383=((IkReal(0.105999966080016))*(cj5)); -IkReal x1384=((IkReal(0.999999985550000))*(pz)); -IkReal x1385=((px)*(sj1)); -IkReal x1386=((IkReal(0.106000000000000))*(r02)); -IkReal x1387=((IkReal(8.47999728640130e-5))*(cj5)); -IkReal x1388=((IkReal(0.000169999997543500))*(pz)); -IkReal x1389=((IkReal(0.211999996936600))*(cj1)); -IkReal x1390=((IkReal(0.000339999995087000))*(pz)); -IkReal x1391=((IkReal(0.105999998468300))*(cj1)); -IkReal x1392=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x1393=((IkReal(1.99999997110000))*(pz)); -IkReal x1394=((IkReal(0.105999998468300))*(sj1)); -IkReal x1395=((IkReal(0.999999985550000))*(x1371)); -IkReal x1396=((IkReal(0.999999985550000))*(x1372)); -IkReal x1397=((IkReal(0.000169999997543500))*(x1370)); -IkReal x1398=((IkReal(0.000169999997543500))*(x1371)); -IkReal x1399=((IkReal(0.000169999997543500))*(x1372)); -IkReal x1400=((px)*(r02)*(sj0)); -IkReal x1401=((IkReal(0.999999985550000))*(x1370)); -IkReal x1402=((py)*(r02)*(sj0)); -IkReal x1403=((IkReal(0.0237440000000000))*(x1368)); -evalcond[0]=((IkReal(0.0125440000000000))+(((IkReal(-0.211999996936600))*(cj0)*(x1375)))+(((IkReal(0.211999996936600))*(sj0)*(x1385)))+(((IkReal(-3.60399994792220e-5))*(sj0)*(x1375)))+(((IkReal(-3.60399994792220e-5))*(cj0)*(x1385)))+(((IkReal(0.212000000000000))*(x1376)))+(((IkReal(-1.00000000000000))*(pp)))+(x1403)); -evalcond[1]=((((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-1.00000000000000))*(x1377)*(x1394)))+(((IkReal(-1.00000000000000))*(x1374)*(x1380)))+(((IkReal(8.95999713280138e-5))*(cj5)))+(((IkReal(-1.00000000000000))*(pz)*(r02)))+(((x1373)*(x1394)))+(((IkReal(-1.00000000000000))*(py)*(r01)))+(((cj1)*(x1386)))+(((x1369)*(x1383)))+(((x1368)*(x1387)))+(((IkReal(-1.00000000000000))*(x1379)*(x1380)))); -evalcond[2]=((((IkReal(0.000169999997543500))*(px)*(x1381)))+(((IkReal(-0.999999985550000))*(x1400)))+(((IkReal(-1.00000000000000))*(x1377)*(x1384)))+(((IkReal(-1.00000000000000))*(x1374)*(x1388)))+(((sj1)*(x1386)))+(((x1379)*(x1392)))+(((x1377)*(x1391)))+(((x1374)*(x1392)))+(((IkReal(0.000169999997543500))*(x1402)))+(((x1373)*(x1384)))+(((IkReal(-1.00000000000000))*(x1369)*(x1387)))+(((IkReal(0.111999964160017))*(cj5)))+(((x1368)*(x1383)))+(((IkReal(-1.00000000000000))*(x1379)*(x1388)))+(((IkReal(0.999999985550000))*(py)*(x1381)))+(((IkReal(-1.00000000000000))*(x1373)*(x1391)))); -evalcond[3]=((((IkReal(0.211999996936600))*(x1376)*(x1379)))+(((IkReal(-1.00000000000000))*(px)*(x1381)*(x1389)))+(((IkReal(-1.00000000000000))*(x1389)*(x1402)))+(((IkReal(0.212000000000000))*(r01)*(x1385)))+(((px)*(x1381)*(x1393)))+(((IkReal(-1.00000000000000))*(py)*(x1381)*(x1390)))+(((IkReal(-0.0112359998376398))*(x1374)))+(((IkReal(-0.0112359998376398))*(x1379)))+(((IkReal(3.60399994792220e-5))*(x1373)*(x1376)))+(((IkReal(0.000339999995087000))*(x1378)*(x1379)))+(((IkReal(1.99999997110000))*(x1377)*(x1378)))+(((x1393)*(x1402)))+(((IkReal(0.211999996936600))*(x1374)*(x1376)))+(((IkReal(-1.00000000000000))*(x1377)*(x1398)))+(((py)*(x1381)*(x1382)))+(((IkReal(-0.212000000000000))*(r00)*(x1375)))+(((IkReal(-1.00000000000000))*(x1382)*(x1400)))+(((IkReal(-1.00000000000000))*(x1374)*(x1396)))+(((IkReal(-1.00000000000000))*(x1374)*(x1395)))+(((IkReal(-0.0237800000000000))*(sj5)))+(((x1379)*(x1395)))+(((IkReal(-1.00000000000000))*(x1379)*(x1401)))+(((IkReal(-1.91011997239877e-6))*(x1373)))+(((x1377)*(x1397)))+(((x1377)*(x1399)))+(((IkReal(-0.000339999995087000))*(x1374)*(x1378)))+(((x1373)*(x1397)))+(((IkReal(1.99999997110000))*(x1373)*(x1378)))+(((IkReal(1.91011997239877e-6))*(x1377)))+(((IkReal(-1.00000000000000))*(sj5)*(x1403)))+(((IkReal(-3.60399994792220e-5))*(x1376)*(x1377)))+(((x1390)*(x1400)))+(((IkReal(-1.00000000000000))*(x1373)*(x1398)))+(((IkReal(-1.00000000000000))*(x1373)*(x1399)))+(((x1374)*(x1401)))+(((IkReal(-1.00000000000000))*(x1379)*(x1396)))); -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 gconst1; -gconst1=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst2; -IkReal x1404=((IkReal(13.9999955200021))*(cj5)); -gconst2=IKsign(((((IkReal(-1.00000000000000))*(x1404)*((cj3)*(cj3))))+(((IkReal(0.0105999966080016))*(cj5)*(sj3)))+(((IkReal(-1.00000000000000))*(x1404)*((sj3)*(sj3))))+(((IkReal(-13.2499957600020))*(cj3)*(cj5))))); -IkReal x1405=((IkReal(1320.75471698113))*(cj5)); -dummyeval[0]=((((cj5)*(sj3)))+(((IkReal(-1250.00000000000))*(cj3)*(cj5)))+(((IkReal(-1.00000000000000))*(x1405)*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*(x1405)*((sj3)*(sj3))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x1406=(px)*(px); -IkReal x1407=(py)*(py); -IkReal x1408=(pz)*(pz); -IkReal x1409=((IkReal(0.0237440000000000))*(cj3)); -IkReal x1410=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1411=((cj0)*(r00)); -IkReal x1412=((r01)*(sj0)); -IkReal x1413=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1414=((IkReal(0.000339999995087000))*(py)); -IkReal x1415=((pz)*(sj1)); -IkReal x1416=((r02)*(sj0)); -IkReal x1417=((cj0)*(px)); -IkReal x1418=((cj1)*(r01)); -IkReal x1419=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1420=((IkReal(3.60399994792220e-5))*(sj1)); -IkReal x1421=((py)*(r01)); -IkReal x1422=((r00)*(sj0)); -IkReal x1423=((IkReal(0.000169999997543500))*(pz)); -IkReal x1424=((IkReal(0.105999998468300))*(cj0)); -IkReal x1425=((px)*(r00)); -IkReal x1426=((IkReal(1.99999997110000))*(pz)); -IkReal x1427=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1428=((r01)*(sj1)); -IkReal x1429=((cj1)*(r02)); -IkReal x1430=((r02)*(sj1)); -IkReal x1431=((IkReal(0.999999985550000))*(py)); -IkReal x1432=((IkReal(0.999999985550000))*(cj0)); -IkReal x1433=((IkReal(0.211999996936600))*(px)); -IkReal x1434=((IkReal(0.0112359998376398))*(cj0)); -IkReal x1435=((cj0)*(r02)); -IkReal x1436=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x1437=((IkReal(0.211999996936600))*(py)); -IkReal x1438=((cj1)*(px)); -IkReal x1439=((IkReal(0.000339999995087000))*(pz)); -IkReal x1440=((IkReal(0.999999985550000))*(cj1)); -IkReal x1441=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1442=((cj0)*(py)); -IkReal x1443=((pz)*(r02)); -IkReal x1444=((IkReal(0.211999996936600))*(pz)); -IkReal x1445=((cj1)*(pz)); -IkReal x1446=((IkReal(0.999999985550000))*(sj1)); -IkReal x1447=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x1448=((cj0)*(r01)); -IkReal x1449=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1450=((IkReal(0.999999985550000))*(x1408)); -IkReal x1451=((IkReal(0.000169999997543500))*(x1407)); -IkReal x1452=((IkReal(0.999999985550000))*(x1406)); -IkReal x1453=((IkReal(0.000169999997543500))*(x1408)); -IkReal x1454=((IkReal(1.99999997110000))*(px)*(py)); -IkReal x1455=((IkReal(0.000169999997543500))*(x1406)); -IkReal x1456=((IkReal(2.00000000000000))*(x1408)); -IkReal x1457=((IkReal(0.999999985550000))*(x1407)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((py)*(x1419)))+(((IkReal(-1.00000000000000))*(sj0)*(x1431)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((IkReal(-0.999999985550000))*(x1417)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((r01)*(x1419)))+(((IkReal(-0.999999985550000))*(x1412)))+(((IkReal(-0.999999985550000))*(x1411)))+(((IkReal(-0.000169999997543500))*(x1422)))); -evalcond[3]=((IkReal(0.0125440000000000))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x1437)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x1420)))+(((IkReal(0.212000000000000))*(x1445)))+(((IkReal(-1.00000000000000))*(x1417)*(x1420)))+(((IkReal(-1.00000000000000))*(pp)))+(x1409)+(((sj0)*(sj1)*(x1433)))); -evalcond[4]=((((IkReal(0.105999998468300))*(sj1)*(x1422)))+(((IkReal(-1.00000000000000))*(x1424)*(x1428)))+(((IkReal(-1.00000000000000))*(x1443)))+(((IkReal(0.106000000000000))*(x1429)))+(((IkReal(-1.00000000000000))*(x1412)*(x1436)))+(((IkReal(-1.00000000000000))*(x1421)))+(((IkReal(-1.00000000000000))*(x1425)))+(((IkReal(-1.00000000000000))*(x1411)*(x1436)))); -evalcond[5]=((((x1411)*(x1441)))+(((x1418)*(x1432)))+(x1430)+(((IkReal(-1.00000000000000))*(x1422)*(x1440)))+(((x1412)*(x1441)))); -evalcond[6]=((((x1422)*(x1446)))+(((IkReal(-1.00000000000000))*(x1412)*(x1413)))+(((IkReal(-1.00000000000000))*(x1428)*(x1432)))+(((IkReal(-1.00000000000000))*(x1411)*(x1413)))+(x1429)); -evalcond[7]=((((x1411)*(x1447)))+(((IkReal(0.000169999997543500))*(r02)*(x1417)))+(((IkReal(0.000169999997543500))*(py)*(x1416)))+(((x1418)*(x1424)))+(((IkReal(-0.105999998468300))*(cj1)*(x1422)))+(((IkReal(0.999999985550000))*(pz)*(x1422)))+(((IkReal(-1.00000000000000))*(x1412)*(x1423)))+(((x1412)*(x1447)))+(((IkReal(0.106000000000000))*(x1430)))+(((x1431)*(x1435)))+(((IkReal(-1.00000000000000))*(x1411)*(x1423)))+(((IkReal(-1.00000000000000))*(pz)*(r01)*(x1432)))+(((IkReal(-0.999999985550000))*(px)*(x1416)))); -evalcond[8]=((IkReal(-0.0237800000000000))+(((cj1)*(x1410)*(x1422)))+(((IkReal(-1.00000000000000))*(px)*(x1411)*(x1414)))+(((IkReal(-1.00000000000000))*(x1422)*(x1453)))+(((IkReal(-1.00000000000000))*(x1422)*(x1451)))+(((cj1)*(x1412)*(x1444)))+(((IkReal(3.60399994792220e-5))*(x1429)*(x1442)))+(((IkReal(-1.00000000000000))*(cj0)*(x1410)*(x1418)))+(((r01)*(x1406)*(x1419)))+(((r02)*(x1417)*(x1426)))+(((px)*(x1416)*(x1439)))+(((IkReal(-3.60399994792220e-5))*(x1416)*(x1438)))+(((IkReal(1.91011997239877e-6))*(x1448)))+(((x1412)*(x1457)))+(((IkReal(-1.00000000000000))*(cj1)*(x1416)*(x1437)))+(((IkReal(-1.00000000000000))*(x1411)*(x1457)))+(((IkReal(-1.00000000000000))*(x1411)*(x1450)))+(((IkReal(-1.00000000000000))*(x1409)))+(((IkReal(-1.00000000000000))*(r01)*(x1407)*(x1419)))+(((IkReal(-0.0112359998376398))*(x1412)))+(((IkReal(-0.0112359998376398))*(x1411)))+(((IkReal(-1.00000000000000))*(x1412)*(x1452)))+(((IkReal(-1.00000000000000))*(x1412)*(x1450)))+(((x1422)*(x1455)))+(((x1422)*(x1454)))+(((IkReal(1.99999997110000))*(x1417)*(x1421)))+(((x1411)*(x1452)))+(((IkReal(-1.00000000000000))*(pz)*(x1414)*(x1435)))+(((IkReal(-0.212000000000000))*(py)*(r00)*(sj1)))+(((IkReal(0.212000000000000))*(px)*(x1428)))+(((py)*(x1416)*(x1426)))+(((r01)*(x1408)*(x1419)))+(((cj1)*(x1411)*(x1444)))+(((px)*(x1412)*(x1414)))+(((IkReal(-0.211999996936600))*(x1417)*(x1429)))+(((IkReal(-1.91011997239877e-6))*(x1422)))); -evalcond[9]=((((x1428)*(x1434)))+(((IkReal(0.000339999995087000))*(r02)*(x1415)*(x1417)))+(((IkReal(-1.00000000000000))*(x1406)*(x1412)*(x1413)))+(((x1411)*(x1449)))+(((IkReal(-0.0112359998376398))*(sj1)*(x1422)))+(((IkReal(-1.00000000000000))*(x1429)*(x1456)))+(((IkReal(-0.0112360000000000))*(x1429)))+(((IkReal(-2.00000000000000))*(py)*(pz)*(x1418)))+(((x1407)*(x1428)*(x1432)))+(((pp)*(x1429)))+(((x1407)*(x1412)*(x1413)))+(((IkReal(-1.00000000000000))*(x1406)*(x1422)*(x1446)))+(((IkReal(0.212000000000000))*(x1443)))+(((IkReal(1.99999997110000))*(py)*(x1415)*(x1435)))+(((IkReal(0.212000000000000))*(x1421)))+(((IkReal(0.212000000000000))*(x1425)))+(((IkReal(-1.00000000000000))*(x1406)*(x1428)*(x1432)))+(((IkReal(-1.00000000000000))*(x1407)*(x1411)*(x1413)))+(((x1414)*(x1415)*(x1416)))+(((IkReal(-2.00000000000000))*(x1425)*(x1445)))+(((IkReal(-1.00000000000000))*(sj1)*(x1412)*(x1454)))+(((x1408)*(x1422)*(x1446)))+(((px)*(sj1)*(x1414)*(x1422)))+(((IkReal(-1.00000000000000))*(x1408)*(x1411)*(x1413)))+(((IkReal(-1.00000000000000))*(x1408)*(x1428)*(x1432)))+(((sj1)*(x1411)*(x1454)))+(((x1406)*(x1411)*(x1413)))+(((x1407)*(x1422)*(x1446)))+(((x1412)*(x1449)))+(((IkReal(-1.99999997110000))*(px)*(x1415)*(x1416)))+(((x1414)*(x1417)*(x1428)))+(((IkReal(-1.00000000000000))*(x1408)*(x1412)*(x1413)))); -evalcond[10]=((((IkReal(-1.00000000000000))*(x1407)*(x1412)*(x1441)))+(((x1422)*(x1444)))+(((IkReal(-1.99999997110000))*(py)*(x1411)*(x1438)))+(((x1406)*(x1418)*(x1432)))+(((IkReal(-0.0112359998376398))*(cj1)*(x1422)))+(((x1408)*(x1411)*(x1441)))+(((IkReal(0.0112360000000000))*(x1430)))+(((x1418)*(x1434)))+(((x1406)*(x1422)*(x1440)))+(((x1411)*(x1427)))+(((IkReal(-1.00000000000000))*(x1407)*(x1418)*(x1432)))+(((x1435)*(x1437)))+(((IkReal(-1.00000000000000))*(x1426)*(x1429)*(x1442)))+(((IkReal(-1.00000000000000))*(x1414)*(x1417)*(x1418)))+(((x1406)*(x1412)*(x1441)))+(((IkReal(-1.00000000000000))*(x1408)*(x1422)*(x1440)))+(((x1416)*(x1426)*(x1438)))+(((IkReal(-1.00000000000000))*(x1414)*(x1422)*(x1438)))+(((IkReal(-1.00000000000000))*(x1416)*(x1433)))+(((IkReal(-1.00000000000000))*(x1410)*(x1411)))+(((IkReal(-1.00000000000000))*(x1410)*(x1412)))+(((IkReal(-2.00000000000000))*(x1415)*(x1425)))+(((IkReal(-2.00000000000000))*(x1415)*(x1421)))+(((x1408)*(x1418)*(x1432)))+(((IkReal(-1.00000000000000))*(x1417)*(x1429)*(x1439)))+(((IkReal(1.99999997110000))*(py)*(x1412)*(x1438)))+(((IkReal(-1.00000000000000))*(x1406)*(x1411)*(x1441)))+(((pp)*(x1430)))+(((x1408)*(x1412)*(x1441)))+(((IkReal(-1.00000000000000))*(x1444)*(x1448)))+(((IkReal(-1.00000000000000))*(x1414)*(x1416)*(x1445)))+(((IkReal(-1.00000000000000))*(x1407)*(x1422)*(x1440)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1417)))+(((IkReal(3.60399994792220e-5))*(py)*(x1416)))+(((x1412)*(x1427)))+(((IkReal(-1.00000000000000))*(x1430)*(x1456)))+(((x1407)*(x1411)*(x1441)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst3; -gconst3=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst4; -gconst4=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1458=((py)*(r02)); -IkReal x1459=((IkReal(1750.00000000000))*(cj1)); -IkReal x1460=((px)*(sj3)); -IkReal x1461=((cj3)*(pz)); -IkReal x1462=((cj0)*(sj1)); -IkReal x1463=((IkReal(1749.99997471250))*(r02)); -IkReal x1464=((px)*(r01)); -IkReal x1465=((r00)*(sj3)); -IkReal x1466=((pz)*(sj3)); -IkReal x1467=((pz)*(r01)); -IkReal x1468=((IkReal(0.297499995701125))*(sj3)); -IkReal x1469=((IkReal(1656.25000000000))*(cj1)); -IkReal x1470=((IkReal(0.297499995701125))*(cj3)); -IkReal x1471=((pz)*(r00)); -IkReal x1472=((cj3)*(px)); -IkReal x1473=((cj3)*(py)); -IkReal x1474=((px)*(r02)); -IkReal x1475=((IkReal(0.297499995701125))*(r01)); -IkReal x1476=((sj0)*(sj1)); -IkReal x1477=((IkReal(0.281562495931422))*(x1462)); -IkReal x1478=((IkReal(1656.24997606719))*(x1476)); -IkReal x1479=((IkReal(1749.99997471250))*(x1476)); -IkReal x1480=((py)*(x1476)); -IkReal x1481=((IkReal(0.281562495931422))*(x1476)); -if( IKabs(((gconst4)*(((((IkReal(-1.00000000000000))*(r00)*(x1459)*(x1473)))+(((IkReal(-1.00000000000000))*(x1468)*(x1480)))+(((cj3)*(x1459)*(x1464)))+(((x1462)*(x1463)*(x1472)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x1462)))+(((x1460)*(x1479)))+(((IkReal(-1749.99997471250))*(r00)*(x1461)*(x1462)))+(((cj3)*(x1458)*(x1479)))+(((IkReal(-1.00000000000000))*(x1458)*(x1462)*(x1470)))+(((x1459)*(x1466)))+(((IkReal(-185.500000000000))*(sj3)))+(((x1467)*(x1477)))+(((x1464)*(x1469)))+(((x1461)*(x1462)*(x1475)))+(((IkReal(-0.297499995701125))*(x1460)*(x1462)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1469)))+(((IkReal(-1.00000000000000))*(x1467)*(x1478)))+(((x1474)*(x1481)))+(((IkReal(-1.00000000000000))*(x1458)*(x1477)))+(((IkReal(-1656.24997606719))*(x1462)*(x1471)))+(((IkReal(-1.00000000000000))*(r01)*(x1461)*(x1479)))+(((IkReal(-1.00000000000000))*(x1471)*(x1481)))+(((x1470)*(x1474)*(x1476)))+(((IkReal(1656.24997606719))*(x1462)*(x1474)))+(((IkReal(-0.297499995701125))*(r00)*(x1461)*(x1476)))+(((x1458)*(x1478))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(175.562500000000))+(((px)*(x1462)*(x1470)))+(((IkReal(-1.00000000000000))*(pz)*(x1469)))+(((sj3)*(x1458)*(x1479)))+(((IkReal(-1.00000000000000))*(py)*(x1459)*(x1465)))+(((x1462)*(x1466)*(x1475)))+(((r01)*(x1459)*(x1460)))+(((IkReal(-1.00000000000000))*(px)*(x1478)))+(((x1470)*(x1480)))+(((IkReal(-1.00000000000000))*(x1459)*(x1461)))+(((IkReal(0.297499995701125))*(r02)*(x1460)*(x1476)))+(((IkReal(-1749.99997471250))*(pz)*(x1462)*(x1465)))+(((IkReal(0.281562495931422))*(x1480)))+(((IkReal(-1.00000000000000))*(r01)*(x1466)*(x1479)))+(((IkReal(-1.00000000000000))*(x1458)*(x1462)*(x1468)))+(((IkReal(1656.24997606719))*(py)*(x1462)))+(((IkReal(1749.99997471250))*(x1462)*(x1473)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-0.297499995701125))*(pz)*(x1465)*(x1476)))+(((x1460)*(x1462)*(x1463)))+(((px)*(x1477)))+(((IkReal(-1.00000000000000))*(x1472)*(x1479))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst4)*(((((IkReal(-1.00000000000000))*(r00)*(x1459)*(x1473)))+(((IkReal(-1.00000000000000))*(x1468)*(x1480)))+(((cj3)*(x1459)*(x1464)))+(((x1462)*(x1463)*(x1472)))+(((IkReal(-1749.99997471250))*(py)*(sj3)*(x1462)))+(((x1460)*(x1479)))+(((IkReal(-1749.99997471250))*(r00)*(x1461)*(x1462)))+(((cj3)*(x1458)*(x1479)))+(((IkReal(-1.00000000000000))*(x1458)*(x1462)*(x1470)))+(((x1459)*(x1466)))+(((IkReal(-185.500000000000))*(sj3)))+(((x1467)*(x1477)))+(((x1464)*(x1469)))+(((x1461)*(x1462)*(x1475)))+(((IkReal(-0.297499995701125))*(x1460)*(x1462)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1469)))+(((IkReal(-1.00000000000000))*(x1467)*(x1478)))+(((x1474)*(x1481)))+(((IkReal(-1.00000000000000))*(x1458)*(x1477)))+(((IkReal(-1656.24997606719))*(x1462)*(x1471)))+(((IkReal(-1.00000000000000))*(r01)*(x1461)*(x1479)))+(((IkReal(-1.00000000000000))*(x1471)*(x1481)))+(((x1470)*(x1474)*(x1476)))+(((IkReal(1656.24997606719))*(x1462)*(x1474)))+(((IkReal(-0.297499995701125))*(r00)*(x1461)*(x1476)))+(((x1458)*(x1478)))))), ((gconst4)*(((IkReal(175.562500000000))+(((px)*(x1462)*(x1470)))+(((IkReal(-1.00000000000000))*(pz)*(x1469)))+(((sj3)*(x1458)*(x1479)))+(((IkReal(-1.00000000000000))*(py)*(x1459)*(x1465)))+(((x1462)*(x1466)*(x1475)))+(((r01)*(x1459)*(x1460)))+(((IkReal(-1.00000000000000))*(px)*(x1478)))+(((x1470)*(x1480)))+(((IkReal(-1.00000000000000))*(x1459)*(x1461)))+(((IkReal(0.297499995701125))*(r02)*(x1460)*(x1476)))+(((IkReal(-1749.99997471250))*(pz)*(x1462)*(x1465)))+(((IkReal(0.281562495931422))*(x1480)))+(((IkReal(-1.00000000000000))*(r01)*(x1466)*(x1479)))+(((IkReal(-1.00000000000000))*(x1458)*(x1462)*(x1468)))+(((IkReal(1656.24997606719))*(py)*(x1462)))+(((IkReal(1749.99997471250))*(x1462)*(x1473)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-0.297499995701125))*(pz)*(x1465)*(x1476)))+(((x1460)*(x1462)*(x1463)))+(((px)*(x1477)))+(((IkReal(-1.00000000000000))*(x1472)*(x1479))))))); -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[4]; -IkReal x1482=IKsin(j2); -IkReal x1483=IKcos(j2); -IkReal x1484=((r00)*(sj0)); -IkReal x1485=((r01)*(sj0)); -IkReal x1486=((py)*(sj1)); -IkReal x1487=((IkReal(0.999999985550000))*(r02)); -IkReal x1488=((px)*(sj1)); -IkReal x1489=((IkReal(0.999999985550000))*(sj0)); -IkReal x1490=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1491=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1492=((cj1)*(px)); -IkReal x1493=((pz)*(sj1)); -IkReal x1494=((cj0)*(r00)); -IkReal x1495=((cj1)*(pz)); -IkReal x1496=((IkReal(1.00000000000000))*(r01)); -IkReal x1497=((cj1)*(py)); -IkReal x1498=((IkReal(0.999999985550000))*(cj0)); -IkReal x1499=((IkReal(0.106000000000000))*(x1482)); -IkReal x1500=((IkReal(0.106000000000000))*(x1483)); -IkReal x1501=((IkReal(0.112000000000000))*(x1483)); -IkReal x1502=((IkReal(0.112000000000000))*(x1482)); -IkReal x1503=((cj3)*(x1502)); -IkReal x1504=((sj3)*(x1501)); -IkReal x1505=((sj3)*(x1502)); -IkReal x1506=((cj3)*(x1501)); -IkReal x1507=((x1500)+(x1506)); -IkReal x1508=((x1503)+(x1504)+(x1499)); -evalcond[0]=((((x1491)*(x1497)))+(((x1497)*(x1498)))+(x1508)+(x1493)+(((x1490)*(x1492)))+(((IkReal(-1.00000000000000))*(x1489)*(x1492)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x1507)))+(((IkReal(-1.00000000000000))*(x1486)*(x1491)))+(((IkReal(-1.00000000000000))*(x1486)*(x1498)))+(x1505)+(x1495)+(((IkReal(-1.00000000000000))*(x1488)*(x1490)))+(((x1488)*(x1489)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x1508)))+(((r00)*(x1497)))+(((IkReal(0.999999985550000))*(x1493)*(x1494)))+(((IkReal(-1.00000000000000))*(x1492)*(x1496)))+(((IkReal(-1.00000000000000))*(r01)*(x1490)*(x1493)))+(((IkReal(0.000169999997543500))*(x1484)*(x1493)))+(((r02)*(x1486)*(x1490)))+(((IkReal(0.999999985550000))*(x1485)*(x1493)))+(((IkReal(-1.00000000000000))*(r02)*(x1488)*(x1491)))+(((IkReal(-1.00000000000000))*(cj0)*(x1487)*(x1488)))+(((IkReal(-1.00000000000000))*(sj0)*(x1486)*(x1487)))); -evalcond[3]=((((cj0)*(x1487)*(x1492)))+(((IkReal(-1.00000000000000))*(x1507)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((r00)*(x1486)))+(((IkReal(-0.999999985550000))*(x1494)*(x1495)))+(((IkReal(1.80199997396110e-5))*(x1484)))+(((IkReal(0.105999998468300))*(x1494)))+(((IkReal(0.105999998468300))*(x1485)))+(x1505)+(((r01)*(x1490)*(x1495)))+(((IkReal(-0.999999985550000))*(x1485)*(x1495)))+(((IkReal(-1.00000000000000))*(r02)*(x1490)*(x1497)))+(((sj0)*(x1487)*(x1497)))+(((r02)*(x1491)*(x1492)))+(((IkReal(-1.00000000000000))*(x1488)*(x1496)))+(((IkReal(-0.000169999997543500))*(x1484)*(x1495)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1509=((px)*(sj1)); -IkReal x1510=((IkReal(0.281562495931422))*(cj0)); -IkReal x1511=((cj1)*(sj0)); -IkReal x1512=((IkReal(0.281562495931422))*(py)); -IkReal x1513=((IkReal(1749.99997471250))*(sj3)); -IkReal x1514=((IkReal(1749.99997471250))*(cj3)); -IkReal x1515=((cj0)*(cj1)); -IkReal x1516=((IkReal(0.297499995701125))*(sj3)); -IkReal x1517=((IkReal(0.297499995701125))*(cj3)); -IkReal x1518=((IkReal(1656.25000000000))*(pz)); -IkReal x1519=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x1520=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x1521=((IkReal(1656.24997606719))*(cj0)*(py)); -IkReal x1522=((py)*(sj0)*(sj1)); -IkReal x1523=((cj0)*(py)*(sj1)); -if( IKabs(((gconst3)*(((((py)*(x1511)*(x1517)))+(((cj1)*(x1520)))+(((IkReal(-185.500000000000))*(sj3)))+(((px)*(x1515)*(x1517)))+(((IkReal(-1656.24997606719))*(px)*(x1511)))+(((IkReal(-1.00000000000000))*(x1513)*(x1523)))+(((sj1)*(x1518)))+(((sj1)*(x1519)))+(((cj1)*(px)*(x1510)))+(((IkReal(1656.24997606719))*(py)*(x1515)))+(((x1511)*(x1512)))+(((IkReal(-1.00000000000000))*(x1516)*(x1522)))+(((py)*(x1514)*(x1515)))+(((sj0)*(x1509)*(x1513)))+(((IkReal(-1.00000000000000))*(px)*(x1511)*(x1514)))+(((IkReal(-1.00000000000000))*(cj0)*(x1509)*(x1516))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(175.562500000000))+(((x1509)*(x1510)))+(((IkReal(-1656.24997606719))*(sj0)*(x1509)))+(((py)*(x1511)*(x1516)))+(((IkReal(-1.00000000000000))*(cj1)*(x1518)))+(((IkReal(-1.00000000000000))*(cj1)*(x1519)))+(((sj0)*(sj1)*(x1512)))+(((IkReal(-1.00000000000000))*(sj0)*(x1509)*(x1514)))+(((sj1)*(x1521)))+(((sj1)*(x1520)))+(((px)*(x1515)*(x1516)))+(((x1517)*(x1522)))+(((IkReal(185.500000000000))*(cj3)))+(((x1514)*(x1523)))+(((py)*(x1513)*(x1515)))+(((IkReal(-1.00000000000000))*(px)*(x1511)*(x1513)))+(((cj0)*(x1509)*(x1517))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst3)*(((((py)*(x1511)*(x1517)))+(((cj1)*(x1520)))+(((IkReal(-185.500000000000))*(sj3)))+(((px)*(x1515)*(x1517)))+(((IkReal(-1656.24997606719))*(px)*(x1511)))+(((IkReal(-1.00000000000000))*(x1513)*(x1523)))+(((sj1)*(x1518)))+(((sj1)*(x1519)))+(((cj1)*(px)*(x1510)))+(((IkReal(1656.24997606719))*(py)*(x1515)))+(((x1511)*(x1512)))+(((IkReal(-1.00000000000000))*(x1516)*(x1522)))+(((py)*(x1514)*(x1515)))+(((sj0)*(x1509)*(x1513)))+(((IkReal(-1.00000000000000))*(px)*(x1511)*(x1514)))+(((IkReal(-1.00000000000000))*(cj0)*(x1509)*(x1516)))))), ((gconst3)*(((IkReal(175.562500000000))+(((x1509)*(x1510)))+(((IkReal(-1656.24997606719))*(sj0)*(x1509)))+(((py)*(x1511)*(x1516)))+(((IkReal(-1.00000000000000))*(cj1)*(x1518)))+(((IkReal(-1.00000000000000))*(cj1)*(x1519)))+(((sj0)*(sj1)*(x1512)))+(((IkReal(-1.00000000000000))*(sj0)*(x1509)*(x1514)))+(((sj1)*(x1521)))+(((sj1)*(x1520)))+(((px)*(x1515)*(x1516)))+(((x1517)*(x1522)))+(((IkReal(185.500000000000))*(cj3)))+(((x1514)*(x1523)))+(((py)*(x1513)*(x1515)))+(((IkReal(-1.00000000000000))*(px)*(x1511)*(x1513)))+(((cj0)*(x1509)*(x1517))))))); -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[4]; -IkReal x1524=IKsin(j2); -IkReal x1525=IKcos(j2); -IkReal x1526=((r00)*(sj0)); -IkReal x1527=((r01)*(sj0)); -IkReal x1528=((py)*(sj1)); -IkReal x1529=((IkReal(0.999999985550000))*(r02)); -IkReal x1530=((px)*(sj1)); -IkReal x1531=((IkReal(0.999999985550000))*(sj0)); -IkReal x1532=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1533=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1534=((cj1)*(px)); -IkReal x1535=((pz)*(sj1)); -IkReal x1536=((cj0)*(r00)); -IkReal x1537=((cj1)*(pz)); -IkReal x1538=((IkReal(1.00000000000000))*(r01)); -IkReal x1539=((cj1)*(py)); -IkReal x1540=((IkReal(0.999999985550000))*(cj0)); -IkReal x1541=((IkReal(0.106000000000000))*(x1524)); -IkReal x1542=((IkReal(0.106000000000000))*(x1525)); -IkReal x1543=((IkReal(0.112000000000000))*(x1525)); -IkReal x1544=((IkReal(0.112000000000000))*(x1524)); -IkReal x1545=((cj3)*(x1544)); -IkReal x1546=((sj3)*(x1543)); -IkReal x1547=((sj3)*(x1544)); -IkReal x1548=((cj3)*(x1543)); -IkReal x1549=((x1542)+(x1548)); -IkReal x1550=((x1546)+(x1545)+(x1541)); -evalcond[0]=((((x1539)*(x1540)))+(((IkReal(-1.00000000000000))*(x1531)*(x1534)))+(((x1532)*(x1534)))+(x1550)+(x1535)+(((x1533)*(x1539)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x1530)*(x1532)))+(((x1530)*(x1531)))+(x1547)+(x1537)+(((IkReal(-1.00000000000000))*(x1528)*(x1533)))+(((IkReal(-1.00000000000000))*(x1549)))+(((IkReal(-1.00000000000000))*(x1528)*(x1540)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x1532)*(x1535)))+(((IkReal(-1.00000000000000))*(cj0)*(x1529)*(x1530)))+(((IkReal(0.999999985550000))*(x1535)*(x1536)))+(((r00)*(x1539)))+(((IkReal(-1.00000000000000))*(r02)*(x1530)*(x1533)))+(((IkReal(-1.00000000000000))*(sj0)*(x1528)*(x1529)))+(((r02)*(x1528)*(x1532)))+(((IkReal(-1.00000000000000))*(x1534)*(x1538)))+(((IkReal(-1.00000000000000))*(x1550)))+(((IkReal(0.999999985550000))*(x1527)*(x1535)))+(((IkReal(0.000169999997543500))*(x1526)*(x1535)))); -evalcond[3]=((((IkReal(0.105999998468300))*(x1536)))+(((IkReal(-0.000169999997543500))*(x1526)*(x1537)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-0.999999985550000))*(x1527)*(x1537)))+(((IkReal(-1.00000000000000))*(x1530)*(x1538)))+(((r00)*(x1528)))+(((cj0)*(x1529)*(x1534)))+(((r02)*(x1533)*(x1534)))+(x1547)+(((IkReal(-1.00000000000000))*(r02)*(x1532)*(x1539)))+(((IkReal(-0.999999985550000))*(x1536)*(x1537)))+(((IkReal(1.80199997396110e-5))*(x1526)))+(((IkReal(-1.00000000000000))*(x1549)))+(((r01)*(x1532)*(x1537)))+(((IkReal(0.105999998468300))*(x1527)))+(((sj0)*(x1529)*(x1539)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -IkReal x1551=(px)*(px); -IkReal x1552=((IkReal(0.0237440000000000))*(cj3)); -IkReal x1553=(py)*(py); -IkReal x1554=(pz)*(pz); -IkReal x1555=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1556=((cj0)*(r00)); -IkReal x1557=((r01)*(sj0)); -IkReal x1558=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1559=((IkReal(0.000339999995087000))*(py)); -IkReal x1560=((pz)*(sj1)); -IkReal x1561=((r02)*(sj0)); -IkReal x1562=((cj0)*(px)); -IkReal x1563=((cj1)*(r01)); -IkReal x1564=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1565=((IkReal(3.60399994792220e-5))*(sj1)); -IkReal x1566=((py)*(r01)); -IkReal x1567=((r00)*(sj0)); -IkReal x1568=((IkReal(0.000169999997543500))*(pz)); -IkReal x1569=((IkReal(0.105999998468300))*(cj0)); -IkReal x1570=((px)*(r00)); -IkReal x1571=((IkReal(1.99999997110000))*(pz)); -IkReal x1572=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1573=((r01)*(sj1)); -IkReal x1574=((cj1)*(r02)); -IkReal x1575=((r02)*(sj1)); -IkReal x1576=((IkReal(0.999999985550000))*(py)); -IkReal x1577=((IkReal(0.999999985550000))*(cj0)); -IkReal x1578=((IkReal(0.211999996936600))*(px)); -IkReal x1579=((IkReal(0.0112359998376398))*(cj0)); -IkReal x1580=((cj0)*(r02)); -IkReal x1581=((IkReal(1.80199997396110e-5))*(sj1)); -IkReal x1582=((IkReal(0.211999996936600))*(py)); -IkReal x1583=((cj1)*(px)); -IkReal x1584=((IkReal(0.000339999995087000))*(pz)); -IkReal x1585=((IkReal(0.999999985550000))*(cj1)); -IkReal x1586=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1587=((cj0)*(py)); -IkReal x1588=((pz)*(r02)); -IkReal x1589=((IkReal(0.211999996936600))*(pz)); -IkReal x1590=((cj1)*(pz)); -IkReal x1591=((IkReal(0.999999985550000))*(sj1)); -IkReal x1592=((IkReal(1.80199997396110e-5))*(cj1)); -IkReal x1593=((cj0)*(r01)); -IkReal x1594=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1595=((IkReal(0.999999985550000))*(x1554)); -IkReal x1596=((IkReal(0.000169999997543500))*(x1553)); -IkReal x1597=((IkReal(0.999999985550000))*(x1551)); -IkReal x1598=((IkReal(0.000169999997543500))*(x1554)); -IkReal x1599=((IkReal(1.99999997110000))*(px)*(py)); -IkReal x1600=((IkReal(0.000169999997543500))*(x1551)); -IkReal x1601=((IkReal(2.00000000000000))*(x1554)); -IkReal x1602=((IkReal(0.999999985550000))*(x1553)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(sj0)*(x1576)))+(((IkReal(-0.000169999997543500))*(px)*(sj0)))+(((py)*(x1564)))+(((IkReal(-0.999999985550000))*(x1562)))); -evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-0.000169999997543500))*(x1567)))+(((IkReal(-0.999999985550000))*(x1556)))+(((IkReal(-0.999999985550000))*(x1557)))+(((r01)*(x1564)))); -evalcond[3]=((IkReal(0.0125440000000000))+(((IkReal(0.212000000000000))*(x1590)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x1565)))+(((IkReal(-1.00000000000000))*(x1562)*(x1565)))+(((IkReal(-1.00000000000000))*(pp)))+(x1552)+(((sj0)*(sj1)*(x1578)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x1582)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x1569)*(x1573)))+(((IkReal(-1.00000000000000))*(x1557)*(x1581)))+(((IkReal(0.105999998468300))*(sj1)*(x1567)))+(((IkReal(-1.00000000000000))*(x1556)*(x1581)))+(((IkReal(-1.00000000000000))*(x1588)))+(((IkReal(0.106000000000000))*(x1574)))+(((IkReal(-1.00000000000000))*(x1566)))+(((IkReal(-1.00000000000000))*(x1570)))); -evalcond[5]=((((x1556)*(x1586)))+(x1575)+(((IkReal(-1.00000000000000))*(x1567)*(x1585)))+(((x1557)*(x1586)))+(((x1563)*(x1577)))); -evalcond[6]=((((x1567)*(x1591)))+(((IkReal(-1.00000000000000))*(x1573)*(x1577)))+(x1574)+(((IkReal(-1.00000000000000))*(x1557)*(x1558)))+(((IkReal(-1.00000000000000))*(x1556)*(x1558)))); -evalcond[7]=((((IkReal(0.000169999997543500))*(r02)*(x1562)))+(((IkReal(-0.999999985550000))*(px)*(x1561)))+(((IkReal(0.000169999997543500))*(py)*(x1561)))+(((IkReal(0.999999985550000))*(pz)*(x1567)))+(((x1556)*(x1592)))+(((x1576)*(x1580)))+(((IkReal(-0.105999998468300))*(cj1)*(x1567)))+(((IkReal(-1.00000000000000))*(x1557)*(x1568)))+(((IkReal(-1.00000000000000))*(x1556)*(x1568)))+(((IkReal(-1.00000000000000))*(pz)*(r01)*(x1577)))+(((x1557)*(x1592)))+(((x1563)*(x1569)))+(((IkReal(0.106000000000000))*(x1575)))); -evalcond[8]=((IkReal(0.0237800000000000))+(((x1567)*(x1600)))+(((cj1)*(x1557)*(x1589)))+(((py)*(x1561)*(x1571)))+(((cj1)*(x1555)*(x1567)))+(((IkReal(-1.91011997239877e-6))*(x1567)))+(((IkReal(1.99999997110000))*(x1562)*(x1566)))+(((x1567)*(x1599)))+(((x1556)*(x1597)))+(((IkReal(3.60399994792220e-5))*(x1574)*(x1587)))+(((x1557)*(x1602)))+(((IkReal(-1.00000000000000))*(px)*(x1556)*(x1559)))+(((IkReal(-1.00000000000000))*(cj0)*(x1555)*(x1563)))+(((px)*(x1561)*(x1584)))+(((r01)*(x1551)*(x1564)))+(((IkReal(-1.00000000000000))*(x1567)*(x1598)))+(((IkReal(-1.00000000000000))*(x1567)*(x1596)))+(((IkReal(-0.0112359998376398))*(x1557)))+(((IkReal(-0.0112359998376398))*(x1556)))+(((px)*(x1557)*(x1559)))+(x1552)+(((IkReal(-1.00000000000000))*(r01)*(x1553)*(x1564)))+(((IkReal(-1.00000000000000))*(pz)*(x1559)*(x1580)))+(((r02)*(x1562)*(x1571)))+(((IkReal(-0.211999996936600))*(x1562)*(x1574)))+(((IkReal(-1.00000000000000))*(cj1)*(x1561)*(x1582)))+(((r01)*(x1554)*(x1564)))+(((IkReal(-0.212000000000000))*(py)*(r00)*(sj1)))+(((IkReal(-3.60399994792220e-5))*(x1561)*(x1583)))+(((IkReal(-1.00000000000000))*(x1557)*(x1595)))+(((IkReal(-1.00000000000000))*(x1557)*(x1597)))+(((IkReal(-1.00000000000000))*(x1556)*(x1602)))+(((IkReal(-1.00000000000000))*(x1556)*(x1595)))+(((cj1)*(x1556)*(x1589)))+(((IkReal(0.212000000000000))*(px)*(x1573)))+(((IkReal(1.91011997239877e-6))*(x1593)))); -evalcond[9]=((((IkReal(0.212000000000000))*(x1566)))+(((x1559)*(x1562)*(x1573)))+(((IkReal(-1.00000000000000))*(x1551)*(x1567)*(x1591)))+(((IkReal(-1.00000000000000))*(x1553)*(x1556)*(x1558)))+(((IkReal(-2.00000000000000))*(x1570)*(x1590)))+(((IkReal(0.212000000000000))*(x1588)))+(((pp)*(x1574)))+(((x1556)*(x1594)))+(((IkReal(-0.0112360000000000))*(x1574)))+(((x1559)*(x1560)*(x1561)))+(((IkReal(-1.00000000000000))*(x1551)*(x1557)*(x1558)))+(((x1573)*(x1579)))+(((IkReal(-1.00000000000000))*(x1574)*(x1601)))+(((x1553)*(x1557)*(x1558)))+(((sj1)*(x1556)*(x1599)))+(((x1553)*(x1567)*(x1591)))+(((IkReal(0.000339999995087000))*(r02)*(x1560)*(x1562)))+(((IkReal(1.99999997110000))*(py)*(x1560)*(x1580)))+(((x1554)*(x1567)*(x1591)))+(((IkReal(-1.00000000000000))*(x1554)*(x1573)*(x1577)))+(((IkReal(-1.00000000000000))*(x1554)*(x1556)*(x1558)))+(((IkReal(-1.00000000000000))*(sj1)*(x1557)*(x1599)))+(((px)*(sj1)*(x1559)*(x1567)))+(((x1557)*(x1594)))+(((x1551)*(x1556)*(x1558)))+(((IkReal(-1.00000000000000))*(x1554)*(x1557)*(x1558)))+(((IkReal(-2.00000000000000))*(py)*(pz)*(x1563)))+(((x1553)*(x1573)*(x1577)))+(((IkReal(-0.0112359998376398))*(sj1)*(x1567)))+(((IkReal(-1.99999997110000))*(px)*(x1560)*(x1561)))+(((IkReal(0.212000000000000))*(x1570)))+(((IkReal(-1.00000000000000))*(x1551)*(x1573)*(x1577)))); -evalcond[10]=((((IkReal(-1.00000000000000))*(x1559)*(x1562)*(x1563)))+(((x1554)*(x1557)*(x1586)))+(((IkReal(-1.00000000000000))*(x1575)*(x1601)))+(((x1553)*(x1556)*(x1586)))+(((x1551)*(x1563)*(x1577)))+(((x1567)*(x1589)))+(((x1554)*(x1563)*(x1577)))+(((x1556)*(x1572)))+(((pp)*(x1575)))+(((IkReal(-1.00000000000000))*(x1559)*(x1561)*(x1590)))+(((IkReal(-1.00000000000000))*(x1562)*(x1574)*(x1584)))+(((IkReal(-2.00000000000000))*(x1560)*(x1566)))+(((IkReal(-2.00000000000000))*(x1560)*(x1570)))+(((IkReal(-1.00000000000000))*(x1555)*(x1557)))+(((IkReal(-1.00000000000000))*(x1555)*(x1556)))+(((IkReal(0.0112360000000000))*(x1575)))+(((x1561)*(x1571)*(x1583)))+(((IkReal(-1.00000000000000))*(x1553)*(x1567)*(x1585)))+(((IkReal(-1.99999997110000))*(py)*(x1556)*(x1583)))+(((x1557)*(x1572)))+(((IkReal(-1.00000000000000))*(x1589)*(x1593)))+(((IkReal(-1.00000000000000))*(x1559)*(x1567)*(x1583)))+(((x1580)*(x1582)))+(((x1554)*(x1556)*(x1586)))+(((IkReal(-1.00000000000000))*(x1553)*(x1563)*(x1577)))+(((x1551)*(x1567)*(x1585)))+(((IkReal(-1.00000000000000))*(x1554)*(x1567)*(x1585)))+(((x1551)*(x1557)*(x1586)))+(((IkReal(-1.00000000000000))*(x1551)*(x1556)*(x1586)))+(((x1563)*(x1579)))+(((IkReal(-1.00000000000000))*(x1571)*(x1574)*(x1587)))+(((IkReal(1.99999997110000))*(py)*(x1557)*(x1583)))+(((IkReal(-0.0112359998376398))*(cj1)*(x1567)))+(((IkReal(-1.00000000000000))*(x1553)*(x1557)*(x1586)))+(((IkReal(3.60399994792220e-5))*(py)*(x1561)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1562)))+(((IkReal(-1.00000000000000))*(x1561)*(x1578)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst6; -gconst6=IKsign(((IkReal(-175.562500000000))+(((IkReal(-196.000000000000))*((cj3)*(cj3))))+(((IkReal(-371.000000000000))*(cj3)))+(((IkReal(-196.000000000000))*((sj3)*(sj3)))))); -dummyeval[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.11641153435386))*((cj3)*(cj3))))+(((IkReal(-1.11641153435386))*((sj3)*(sj3))))+(((IkReal(-2.11320754716981))*(cj3)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1603=((cj0)*(sj1)); -IkReal x1604=((IkReal(1656.24997606719))*(pz)); -IkReal x1605=((py)*(r02)); -IkReal x1606=((IkReal(1750.00000000000))*(cj1)); -IkReal x1607=((px)*(sj3)); -IkReal x1608=((cj3)*(pz)); -IkReal x1609=((sj0)*(sj1)); -IkReal x1610=((IkReal(1749.99997471250))*(r02)); -IkReal x1611=((px)*(r01)); -IkReal x1612=((py)*(sj3)); -IkReal x1613=((pz)*(sj3)); -IkReal x1614=((IkReal(0.281562495931422))*(px)); -IkReal x1615=((IkReal(0.297499995701125))*(cj3)); -IkReal x1616=((IkReal(1749.99997471250))*(r01)); -IkReal x1617=((IkReal(1749.99997471250))*(r00)); -IkReal x1618=((IkReal(1656.25000000000))*(cj1)); -IkReal x1619=((IkReal(0.281562495931422))*(pz)); -IkReal x1620=((IkReal(1749.99997471250))*(cj3)); -IkReal x1621=((IkReal(1656.24997606719))*(px)); -IkReal x1622=((IkReal(0.297499995701125))*(r01)); -IkReal x1623=((py)*(r00)); -IkReal x1624=((IkReal(0.297499995701125))*(r00)); -if( IKabs(((gconst6)*(((((x1608)*(x1609)*(x1616)))+(((IkReal(-1.00000000000000))*(r02)*(x1609)*(x1614)))+(((IkReal(0.281562495931422))*(x1603)*(x1605)))+(((IkReal(-0.297499995701125))*(x1603)*(x1607)))+(((IkReal(-1.00000000000000))*(x1611)*(x1618)))+(((IkReal(-0.297499995701125))*(x1609)*(x1612)))+(((r01)*(x1604)*(x1609)))+(((IkReal(-1.00000000000000))*(x1605)*(x1609)*(x1620)))+(((x1603)*(x1605)*(x1615)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x1609)*(x1615)))+(((r00)*(x1609)*(x1619)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r02)*(x1603)*(x1621)))+(((cj3)*(x1606)*(x1623)))+(((IkReal(-1.00000000000000))*(cj3)*(x1606)*(x1611)))+(((IkReal(-1.00000000000000))*(r01)*(x1603)*(x1619)))+(((r00)*(x1603)*(x1604)))+(((IkReal(-1.00000000000000))*(cj3)*(px)*(x1603)*(x1610)))+(((IkReal(-1749.99997471250))*(x1603)*(x1612)))+(((x1618)*(x1623)))+(((IkReal(-1656.24997606719))*(x1605)*(x1609)))+(((IkReal(-1.00000000000000))*(x1603)*(x1608)*(x1622)))+(((IkReal(1749.99997471250))*(x1607)*(x1609)))+(((x1606)*(x1613)))+(((x1603)*(x1608)*(x1617)))+(((x1608)*(x1609)*(x1624))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(175.562500000000))+(((IkReal(-0.297499995701125))*(r02)*(x1607)*(x1609)))+(((x1609)*(x1613)*(x1616)))+(((IkReal(-1.00000000000000))*(x1603)*(x1613)*(x1622)))+(((x1603)*(x1613)*(x1617)))+(((IkReal(-1.00000000000000))*(x1603)*(x1607)*(x1610)))+(((IkReal(-1749.99997471250))*(sj3)*(x1605)*(x1609)))+(((py)*(x1603)*(x1620)))+(((IkReal(-1.00000000000000))*(px)*(x1609)*(x1620)))+(((IkReal(-1.00000000000000))*(pz)*(x1618)))+(((py)*(x1609)*(x1615)))+(((IkReal(-1.00000000000000))*(x1609)*(x1621)))+(((IkReal(0.281562495931422))*(py)*(x1609)))+(((IkReal(0.297499995701125))*(sj3)*(x1603)*(x1605)))+(((r00)*(x1606)*(x1612)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x1606)*(x1608)))+(((IkReal(1656.24997606719))*(py)*(x1603)))+(((px)*(x1603)*(x1615)))+(((x1603)*(x1614)))+(((IkReal(-1.00000000000000))*(r01)*(x1606)*(x1607)))+(((x1609)*(x1613)*(x1624))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst6)*(((((x1608)*(x1609)*(x1616)))+(((IkReal(-1.00000000000000))*(r02)*(x1609)*(x1614)))+(((IkReal(0.281562495931422))*(x1603)*(x1605)))+(((IkReal(-0.297499995701125))*(x1603)*(x1607)))+(((IkReal(-1.00000000000000))*(x1611)*(x1618)))+(((IkReal(-0.297499995701125))*(x1609)*(x1612)))+(((r01)*(x1604)*(x1609)))+(((IkReal(-1.00000000000000))*(x1605)*(x1609)*(x1620)))+(((x1603)*(x1605)*(x1615)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x1609)*(x1615)))+(((r00)*(x1609)*(x1619)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r02)*(x1603)*(x1621)))+(((cj3)*(x1606)*(x1623)))+(((IkReal(-1.00000000000000))*(cj3)*(x1606)*(x1611)))+(((IkReal(-1.00000000000000))*(r01)*(x1603)*(x1619)))+(((r00)*(x1603)*(x1604)))+(((IkReal(-1.00000000000000))*(cj3)*(px)*(x1603)*(x1610)))+(((IkReal(-1749.99997471250))*(x1603)*(x1612)))+(((x1618)*(x1623)))+(((IkReal(-1656.24997606719))*(x1605)*(x1609)))+(((IkReal(-1.00000000000000))*(x1603)*(x1608)*(x1622)))+(((IkReal(1749.99997471250))*(x1607)*(x1609)))+(((x1606)*(x1613)))+(((x1603)*(x1608)*(x1617)))+(((x1608)*(x1609)*(x1624)))))), ((gconst6)*(((IkReal(175.562500000000))+(((IkReal(-0.297499995701125))*(r02)*(x1607)*(x1609)))+(((x1609)*(x1613)*(x1616)))+(((IkReal(-1.00000000000000))*(x1603)*(x1613)*(x1622)))+(((x1603)*(x1613)*(x1617)))+(((IkReal(-1.00000000000000))*(x1603)*(x1607)*(x1610)))+(((IkReal(-1749.99997471250))*(sj3)*(x1605)*(x1609)))+(((py)*(x1603)*(x1620)))+(((IkReal(-1.00000000000000))*(px)*(x1609)*(x1620)))+(((IkReal(-1.00000000000000))*(pz)*(x1618)))+(((py)*(x1609)*(x1615)))+(((IkReal(-1.00000000000000))*(x1609)*(x1621)))+(((IkReal(0.281562495931422))*(py)*(x1609)))+(((IkReal(0.297499995701125))*(sj3)*(x1603)*(x1605)))+(((r00)*(x1606)*(x1612)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x1606)*(x1608)))+(((IkReal(1656.24997606719))*(py)*(x1603)))+(((px)*(x1603)*(x1615)))+(((x1603)*(x1614)))+(((IkReal(-1.00000000000000))*(r01)*(x1606)*(x1607)))+(((x1609)*(x1613)*(x1624))))))); -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[4]; -IkReal x1625=IKsin(j2); -IkReal x1626=IKcos(j2); -IkReal x1627=((r00)*(sj0)); -IkReal x1628=((r01)*(sj0)); -IkReal x1629=((py)*(sj1)); -IkReal x1630=((IkReal(0.999999985550000))*(r02)); -IkReal x1631=((px)*(sj1)); -IkReal x1632=((IkReal(0.999999985550000))*(sj0)); -IkReal x1633=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1634=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1635=((cj1)*(px)); -IkReal x1636=((pz)*(sj1)); -IkReal x1637=((cj0)*(r00)); -IkReal x1638=((cj1)*(pz)); -IkReal x1639=((IkReal(1.00000000000000))*(r01)); -IkReal x1640=((cj1)*(py)); -IkReal x1641=((IkReal(0.999999985550000))*(cj0)); -IkReal x1642=((IkReal(0.106000000000000))*(x1625)); -IkReal x1643=((IkReal(0.106000000000000))*(x1626)); -IkReal x1644=((IkReal(0.112000000000000))*(x1626)); -IkReal x1645=((IkReal(0.112000000000000))*(x1625)); -IkReal x1646=((cj3)*(x1645)); -IkReal x1647=((sj3)*(x1644)); -IkReal x1648=((cj3)*(x1644)); -IkReal x1649=((sj3)*(x1645)); -IkReal x1650=((x1643)+(x1648)); -IkReal x1651=((x1647)+(x1646)+(x1642)); -evalcond[0]=((((x1640)*(x1641)))+(x1651)+(x1636)+(((IkReal(-1.00000000000000))*(x1632)*(x1635)))+(((x1633)*(x1635)))+(((x1634)*(x1640)))); -evalcond[1]=((IkReal(-0.106000000000000))+(x1649)+(x1638)+(((IkReal(-1.00000000000000))*(x1650)))+(((IkReal(-1.00000000000000))*(x1629)*(x1641)))+(((IkReal(-1.00000000000000))*(x1629)*(x1634)))+(((IkReal(-1.00000000000000))*(x1631)*(x1633)))+(((x1631)*(x1632)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x1635)*(x1639)))+(((IkReal(-1.00000000000000))*(cj0)*(x1630)*(x1631)))+(x1651)+(((IkReal(0.000169999997543500))*(x1627)*(x1636)))+(((IkReal(-1.00000000000000))*(r02)*(x1631)*(x1634)))+(((IkReal(0.999999985550000))*(x1628)*(x1636)))+(((r00)*(x1640)))+(((r02)*(x1629)*(x1633)))+(((IkReal(-1.00000000000000))*(r01)*(x1633)*(x1636)))+(((IkReal(-1.00000000000000))*(sj0)*(x1629)*(x1630)))+(((IkReal(0.999999985550000))*(x1636)*(x1637)))); -evalcond[3]=((((r00)*(x1629)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((r02)*(x1634)*(x1635)))+(x1650)+(((IkReal(0.105999998468300))*(x1628)))+(((IkReal(-1.00000000000000))*(r02)*(x1633)*(x1640)))+(((IkReal(-0.000169999997543500))*(x1627)*(x1638)))+(((IkReal(-1.00000000000000))*(x1649)))+(((IkReal(-0.999999985550000))*(x1637)*(x1638)))+(((IkReal(0.105999998468300))*(x1637)))+(((r01)*(x1633)*(x1638)))+(((IkReal(-0.999999985550000))*(x1628)*(x1638)))+(((sj0)*(x1630)*(x1640)))+(((IkReal(-1.00000000000000))*(x1631)*(x1639)))+(((cj0)*(x1630)*(x1635)))+(((IkReal(1.80199997396110e-5))*(x1627)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1652=((cj0)*(sj1)); -IkReal x1653=((IkReal(0.281562495931422))*(px)); -IkReal x1654=((cj1)*(sj0)); -IkReal x1655=((IkReal(0.281562495931422))*(py)); -IkReal x1656=((IkReal(1656.24997606719))*(px)); -IkReal x1657=((IkReal(1656.24997606719))*(py)); -IkReal x1658=((cj0)*(cj1)); -IkReal x1659=((IkReal(1749.99997471250))*(px)); -IkReal x1660=((IkReal(0.297499995701125))*(px)); -IkReal x1661=((IkReal(1749.99997471250))*(py)); -IkReal x1662=((IkReal(0.297499995701125))*(py)); -IkReal x1663=((sj0)*(sj1)); -IkReal x1664=((IkReal(1656.25000000000))*(pz)); -IkReal x1665=((IkReal(1750.00000000000))*(cj3)*(pz)); -IkReal x1666=((IkReal(1750.00000000000))*(pz)*(sj3)); -IkReal x1667=((sj3)*(x1663)); -IkReal x1668=((cj3)*(x1663)); -if( IKabs(((gconst5)*(((((sj1)*(x1664)))+(((sj1)*(x1665)))+(((x1654)*(x1655)))+(((IkReal(-1.00000000000000))*(x1662)*(x1667)))+(((cj1)*(x1666)))+(((IkReal(-1.00000000000000))*(x1654)*(x1656)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x1652)*(x1661)))+(((IkReal(-1.00000000000000))*(sj3)*(x1652)*(x1660)))+(((cj3)*(x1658)*(x1660)))+(((cj3)*(x1658)*(x1661)))+(((x1659)*(x1667)))+(((x1657)*(x1658)))+(((cj3)*(x1654)*(x1662)))+(((x1653)*(x1658)))+(((IkReal(-1.00000000000000))*(cj3)*(x1654)*(x1659))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(175.562500000000))+(((sj1)*(x1666)))+(((IkReal(-1.00000000000000))*(x1656)*(x1663)))+(((x1655)*(x1663)))+(((IkReal(-1.00000000000000))*(cj1)*(x1665)))+(((IkReal(-1.00000000000000))*(cj1)*(x1664)))+(((IkReal(-1.00000000000000))*(sj3)*(x1654)*(x1659)))+(((x1662)*(x1668)))+(((cj3)*(x1652)*(x1661)))+(((cj3)*(x1652)*(x1660)))+(((sj3)*(x1654)*(x1662)))+(((IkReal(185.500000000000))*(cj3)))+(((sj3)*(x1658)*(x1660)))+(((sj3)*(x1658)*(x1661)))+(((x1652)*(x1653)))+(((x1652)*(x1657)))+(((IkReal(-1.00000000000000))*(x1659)*(x1668))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst5)*(((((sj1)*(x1664)))+(((sj1)*(x1665)))+(((x1654)*(x1655)))+(((IkReal(-1.00000000000000))*(x1662)*(x1667)))+(((cj1)*(x1666)))+(((IkReal(-1.00000000000000))*(x1654)*(x1656)))+(((IkReal(-185.500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x1652)*(x1661)))+(((IkReal(-1.00000000000000))*(sj3)*(x1652)*(x1660)))+(((cj3)*(x1658)*(x1660)))+(((cj3)*(x1658)*(x1661)))+(((x1659)*(x1667)))+(((x1657)*(x1658)))+(((cj3)*(x1654)*(x1662)))+(((x1653)*(x1658)))+(((IkReal(-1.00000000000000))*(cj3)*(x1654)*(x1659)))))), ((gconst5)*(((IkReal(175.562500000000))+(((sj1)*(x1666)))+(((IkReal(-1.00000000000000))*(x1656)*(x1663)))+(((x1655)*(x1663)))+(((IkReal(-1.00000000000000))*(cj1)*(x1665)))+(((IkReal(-1.00000000000000))*(cj1)*(x1664)))+(((IkReal(-1.00000000000000))*(sj3)*(x1654)*(x1659)))+(((x1662)*(x1668)))+(((cj3)*(x1652)*(x1661)))+(((cj3)*(x1652)*(x1660)))+(((sj3)*(x1654)*(x1662)))+(((IkReal(185.500000000000))*(cj3)))+(((sj3)*(x1658)*(x1660)))+(((sj3)*(x1658)*(x1661)))+(((x1652)*(x1653)))+(((x1652)*(x1657)))+(((IkReal(-1.00000000000000))*(x1659)*(x1668))))))); -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[4]; -IkReal x1669=IKsin(j2); -IkReal x1670=IKcos(j2); -IkReal x1671=((r00)*(sj0)); -IkReal x1672=((r01)*(sj0)); -IkReal x1673=((py)*(sj1)); -IkReal x1674=((IkReal(0.999999985550000))*(r02)); -IkReal x1675=((px)*(sj1)); -IkReal x1676=((IkReal(0.999999985550000))*(sj0)); -IkReal x1677=((IkReal(0.000169999997543500))*(cj0)); -IkReal x1678=((IkReal(0.000169999997543500))*(sj0)); -IkReal x1679=((cj1)*(px)); -IkReal x1680=((pz)*(sj1)); -IkReal x1681=((cj0)*(r00)); -IkReal x1682=((cj1)*(pz)); -IkReal x1683=((IkReal(1.00000000000000))*(r01)); -IkReal x1684=((cj1)*(py)); -IkReal x1685=((IkReal(0.999999985550000))*(cj0)); -IkReal x1686=((IkReal(0.106000000000000))*(x1669)); -IkReal x1687=((IkReal(0.106000000000000))*(x1670)); -IkReal x1688=((IkReal(0.112000000000000))*(x1670)); -IkReal x1689=((IkReal(0.112000000000000))*(x1669)); -IkReal x1690=((cj3)*(x1689)); -IkReal x1691=((sj3)*(x1688)); -IkReal x1692=((cj3)*(x1688)); -IkReal x1693=((sj3)*(x1689)); -IkReal x1694=((x1692)+(x1687)); -IkReal x1695=((x1690)+(x1691)+(x1686)); -evalcond[0]=((x1695)+(x1680)+(((x1684)*(x1685)))+(((IkReal(-1.00000000000000))*(x1676)*(x1679)))+(((x1678)*(x1684)))+(((x1677)*(x1679)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((x1675)*(x1676)))+(((IkReal(-1.00000000000000))*(x1673)*(x1678)))+(((IkReal(-1.00000000000000))*(x1694)))+(x1693)+(x1682)+(((IkReal(-1.00000000000000))*(x1673)*(x1685)))+(((IkReal(-1.00000000000000))*(x1675)*(x1677)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x1675)*(x1678)))+(((IkReal(0.999999985550000))*(x1680)*(x1681)))+(((IkReal(0.000169999997543500))*(x1671)*(x1680)))+(((r00)*(x1684)))+(x1695)+(((IkReal(-1.00000000000000))*(x1679)*(x1683)))+(((IkReal(-1.00000000000000))*(sj0)*(x1673)*(x1674)))+(((IkReal(-1.00000000000000))*(r01)*(x1677)*(x1680)))+(((IkReal(-1.00000000000000))*(cj0)*(x1674)*(x1675)))+(((IkReal(0.999999985550000))*(x1672)*(x1680)))+(((r02)*(x1673)*(x1677)))); -evalcond[3]=((((r00)*(x1673)))+(((IkReal(-1.80199997396110e-5))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(x1693)))+(((IkReal(-1.00000000000000))*(x1675)*(x1683)))+(x1694)+(((r01)*(x1677)*(x1682)))+(((r02)*(x1678)*(x1679)))+(((IkReal(-1.00000000000000))*(r02)*(x1677)*(x1684)))+(((IkReal(1.80199997396110e-5))*(x1671)))+(((IkReal(-0.999999985550000))*(x1672)*(x1682)))+(((IkReal(-0.000169999997543500))*(x1671)*(x1682)))+(((sj0)*(x1674)*(x1684)))+(((IkReal(0.105999998468300))*(x1672)))+(((IkReal(-0.999999985550000))*(x1681)*(x1682)))+(((cj0)*(x1674)*(x1679)))+(((IkReal(0.105999998468300))*(x1681)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ +IkReal x404=(cj3*r02); +IkReal x405=((1.0)*sj1); +IkReal x406=(r02*sj3); +IkReal x407=(r00*sj0*sj3); +IkReal x408=(cj3*r00*sj0); +IkReal x409=(cj0*cj3*r01); +IkReal x410=((1.0)*cj0*r01*sj3); +CheckValue x411 = IKatan2WithCheck(IkReal((((sj1*x408))+(((-1.0)*cj1*x410))+(((-1.0)*x405*x406))+(((-1.0)*x405*x409))+((cj1*x404))+((cj1*x407)))),IkReal((((sj1*x407))+((sj1*x404))+(((-1.0)*cj1*x408))+(((-1.0)*cj0*r01*sj3*x405))+((cj1*x406))+((cj1*x409)))),IKFAST_ATAN2_MAGTHRESH); +if(!x411.valid){ continue; } -} - -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -if( 1 ) -{ +CheckValue x412=IKPowWithIntegerCheck(IKsign(cj5),-1); +if(!x412.valid){ continue; - -} else -{ } -} -} -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1696=((cj0)*(py)); -IkReal x1697=((r02)*(sj1)); -IkReal x1698=((px)*(sj0)); -IkReal x1699=((IkReal(13.9999997977000))*(r00)); -IkReal x1700=((IkReal(1.69999943143527e-5))*(cj5)); -IkReal x1701=((cj0)*(cj1)); -IkReal x1702=((IkReal(0.00237999996560900))*(r01)); -IkReal x1703=((cj3)*(px)); -IkReal x1704=((IkReal(13.9999997977000))*(r01)); -IkReal x1705=((cj1)*(sj0)); -IkReal x1706=((px)*(sj3)); -IkReal x1707=((IkReal(0.00237999996560900))*(r00)); -IkReal x1708=((IkReal(0.0212499928929409))*(cj5)); -IkReal x1709=((cj3)*(py)); -IkReal x1710=((sj3)*(x1705)); -IkReal x1711=((IkReal(0.0999999665550159))*(cj1)*(cj5)*(sj3)); -IkReal x1712=((cj5)*(pz)*(sj1)*(sj3)); -IkReal x1713=((IkReal(0.0999999665550159))*(cj1)*(cj3)*(cj5)); -IkReal x1714=((cj3)*(cj5)*(pz)*(sj1)); -IkReal x1715=((IkReal(124.999958193770))*(cj1)*(cj5)*(sj3)); -IkReal x1716=((IkReal(124.999958193770))*(cj1)*(cj3)*(cj5)); -if( IKabs(((gconst2)*(((((x1698)*(x1711)))+(((x1696)*(x1716)))+(((x1705)*(x1708)*(x1709)))+(((IkReal(-1.00000000000000))*(x1699)*(x1710)))+(((x1702)*(x1710)))+(((sj3)*(x1701)*(x1707)))+(((sj3)*(x1701)*(x1704)))+(((IkReal(-0.0999999680000154))*(x1712)))+(((IkReal(14.0000000000000))*(sj3)*(x1697)))+(((x1701)*(x1703)*(x1708)))+(((IkReal(-1.00000000000000))*(x1696)*(x1711)))+(((IkReal(-1.00000000000000))*(x1700)*(x1701)*(x1706)))+(((IkReal(-1.00000000000000))*(x1698)*(x1716)))+(((IkReal(124.999960000019))*(x1714)))+(((IkReal(-1.00000000000000))*(py)*(x1700)*(x1710))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((x1696)*(x1715)))+(((x1696)*(x1713)))+(((py)*(x1708)*(x1710)))+(((IkReal(-13.2500000000000))*(x1697)))+(((x1700)*(x1705)*(x1709)))+(((x1700)*(x1701)*(x1703)))+(((IkReal(-1.00000000000000))*(cj3)*(x1701)*(x1704)))+(((IkReal(-1.00000000000000))*(cj3)*(x1701)*(x1707)))+(((IkReal(-14.0000000000000))*(cj3)*(x1697)))+(((IkReal(-0.00225249996745138))*(r01)*(x1705)))+(((cj3)*(x1699)*(x1705)))+(((IkReal(-13.2499998085375))*(r01)*(x1701)))+(((IkReal(-1.00000000000000))*(cj3)*(x1702)*(x1705)))+(((IkReal(0.0999999680000154))*(x1714)))+(((x1701)*(x1706)*(x1708)))+(((IkReal(13.2499998085375))*(r00)*(x1705)))+(((IkReal(-0.00225249996745138))*(r00)*(x1701)))+(((IkReal(-1.00000000000000))*(x1698)*(x1713)))+(((IkReal(-1.00000000000000))*(x1698)*(x1715)))+(((IkReal(124.999960000019))*(x1712))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst2)*(((((x1698)*(x1711)))+(((x1696)*(x1716)))+(((x1705)*(x1708)*(x1709)))+(((IkReal(-1.00000000000000))*(x1699)*(x1710)))+(((x1702)*(x1710)))+(((sj3)*(x1701)*(x1707)))+(((sj3)*(x1701)*(x1704)))+(((IkReal(-0.0999999680000154))*(x1712)))+(((IkReal(14.0000000000000))*(sj3)*(x1697)))+(((x1701)*(x1703)*(x1708)))+(((IkReal(-1.00000000000000))*(x1696)*(x1711)))+(((IkReal(-1.00000000000000))*(x1700)*(x1701)*(x1706)))+(((IkReal(-1.00000000000000))*(x1698)*(x1716)))+(((IkReal(124.999960000019))*(x1714)))+(((IkReal(-1.00000000000000))*(py)*(x1700)*(x1710)))))), ((gconst2)*(((((x1696)*(x1715)))+(((x1696)*(x1713)))+(((py)*(x1708)*(x1710)))+(((IkReal(-13.2500000000000))*(x1697)))+(((x1700)*(x1705)*(x1709)))+(((x1700)*(x1701)*(x1703)))+(((IkReal(-1.00000000000000))*(cj3)*(x1701)*(x1704)))+(((IkReal(-1.00000000000000))*(cj3)*(x1701)*(x1707)))+(((IkReal(-14.0000000000000))*(cj3)*(x1697)))+(((IkReal(-0.00225249996745138))*(r01)*(x1705)))+(((cj3)*(x1699)*(x1705)))+(((IkReal(-13.2499998085375))*(r01)*(x1701)))+(((IkReal(-1.00000000000000))*(cj3)*(x1702)*(x1705)))+(((IkReal(0.0999999680000154))*(x1714)))+(((x1701)*(x1706)*(x1708)))+(((IkReal(13.2499998085375))*(r00)*(x1705)))+(((IkReal(-0.00225249996745138))*(r00)*(x1701)))+(((IkReal(-1.00000000000000))*(x1698)*(x1713)))+(((IkReal(-1.00000000000000))*(x1698)*(x1715)))+(((IkReal(124.999960000019))*(x1712))))))); +j2array[0]=((-1.5707963267949)+(x411.value)+(((1.5707963267949)*(x412.value)))); sj2array[0]=IKsin(j2array[0]); cj2array[0]=IKcos(j2array[0]); if( j2array[0] > IKPI ) @@ -4404,76 +1771,64 @@ if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRES j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; { IkReal evalcond[8]; -IkReal x1717=IKsin(j2); -IkReal x1718=IKcos(j2); -IkReal x1719=(py)*(py); -IkReal x1720=(pz)*(pz); -IkReal x1721=(px)*(px); -IkReal x1722=((cj0)*(r00)); -IkReal x1723=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1724=((r01)*(sj0)); -IkReal x1725=((cj3)*(cj5)); -IkReal x1726=((sj0)*(sj1)); -IkReal x1727=((IkReal(0.999999985550000))*(px)); -IkReal x1728=((IkReal(0.000339999995087000))*(py)); -IkReal x1729=((pz)*(r02)); -IkReal x1730=((py)*(sj1)); -IkReal x1731=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1732=((IkReal(0.999999680000154))*(cj5)); -IkReal x1733=((px)*(r00)); -IkReal x1734=((IkReal(0.000799999744000123))*(cj5)); -IkReal x1735=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1736=((cj0)*(sj1)); -IkReal x1737=((IkReal(1.99999997110000))*(px)); -IkReal x1738=((IkReal(0.000169999997543500))*(pz)); -IkReal x1739=((IkReal(0.112000000000000))*(sj5)); -IkReal x1740=((cj1)*(r02)); -IkReal x1741=((IkReal(0.106000000000000))*(sj5)); -IkReal x1742=((IkReal(0.000339999995087000))*(px)); -IkReal x1743=((IkReal(0.112000000000000))*(cj3)); -IkReal x1744=((cj1)*(py)); -IkReal x1745=((py)*(r02)); -IkReal x1746=((cj0)*(px)); -IkReal x1747=((px)*(r01)); -IkReal x1748=((cj0)*(r01)); -IkReal x1749=((IkReal(0.999999985550000))*(r01)); -IkReal x1750=((IkReal(0.999999985550000))*(pz)); -IkReal x1751=((r02)*(sj1)); -IkReal x1752=((cj1)*(sj0)); -IkReal x1753=((IkReal(1.99999997110000))*(cj0)); -IkReal x1754=((IkReal(0.000169999997543500))*(r02)); -IkReal x1755=((IkReal(1.04639966515216e-6))*(cj5)); -IkReal x1756=((IkReal(0.999999985550000))*(cj0)); -IkReal x1757=((IkReal(0.999999985550000))*(cj1)); -IkReal x1758=((cj1)*(pz)); -IkReal x1759=((py)*(sj0)); -IkReal x1760=((r00)*(sj0)); -IkReal x1761=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1762=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1763=((IkReal(0.00130799958144020))*(cj5)); -IkReal x1764=((IkReal(0.211999996936600))*(pz)); -IkReal x1765=((pz)*(sj1)); -IkReal x1766=((sj3)*(x1718)); -IkReal x1767=((cj1)*(x1747)); -IkReal x1768=((r00)*(x1752)); -IkReal x1769=((IkReal(0.999999985550000))*(x1720)); -IkReal x1770=((IkReal(2.00000000000000))*(pz)*(r01)); -IkReal x1771=((sj3)*(x1717)); -IkReal x1772=((cj5)*(x1718)); -IkReal x1773=((IkReal(0.999999985550000))*(x1719)); -IkReal x1774=((IkReal(2.00000000000000))*(x1720)); -IkReal x1775=((IkReal(0.999999985550000))*(x1721)); -IkReal x1776=((px)*(r02)*(sj0)); -IkReal x1777=((cj5)*(x1717)); -evalcond[0]=((x1765)+(((x1744)*(x1756)))+(((IkReal(0.106000000000000))*(x1717)))+(((IkReal(0.112000000000000))*(x1766)))+(((IkReal(-1.00000000000000))*(x1727)*(x1752)))+(((x1735)*(x1759)))+(((x1717)*(x1743)))+(((x1735)*(x1746)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(0.112000000000000))*(x1771)))+(x1758)+(((x1726)*(x1727)))+(((IkReal(-0.106000000000000))*(x1718)))+(((IkReal(-1.00000000000000))*(x1730)*(x1756)))+(((IkReal(-1.00000000000000))*(x1731)*(x1746)))+(((IkReal(-0.000169999997543500))*(py)*(x1726)))+(((IkReal(-1.00000000000000))*(x1718)*(x1743)))); -evalcond[2]=((((x1732)*(x1771)))+(((x1722)*(x1735)))+(((x1724)*(x1735)))+(x1751)+(((IkReal(0.000799999744000123))*(x1717)*(x1725)))+(((x1748)*(x1757)))+(((x1734)*(x1766)))+(((IkReal(-0.999999680000154))*(x1718)*(x1725)))+(((IkReal(-0.999999985550000))*(x1768)))); -evalcond[3]=((x1740)+(((IkReal(-1.00000000000000))*(x1722)*(x1731)))+(((IkReal(-1.00000000000000))*(x1736)*(x1749)))+(((IkReal(-1.00000000000000))*(x1732)*(x1766)))+(((IkReal(0.999999985550000))*(r00)*(x1726)))+(((x1734)*(x1771)))+(((IkReal(-0.000799999744000123))*(x1718)*(x1725)))+(((IkReal(-1.00000000000000))*(x1724)*(x1731)))+(((IkReal(-0.999999680000154))*(x1717)*(x1725)))); -evalcond[4]=((((r00)*(x1726)*(x1738)))+(((IkReal(-1.00000000000000))*(px)*(x1726)*(x1754)))+(((IkReal(-1.00000000000000))*(x1767)))+(((sj1)*(x1724)*(x1750)))+(((IkReal(-1.00000000000000))*(x1717)*(x1741)))+(((sj1)*(x1722)*(x1750)))+(((IkReal(-1.00000000000000))*(cj3)*(x1717)*(x1739)))+(((IkReal(-0.999999985550000))*(x1726)*(x1745)))+(((IkReal(-1.00000000000000))*(r02)*(x1727)*(x1736)))+(((IkReal(-1.00000000000000))*(pz)*(x1731)*(x1748)))+(((r00)*(x1744)))+(((IkReal(-1.00000000000000))*(x1739)*(x1766)))+(((cj0)*(x1730)*(x1754)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(pz)*(x1735)*(x1760)))+(((IkReal(-1.80199997396110e-5))*(x1748)))+(((r00)*(x1730)))+(((x1735)*(x1776)))+(((IkReal(-1.00000000000000))*(cj3)*(x1718)*(x1739)))+(((IkReal(-1.00000000000000))*(cj0)*(x1735)*(x1745)))+(((cj0)*(x1727)*(x1740)))+(((pz)*(x1735)*(x1748)))+(((IkReal(-1.00000000000000))*(sj1)*(x1747)))+(((IkReal(-1.00000000000000))*(cj1)*(x1722)*(x1750)))+(((IkReal(0.999999985550000))*(x1740)*(x1759)))+(((IkReal(-1.00000000000000))*(x1718)*(x1741)))+(((IkReal(-1.00000000000000))*(cj1)*(x1724)*(x1750)))+(((x1739)*(x1771)))+(((IkReal(1.80199997396110e-5))*(x1760)))+(((IkReal(0.105999998468300))*(x1724)))+(((IkReal(0.105999998468300))*(x1722)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x1721)*(x1736)*(x1749)))+(((x1721)*(x1722)*(x1731)))+(((r00)*(x1726)*(x1773)))+(((IkReal(-0.0112359998376398))*(r00)*(x1726)))+(((x1722)*(x1730)*(x1737)))+(((IkReal(-1.00000000000000))*(x1724)*(x1730)*(x1737)))+(((IkReal(0.212000000000000))*(x1733)))+(((IkReal(-1.00000000000000))*(x1726)*(x1729)*(x1737)))+(((IkReal(0.212000000000000))*(x1729)))+(((x1728)*(x1736)*(x1747)))+(((IkReal(-2.00000000000000))*(x1733)*(x1758)))+(((x1722)*(x1762)))+(((IkReal(1.89951939215389e-5))*(x1772)))+(((IkReal(-1.00000000000000))*(x1763)*(x1766)))+(((x1719)*(x1736)*(x1749)))+(((IkReal(-0.0237439924019236))*(x1777)))+(((IkReal(-1.00000000000000))*(x1719)*(x1722)*(x1731)))+(((x1724)*(x1762)))+(((IkReal(-1.00000000000000))*(x1755)*(x1771)))+(((IkReal(-0.0237799923904037))*(x1717)*(x1725)))+(((x1726)*(x1728)*(x1733)))+(((IkReal(-1.00000000000000))*(x1720)*(x1736)*(x1749)))+(((IkReal(0.212000000000000))*(py)*(r01)))+(((x1726)*(x1728)*(x1729)))+(((IkReal(-1.00000000000000))*(x1744)*(x1770)))+(((IkReal(-1.00000000000000))*(x1740)*(x1774)))+(((IkReal(1.90239939123229e-5))*(x1718)*(x1725)))+(((r00)*(x1726)*(x1769)))+(((IkReal(-1.00000000000000))*(x1720)*(x1722)*(x1731)))+(((x1729)*(x1730)*(x1753)))+(((IkReal(-0.0112360000000000))*(x1740)))+(((x1729)*(x1736)*(x1742)))+(((x1719)*(x1724)*(x1731)))+(((IkReal(0.0112359998376398))*(r01)*(x1736)))+(((IkReal(-1.00000000000000))*(r00)*(x1726)*(x1775)))+(((IkReal(-1.00000000000000))*(x1720)*(x1724)*(x1731)))+(((IkReal(-1.00000000000000))*(x1721)*(x1724)*(x1731)))+(((pp)*(x1740)))); -evalcond[7]=((((pp)*(x1751)))+(((IkReal(-1.00000000000000))*(x1719)*(x1724)*(x1735)))+(((x1720)*(x1748)*(x1757)))+(((x1720)*(x1724)*(x1735)))+(((IkReal(-1.00000000000000))*(x1719)*(x1748)*(x1757)))+(((x1721)*(x1724)*(x1735)))+(((IkReal(-0.0237799923904037))*(x1718)*(x1725)))+(((x1763)*(x1771)))+(((IkReal(-1.00000000000000))*(x1722)*(x1737)*(x1744)))+(((x1720)*(x1722)*(x1735)))+(((IkReal(-1.00000000000000))*(x1722)*(x1723)))+(((IkReal(-1.00000000000000))*(x1755)*(x1766)))+(((x1722)*(x1761)))+(((IkReal(-0.0237439924019236))*(x1772)))+(((IkReal(-1.00000000000000))*(x1721)*(x1722)*(x1735)))+(((x1719)*(x1722)*(x1735)))+(((IkReal(-1.00000000000000))*(x1723)*(x1724)))+(((IkReal(-1.00000000000000))*(x1729)*(x1744)*(x1753)))+(((x1724)*(x1761)))+(((IkReal(3.60399994792220e-5))*(sj0)*(x1745)))+(((IkReal(-0.0112359998376398))*(x1768)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x1729)*(x1742)))+(((IkReal(-1.00000000000000))*(x1748)*(x1764)))+(((IkReal(0.211999996936600))*(cj0)*(x1745)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1728)*(x1746)))+(((IkReal(-1.89951939215389e-5))*(x1777)))+(((IkReal(-1.90239939123229e-5))*(x1717)*(x1725)))+(((IkReal(-1.00000000000000))*(x1730)*(x1770)))+(((x1724)*(x1737)*(x1744)))+(((x1721)*(x1748)*(x1757)))+(((IkReal(0.0112359998376398))*(cj1)*(x1748)))+(((IkReal(-1.00000000000000))*(x1751)*(x1774)))+(((x1729)*(x1737)*(x1752)))+(((IkReal(0.0112360000000000))*(x1751)))+(((IkReal(-2.00000000000000))*(x1733)*(x1765)))+(((IkReal(-1.00000000000000))*(x1728)*(x1733)*(x1752)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1746)))+(((IkReal(-1.00000000000000))*(x1768)*(x1773)))+(((x1768)*(x1775)))+(((IkReal(-1.00000000000000))*(x1768)*(x1769)))+(((IkReal(-1.00000000000000))*(x1728)*(x1729)*(x1752)))+(((x1760)*(x1764)))+(((IkReal(-0.211999996936600))*(x1776)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) +IkReal x413=IKcos(j2); +IkReal x414=IKsin(j2); +IkReal x415=px*px; +IkReal x416=pz*pz; +IkReal x417=py*py; +IkReal x418=(py*sj1); +IkReal x419=((2.0)*r01); +IkReal x420=(px*sj0); +IkReal x421=(pz*r00); +IkReal x422=(cj0*sj1); +IkReal x423=((0.0237799999999879)*cj3); +IkReal x424=(cj0*r01); +IkReal x425=(px*r00); +IkReal x426=((0.212)*pz); +IkReal x427=((1.0)*px); +IkReal x428=((2.332e-8)*cj5); +IkReal x429=(cj1*r01); +IkReal x430=(cj1*r02); +IkReal x431=((2.2e-7)*r02); +IkReal x432=(r01*sj0); +IkReal x433=(r00*sj0); +IkReal x434=((1.0)*cj1); +IkReal x435=((2.0)*pz); +IkReal x436=(cj0*r00); +IkReal x437=((0.112)*cj3); +IkReal x438=(cj1*py); +IkReal x439=((1.0)*cj3); +IkReal x440=((2.0)*cj0); +IkReal x441=(cj0*py); +IkReal x442=(r02*sj1); +IkReal x443=((2.332e-8)*cj1); +IkReal x444=((1.0)*r01); +IkReal x445=(pz*sj1); +IkReal x446=((1.0)*sj0); +IkReal x447=(py*r01); +IkReal x448=((0.212)*r02); +IkReal x449=((0.0112359999999879)*cj1); +IkReal x450=((2.0)*px); +IkReal x451=(sj3*x413); +IkReal x452=(cj5*x413); +IkReal x453=(cj1*x415); +IkReal x454=((1.0)*x415); +IkReal x455=(cj5*x414); +IkReal x456=(sj1*x417); +IkReal x457=(sj5*x414); +IkReal x458=(sj3*x414); +IkReal x459=((0.106)*x413); +IkReal x460=((1.1e-7)*x413); +IkReal x461=((1.0)*x416); +evalcond[0]=(((sj3*x455))+(((-1.0)*x433*x434))+(((-1.0)*x439*x452))+x442+((cj1*x424))); +evalcond[1]=((((-1.0)*x439*x455))+(((-1.0)*cj5*x451))+x430+(((-1.0)*x422*x444))+((sj1*x433))); +evalcond[2]=((-1.1e-7)+((x414*x437))+(((-1.0)*x420*x434))+(((0.112)*x451))+(((-1.0)*x460))+((cj0*x438))+x445+(((0.106)*x414))); +evalcond[3]=((-0.106)+(((-1.0)*x413*x437))+(((-1.1e-7)*x414))+(((-1.0)*cj0*x418))+(((0.112)*x458))+((cj1*pz))+(((-1.0)*x459))+((sj1*x420))); +evalcond[4]=(((py*sj0*x430))+(((-1.0)*pz*x429*x446))+(((0.112)*sj3*x457))+(((-1.0)*r01*sj1*x427))+(((-1.0)*sj5*x459))+(((0.106)*x436))+(((0.106)*x432))+(((-1.0)*cj0*x421*x434))+((cj0*px*x430))+((r00*x418))+(((-1.1e-7)*x457))+(((-1.0)*sj5*x413*x437))); +evalcond[5]=((((-1.1e-7)*x436))+(((-1.1e-7)*x432))+(((-1.0)*x437*x457))+(((-1.0)*x427*x429))+(((-0.112)*sj5*x451))+((r00*x438))+(((-1.0)*r02*x418*x446))+((sj5*x460))+(((-0.106)*x457))+((x421*x422))+((x432*x445))+(((-1.0)*r02*x422*x427))); +evalcond[6]=((((-1.0)*x416*x433*x434))+((x419*x420*x438))+(((0.0112359999999879)*x442))+((x428*x451))+(((-1.0)*x430*x435*x441))+((x424*x449))+((x415*x442))+(((-1.0)*x423*x452))+(((-1.0)*sj1*x421*x450))+(((-1.0)*x433*x449))+(((2.332e-8)*r01*x422))+((x433*x453))+(((-1.0)*x420*x448))+(((-1.0)*cj3*x414*x428))+(((-1.0)*pz*x418*x419))+((pz*x431))+(((-2.332e-8)*sj1*x433))+((cj1*x416*x424))+((x424*x453))+(((-1.0)*x417*x424*x434))+((x441*x448))+(((-1.0)*x442*x461))+(((2.2e-7)*x425))+((x420*x430*x435))+(((-1.0)*x424*x426))+(((0.212)*sj0*x421))+(((-0.023744)*x452))+((x417*x442))+(((2.2e-7)*x447))+(((-1.0)*x417*x433*x434))+(((-1.0)*x425*x438*x440))+(((0.0013080000000121)*sj3*x455))+(((-2.464e-8)*x455))+(((-2.332e-8)*x430))); +evalcond[7]=(((x428*x458))+(((-2.332e-8)*x442))+(((-1.0)*x423*x455))+((x415*x430))+(((2.2e-7)*pz*x424))+((x433*x456))+(((2.464e-8)*x452))+((r01*x417*x422))+(((-1.0)*x418*x419*x420))+(((-1.0)*x431*x441))+(((-1.0)*x420*x435*x442))+((r02*x426))+(((-1.0)*pz*x419*x438))+(((0.212)*x425))+(((-1.0)*cj1*x421*x450))+(((0.212)*x447))+(((-1.0)*x424*x443))+((sj1*x416*x433))+((cj0*r02*x418*x435))+(((-2.2e-7)*sj0*x421))+((cj3*x413*x428))+(((-0.0112359999999879)*sj1*x433))+(((-0.023744)*x455))+(((-1.0)*sj1*x433*x454))+(((-1.0)*x415*x422*x444))+((x418*x425*x440))+(((-1.0)*x416*x422*x444))+((x420*x431))+((x433*x443))+((x417*x430))+(((-0.0112359999999879)*x430))+(((-1.0)*x430*x461))+(((-0.0013080000000121)*cj5*x451))+(((0.0112359999999879)*r01*x422))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) { continue; } @@ -4515,168 +1870,15 @@ solutions.AddSolution(vinfos,vfree); } } - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x1778=((cj0)*(sj1)); -IkReal x1779=((IkReal(0.281562495931422))*(px)); -IkReal x1780=((IkReal(1750.00000000000))*(pz)); -IkReal x1781=((sj1)*(sj3)); -IkReal x1782=((cj1)*(cj3)); -IkReal x1783=((IkReal(1656.24997606719))*(cj1)); -IkReal x1784=((px)*(sj0)); -IkReal x1785=((cj3)*(sj1)); -IkReal x1786=((cj0)*(py)); -IkReal x1787=((cj1)*(sj3)); -IkReal x1788=((py)*(sj0)); -IkReal x1789=((IkReal(0.297499995701125))*(px)); -IkReal x1790=((IkReal(1749.99997471250))*(py)); -IkReal x1791=((IkReal(1656.25000000000))*(pz)); -if( IKabs(((gconst1)*(((((cj0)*(cj1)*(x1779)))+(((IkReal(-1.00000000000000))*(x1783)*(x1784)))+(((x1783)*(x1786)))+(((IkReal(0.281562495931422))*(cj1)*(x1788)))+(((IkReal(-1749.99997471250))*(x1782)*(x1784)))+(((sj1)*(x1791)))+(((IkReal(-0.297499995701125))*(x1781)*(x1788)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj0)*(x1782)*(x1789)))+(((IkReal(-1.00000000000000))*(sj3)*(x1778)*(x1790)))+(((IkReal(0.297499995701125))*(x1782)*(x1788)))+(((IkReal(1749.99997471250))*(x1781)*(x1784)))+(((IkReal(1749.99997471250))*(x1782)*(x1786)))+(((x1780)*(x1787)))+(((x1780)*(x1785)))+(((IkReal(-1.00000000000000))*(sj3)*(x1778)*(x1789))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((IkReal(175.562500000000))+(((IkReal(0.281562495931422))*(sj1)*(x1788)))+(((IkReal(0.297499995701125))*(x1785)*(x1788)))+(((cj0)*(x1787)*(x1789)))+(((IkReal(0.297499995701125))*(x1787)*(x1788)))+(((IkReal(-1749.99997471250))*(x1784)*(x1785)))+(((IkReal(-1749.99997471250))*(x1784)*(x1787)))+(((IkReal(1749.99997471250))*(x1786)*(x1787)))+(((IkReal(-1.00000000000000))*(cj1)*(x1791)))+(((cj3)*(x1778)*(x1789)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x1780)*(x1782)))+(((cj3)*(x1778)*(x1790)))+(((x1780)*(x1781)))+(((x1778)*(x1779)))+(((IkReal(-1656.24997606719))*(sj1)*(x1784)))+(((IkReal(1656.24997606719))*(py)*(x1778))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst1)*(((((cj0)*(cj1)*(x1779)))+(((IkReal(-1.00000000000000))*(x1783)*(x1784)))+(((x1783)*(x1786)))+(((IkReal(0.281562495931422))*(cj1)*(x1788)))+(((IkReal(-1749.99997471250))*(x1782)*(x1784)))+(((sj1)*(x1791)))+(((IkReal(-0.297499995701125))*(x1781)*(x1788)))+(((IkReal(-185.500000000000))*(sj3)))+(((cj0)*(x1782)*(x1789)))+(((IkReal(-1.00000000000000))*(sj3)*(x1778)*(x1790)))+(((IkReal(0.297499995701125))*(x1782)*(x1788)))+(((IkReal(1749.99997471250))*(x1781)*(x1784)))+(((IkReal(1749.99997471250))*(x1782)*(x1786)))+(((x1780)*(x1787)))+(((x1780)*(x1785)))+(((IkReal(-1.00000000000000))*(sj3)*(x1778)*(x1789)))))), ((gconst1)*(((IkReal(175.562500000000))+(((IkReal(0.281562495931422))*(sj1)*(x1788)))+(((IkReal(0.297499995701125))*(x1785)*(x1788)))+(((cj0)*(x1787)*(x1789)))+(((IkReal(0.297499995701125))*(x1787)*(x1788)))+(((IkReal(-1749.99997471250))*(x1784)*(x1785)))+(((IkReal(-1749.99997471250))*(x1784)*(x1787)))+(((IkReal(1749.99997471250))*(x1786)*(x1787)))+(((IkReal(-1.00000000000000))*(cj1)*(x1791)))+(((cj3)*(x1778)*(x1789)))+(((IkReal(185.500000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x1780)*(x1782)))+(((cj3)*(x1778)*(x1790)))+(((x1780)*(x1781)))+(((x1778)*(x1779)))+(((IkReal(-1656.24997606719))*(sj1)*(x1784)))+(((IkReal(1656.24997606719))*(py)*(x1778))))))); -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[8]; -IkReal x1792=IKsin(j2); -IkReal x1793=IKcos(j2); -IkReal x1794=(py)*(py); -IkReal x1795=(pz)*(pz); -IkReal x1796=(px)*(px); -IkReal x1797=((cj0)*(r00)); -IkReal x1798=((IkReal(3.60399994792220e-5))*(pz)); -IkReal x1799=((r01)*(sj0)); -IkReal x1800=((cj3)*(cj5)); -IkReal x1801=((sj0)*(sj1)); -IkReal x1802=((IkReal(0.999999985550000))*(px)); -IkReal x1803=((IkReal(0.000339999995087000))*(py)); -IkReal x1804=((pz)*(r02)); -IkReal x1805=((py)*(sj1)); -IkReal x1806=((IkReal(0.000169999997543500))*(sj1)); -IkReal x1807=((IkReal(0.999999680000154))*(cj5)); -IkReal x1808=((px)*(r00)); -IkReal x1809=((IkReal(0.000799999744000123))*(cj5)); -IkReal x1810=((IkReal(0.000169999997543500))*(cj1)); -IkReal x1811=((cj0)*(sj1)); -IkReal x1812=((IkReal(1.99999997110000))*(px)); -IkReal x1813=((IkReal(0.000169999997543500))*(pz)); -IkReal x1814=((IkReal(0.112000000000000))*(sj5)); -IkReal x1815=((cj1)*(r02)); -IkReal x1816=((IkReal(0.106000000000000))*(sj5)); -IkReal x1817=((IkReal(0.000339999995087000))*(px)); -IkReal x1818=((IkReal(0.112000000000000))*(cj3)); -IkReal x1819=((cj1)*(py)); -IkReal x1820=((py)*(r02)); -IkReal x1821=((cj0)*(px)); -IkReal x1822=((px)*(r01)); -IkReal x1823=((cj0)*(r01)); -IkReal x1824=((IkReal(0.999999985550000))*(r01)); -IkReal x1825=((IkReal(0.999999985550000))*(pz)); -IkReal x1826=((r02)*(sj1)); -IkReal x1827=((cj1)*(sj0)); -IkReal x1828=((IkReal(1.99999997110000))*(cj0)); -IkReal x1829=((IkReal(0.000169999997543500))*(r02)); -IkReal x1830=((IkReal(1.04639966515216e-6))*(cj5)); -IkReal x1831=((IkReal(0.999999985550000))*(cj0)); -IkReal x1832=((IkReal(0.999999985550000))*(cj1)); -IkReal x1833=((cj1)*(pz)); -IkReal x1834=((py)*(sj0)); -IkReal x1835=((r00)*(sj0)); -IkReal x1836=((IkReal(1.91011997239877e-6))*(cj1)); -IkReal x1837=((IkReal(1.91011997239877e-6))*(sj1)); -IkReal x1838=((IkReal(0.00130799958144020))*(cj5)); -IkReal x1839=((IkReal(0.211999996936600))*(pz)); -IkReal x1840=((pz)*(sj1)); -IkReal x1841=((sj3)*(x1793)); -IkReal x1842=((cj1)*(x1822)); -IkReal x1843=((r00)*(x1827)); -IkReal x1844=((IkReal(0.999999985550000))*(x1795)); -IkReal x1845=((IkReal(2.00000000000000))*(pz)*(r01)); -IkReal x1846=((sj3)*(x1792)); -IkReal x1847=((cj5)*(x1793)); -IkReal x1848=((IkReal(0.999999985550000))*(x1794)); -IkReal x1849=((IkReal(2.00000000000000))*(x1795)); -IkReal x1850=((IkReal(0.999999985550000))*(x1796)); -IkReal x1851=((px)*(r02)*(sj0)); -IkReal x1852=((cj5)*(x1792)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x1802)*(x1827)))+(((IkReal(0.112000000000000))*(x1841)))+(x1840)+(((x1810)*(x1821)))+(((x1810)*(x1834)))+(((x1819)*(x1831)))+(((IkReal(0.106000000000000))*(x1792)))+(((x1792)*(x1818)))); -evalcond[1]=((IkReal(-0.106000000000000))+(((IkReal(-1.00000000000000))*(x1806)*(x1821)))+(((x1801)*(x1802)))+(((IkReal(-1.00000000000000))*(x1793)*(x1818)))+(((IkReal(-1.00000000000000))*(x1805)*(x1831)))+(((IkReal(-0.000169999997543500))*(py)*(x1801)))+(((IkReal(0.112000000000000))*(x1846)))+(((IkReal(-0.106000000000000))*(x1793)))+(x1833)); -evalcond[2]=((((IkReal(0.000799999744000123))*(x1792)*(x1800)))+(((IkReal(-0.999999680000154))*(x1793)*(x1800)))+(((IkReal(-0.999999985550000))*(x1843)))+(((x1799)*(x1810)))+(((x1823)*(x1832)))+(((x1809)*(x1841)))+(((x1807)*(x1846)))+(x1826)+(((x1797)*(x1810)))); -evalcond[3]=((((IkReal(-0.000799999744000123))*(x1793)*(x1800)))+(((IkReal(-1.00000000000000))*(x1811)*(x1824)))+(((IkReal(-1.00000000000000))*(x1807)*(x1841)))+(((IkReal(-1.00000000000000))*(x1797)*(x1806)))+(((IkReal(-1.00000000000000))*(x1799)*(x1806)))+(((IkReal(0.999999985550000))*(r00)*(x1801)))+(((IkReal(-0.999999680000154))*(x1792)*(x1800)))+(((x1809)*(x1846)))+(x1815)); -evalcond[4]=((((sj1)*(x1797)*(x1825)))+(((sj1)*(x1799)*(x1825)))+(((IkReal(-1.00000000000000))*(cj3)*(x1792)*(x1814)))+(((IkReal(-0.999999985550000))*(x1801)*(x1820)))+(((IkReal(-1.00000000000000))*(x1792)*(x1816)))+(((IkReal(-1.00000000000000))*(pz)*(x1806)*(x1823)))+(((IkReal(-1.00000000000000))*(x1814)*(x1841)))+(((r00)*(x1801)*(x1813)))+(((IkReal(-1.00000000000000))*(x1842)))+(((r00)*(x1819)))+(((IkReal(-1.00000000000000))*(r02)*(x1802)*(x1811)))+(((cj0)*(x1805)*(x1829)))+(((IkReal(-1.00000000000000))*(px)*(x1801)*(x1829)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(cj0)*(x1810)*(x1820)))+(((IkReal(0.105999998468300))*(x1797)))+(((IkReal(0.105999998468300))*(x1799)))+(((IkReal(-1.00000000000000))*(x1793)*(x1816)))+(((IkReal(0.999999985550000))*(x1815)*(x1834)))+(((IkReal(-1.00000000000000))*(cj3)*(x1793)*(x1814)))+(((IkReal(-1.00000000000000))*(sj1)*(x1822)))+(((pz)*(x1810)*(x1823)))+(((IkReal(-1.80199997396110e-5))*(x1823)))+(((IkReal(-1.00000000000000))*(cj1)*(x1797)*(x1825)))+(((x1814)*(x1846)))+(((IkReal(-1.00000000000000))*(pz)*(x1810)*(x1835)))+(((x1810)*(x1851)))+(((r00)*(x1805)))+(((IkReal(-1.00000000000000))*(cj1)*(x1799)*(x1825)))+(((cj0)*(x1802)*(x1815)))+(((IkReal(1.80199997396110e-5))*(x1835)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x1819)*(x1845)))+(((IkReal(-1.00000000000000))*(x1794)*(x1797)*(x1806)))+(((IkReal(-1.00000000000000))*(x1830)*(x1846)))+(((IkReal(1.90239939123229e-5))*(x1793)*(x1800)))+(((IkReal(-0.0237799923904037))*(x1792)*(x1800)))+(((x1794)*(x1799)*(x1806)))+(((IkReal(-1.00000000000000))*(x1799)*(x1805)*(x1812)))+(((IkReal(-0.0112360000000000))*(x1815)))+(((IkReal(1.89951939215389e-5))*(x1847)))+(((x1796)*(x1797)*(x1806)))+(((IkReal(0.0112359998376398))*(r01)*(x1811)))+(((IkReal(-1.00000000000000))*(x1796)*(x1811)*(x1824)))+(((IkReal(-1.00000000000000))*(r00)*(x1801)*(x1850)))+(((x1797)*(x1837)))+(((x1804)*(x1811)*(x1817)))+(((IkReal(-2.00000000000000))*(x1808)*(x1833)))+(((x1794)*(x1811)*(x1824)))+(((IkReal(0.212000000000000))*(x1808)))+(((IkReal(0.212000000000000))*(x1804)))+(((IkReal(-1.00000000000000))*(x1795)*(x1799)*(x1806)))+(((x1804)*(x1805)*(x1828)))+(((IkReal(-1.00000000000000))*(x1815)*(x1849)))+(((IkReal(-1.00000000000000))*(x1796)*(x1799)*(x1806)))+(((IkReal(-0.0237439924019236))*(x1852)))+(((IkReal(0.212000000000000))*(py)*(r01)))+(((x1797)*(x1805)*(x1812)))+(((IkReal(-1.00000000000000))*(x1801)*(x1804)*(x1812)))+(((pp)*(x1815)))+(((x1801)*(x1803)*(x1808)))+(((x1801)*(x1803)*(x1804)))+(((IkReal(-1.00000000000000))*(x1838)*(x1841)))+(((IkReal(-1.00000000000000))*(x1795)*(x1811)*(x1824)))+(((IkReal(-1.00000000000000))*(x1795)*(x1797)*(x1806)))+(((r00)*(x1801)*(x1848)))+(((r00)*(x1801)*(x1844)))+(((x1799)*(x1837)))+(((x1803)*(x1811)*(x1822)))+(((IkReal(-0.0112359998376398))*(r00)*(x1801)))); -evalcond[7]=((((IkReal(3.60399994792220e-5))*(sj0)*(x1820)))+(((IkReal(0.0112359998376398))*(cj1)*(x1823)))+(((x1843)*(x1850)))+(((IkReal(-1.00000000000000))*(x1805)*(x1845)))+(((IkReal(-1.00000000000000))*(x1803)*(x1804)*(x1827)))+(((x1838)*(x1846)))+(((IkReal(-1.00000000000000))*(x1843)*(x1848)))+(((IkReal(-1.00000000000000))*(x1843)*(x1844)))+(((IkReal(-1.00000000000000))*(x1830)*(x1841)))+(((x1795)*(x1799)*(x1810)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x1804)*(x1817)))+(((x1795)*(x1797)*(x1810)))+(((IkReal(0.211999996936600))*(cj0)*(x1820)))+(((IkReal(-1.90239939123229e-5))*(x1792)*(x1800)))+(((pp)*(x1826)))+(((x1835)*(x1839)))+(((IkReal(-0.0237799923904037))*(x1793)*(x1800)))+(((IkReal(-0.211999996936600))*(x1851)))+(((x1796)*(x1823)*(x1832)))+(((IkReal(-1.00000000000000))*(x1796)*(x1797)*(x1810)))+(((IkReal(-1.00000000000000))*(x1797)*(x1812)*(x1819)))+(((IkReal(-1.00000000000000))*(x1803)*(x1808)*(x1827)))+(((x1794)*(x1797)*(x1810)))+(((IkReal(-0.0237439924019236))*(x1847)))+(((x1797)*(x1836)))+(((IkReal(3.60399994792220e-5))*(r02)*(x1821)))+(((IkReal(-0.0112359998376398))*(x1843)))+(((IkReal(-1.00000000000000))*(x1794)*(x1823)*(x1832)))+(((IkReal(-1.00000000000000))*(x1804)*(x1819)*(x1828)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1803)*(x1821)))+(((x1804)*(x1812)*(x1827)))+(((x1796)*(x1799)*(x1810)))+(((IkReal(-1.00000000000000))*(x1826)*(x1849)))+(((IkReal(-1.00000000000000))*(x1823)*(x1839)))+(((IkReal(-1.89951939215389e-5))*(x1852)))+(((IkReal(-1.00000000000000))*(x1794)*(x1799)*(x1810)))+(((x1795)*(x1823)*(x1832)))+(((IkReal(0.0112360000000000))*(x1826)))+(((IkReal(-1.00000000000000))*(x1797)*(x1798)))+(((IkReal(-1.00000000000000))*(x1798)*(x1799)))+(((x1799)*(x1836)))+(((x1799)*(x1812)*(x1819)))+(((IkReal(-2.00000000000000))*(x1808)*(x1840)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; } } -{ -std::vector > vinfos(5); -vinfos[0].jointtype = 1; -vinfos[0].foffset = j0; -vinfos[0].indices[0] = _ij0[0]; -vinfos[0].indices[1] = _ij0[1]; -vinfos[0].maxsolutions = _nj0; -vinfos[1].jointtype = 1; -vinfos[1].foffset = j1; -vinfos[1].indices[0] = _ij1[0]; -vinfos[1].indices[1] = _ij1[1]; -vinfos[1].maxsolutions = _nj1; -vinfos[2].jointtype = 1; -vinfos[2].foffset = j2; -vinfos[2].indices[0] = _ij2[0]; -vinfos[2].indices[1] = _ij2[1]; -vinfos[2].maxsolutions = _nj2; -vinfos[3].jointtype = 1; -vinfos[3].foffset = j3; -vinfos[3].indices[0] = _ij3[0]; -vinfos[3].indices[1] = _ij3[1]; -vinfos[3].maxsolutions = _nj3; -vinfos[4].jointtype = 1; -vinfos[4].foffset = j5; -vinfos[4].indices[0] = _ij5[0]; -vinfos[4].indices[1] = _ij5[1]; -vinfos[4].maxsolutions = _nj5; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} } } - } +} + } } } @@ -4684,16 +1886,110 @@ solutions.AddSolution(vinfos,vfree); } } +return solutions.GetNumSolutions()>0; } +static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots) +{ + using std::complex; + if( rawcoeffs[0] == 0 ) { + // solve with one reduced degree + polyroots2(&rawcoeffs[1], &rawroots[0], numroots); + return; + } + IKFAST_ASSERT(rawcoeffs[0] != 0); + const IkReal tol = 128.0*std::numeric_limits::epsilon(); + const IkReal tolsqrt = sqrt(std::numeric_limits::epsilon()); + complex coeffs[3]; + const int maxsteps = 110; + for(int i = 0; i < 3; ++i) { + coeffs[i] = complex(rawcoeffs[i+1]/rawcoeffs[0]); + } + complex roots[3]; + IkReal err[3]; + roots[0] = complex(1,0); + roots[1] = complex(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 < 3; ++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 < 3; ++i) { + if ( err[i] >= tol ) { + changed = true; + // evaluate + complex x = roots[i] + coeffs[0]; + for(int j = 1; j < 3; ++j) { + x = roots[i] * x + coeffs[j]; + } + for(int j = 0; j < 3; ++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[3] = {false}; + for(int i = 0; i < 3; ++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 newroot=roots[i]; + int n = 1; + for(int j = i+1; j < 3; ++j) { + // care about error in real much more than imaginary + if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) { + 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)) < tolsqrt ) { + rawroots[numroots++] = real(newroot); + } + } } } -} -} -return solutions.GetNumSolutions()>0; +static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) { + IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2]; + if( det < 0 ) { + numroots=0; + } + else if( det == 0 ) { + rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0]; + numroots = 1; + } + else { + det = IKsqrt(det); + rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]); + rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]); + numroots = 2; + } } static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots) { using std::complex; + if( rawcoeffs[0] == 0 ) { + // solve with one reduced degree + polyroots3(&rawcoeffs[1], &rawroots[0], numroots); + return; + } IKFAST_ASSERT(rawcoeffs[0] != 0); const IkReal tol = 128.0*std::numeric_limits::epsilon(); const IkReal tolsqrt = sqrt(std::numeric_limits::epsilon()); @@ -4747,7 +2043,8 @@ static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& nu complex newroot=roots[i]; int n = 1; for(int j = i+1; j < 4; ++j) { - if( abs(roots[i]-roots[j]) < 8*tolsqrt ) { + // care about error in real much more than imaginary + if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) { newroot += roots[j]; n += 1; visited[j] = true; @@ -4773,9 +2070,14 @@ IKSolver solver; return solver.ComputeIk(eetrans,eerot,pfree,solutions); } -IKFAST_API const char* GetKinematicsHash() { return ""; } +IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions, void* pOpenRAVEManip) { +IKSolver solver; +return solver.ComputeIk(eetrans,eerot,pfree,solutions); +} + +IKFAST_API const char* GetKinematicsHash() { return ""; } -IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } +IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; } #ifdef IKFAST_NAMESPACE } // end namespace diff --git a/turtlebot_arm_ikfast_plugin/turtlebot_arm_moveit_ikfast_plugin_description.xml b/turtlebot_arm_ikfast_plugin/turtlebot_arm_moveit_ikfast_plugin_description.xml index ddcaf2c..fa0b421 100644 --- a/turtlebot_arm_ikfast_plugin/turtlebot_arm_moveit_ikfast_plugin_description.xml +++ b/turtlebot_arm_ikfast_plugin/turtlebot_arm_moveit_ikfast_plugin_description.xml @@ -1,5 +1,5 @@ - - + + IKFast61 plugin for closed-form kinematics