diff --git a/motoman_mpx3500_ikfast_plugin/CMakeLists.txt b/motoman_mpx3500_ikfast_plugin/CMakeLists.txt new file mode 100644 index 0000000..48ea071 --- /dev/null +++ b/motoman_mpx3500_ikfast_plugin/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.1.3) +project(motoman_mpx3500_ikfast_plugin) + +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(catkin REQUIRED COMPONENTS + moveit_core + pluginlib + roscpp + tf2_kdl + tf2_eigen + eigen_conversions +) +find_package(LAPACK REQUIRED) + +include_directories(${catkin_INCLUDE_DIRS} include) + +catkin_package() + +set(IKFAST_LIBRARY_NAME motoman_mpx3500_ikfast_plugin) +add_library(${IKFAST_LIBRARY_NAME} src/motoman_mpx3500_ikfast_plugin.cpp) +target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) +# suppress warnings about unused variables in OpenRave's solver code +target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable) + +install(TARGETS + ${IKFAST_LIBRARY_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install( + FILES + motoman_mpx3500_ikfast_plugin_description.xml + DESTINATION + ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/motoman_mpx3500_ikfast_plugin/include/ikfast.h b/motoman_mpx3500_ikfast_plugin/include/ikfast.h new file mode 100644 index 0000000..0728329 --- /dev/null +++ b/motoman_mpx3500_ikfast_plugin/include/ikfast.h @@ -0,0 +1,378 @@ +// -*- coding: utf-8 -*- +// Copyright (C) 2012 Rosen Diankov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/** \brief Header file for all ikfast c++ files/shared objects. + + The ikfast inverse kinematics compiler is part of OpenRAVE. + + 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 + + The defines are as follows, they are also used for the ikfast C++ class: + + - IKFAST_HEADER_COMMON - common classes + - 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_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. + + */ +#include +#include +#include + +#ifndef IKFAST_HEADER_COMMON +#define IKFAST_HEADER_COMMON + +/// should be the same as ikfast.__version__ +#define IKFAST_VERSION 61 + +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 +}; + +/// \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: +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 int GetDOF() const = 0; +}; + +/// \brief manages all the solutions +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; +}; + +/// \brief holds function pointers for all the exported functions of ikfast +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; +}; + +// Implementations of the abstract classes, user doesn't need to use them + +/// \brief Default implementation of \ref IkSolutionBase +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); + } + } + } + } + + 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 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]; + } + } + } + } + } + + std::vector > _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector _vfree; +}; + +/// \brief Default implementation of \ref IkSolutionListBase +template +class IkSolutionList : public IkSolutionListBase +{ +public: + 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()) + { + throw std::runtime_error("GetSolution index is invalid"); + } + typename std::list >::const_iterator it = _listsolutions.begin(); + std::advance(it, index); + return *it; + } + + virtual size_t GetNumSolutions() const + { + return _listsolutions.size(); + } + + virtual void Clear() + { + _listsolutions.clear(); + } + +protected: + std::list > _listsolutions; +}; +} // namespace ikfast + +#endif // OPENRAVE_IKFAST_HEADER + +// The following code is dependent on the C++ library linking with. +#ifdef IKFAST_HAS_LIBRARY + +// defined when creating a shared object/dll +#ifdef IKFAST_CLIBRARY +#ifdef _MSC_VER +#define IKFAST_API extern "C" __declspec(dllexport) +#else +#define IKFAST_API extern "C" +#endif +#else +#define IKFAST_API +#endif + +#ifdef IKFAST_NAMESPACE +namespace IKFAST_NAMESPACE +{ +#endif + +#ifdef IKFAST_REAL +typedef IKFAST_REAL IkReal; +#else +typedef double IkReal; +#endif + +/** \brief Computes all IK solutions given a end effector coordinates and the free joints. + + - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. + - ``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. + */ +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); + +/// \brief returns the number of free parameters users has to set apriori +IKFAST_API int GetNumFreeParameters(); + +/// \brief the indices of the free parameters indexed by the chain joints +IKFAST_API int* GetFreeParameters(); + +/// \brief the total number of indices of the chain +IKFAST_API int GetNumJoints(); + +/// \brief the size in bytes of the configured number type +IKFAST_API int GetIkRealSize(); + +/// \brief the ikfast version used to generate this file +IKFAST_API const char* GetIkFastVersion(); + +/// \brief the ik type ID +IKFAST_API int GetIkType(); + +/// \brief a hash of all the chain values used for double checking that the correct IK is used. +IKFAST_API const char* GetKinematicsHash(); + +#ifdef IKFAST_NAMESPACE +} +#endif + +#endif // IKFAST_HAS_LIBRARY diff --git a/motoman_mpx3500_ikfast_plugin/motoman_mpx3500_ikfast_plugin_description.xml b/motoman_mpx3500_ikfast_plugin/motoman_mpx3500_ikfast_plugin_description.xml new file mode 100644 index 0000000..20928be --- /dev/null +++ b/motoman_mpx3500_ikfast_plugin/motoman_mpx3500_ikfast_plugin_description.xml @@ -0,0 +1,6 @@ + + + + IKFast plugin for closed-form kinematics of Motoman MPX3500 manipulator + + diff --git a/motoman_mpx3500_ikfast_plugin/package.xml b/motoman_mpx3500_ikfast_plugin/package.xml new file mode 100644 index 0000000..6c8b0c8 --- /dev/null +++ b/motoman_mpx3500_ikfast_plugin/package.xml @@ -0,0 +1,28 @@ + + + motoman_mpx3500_ikfast_plugin + 0.1.0 + IKFast plugin for Motoman MPX3500 + BSD + Stevie Dale + + catkin + + moveit_core + tf2_kdl + pluginlib + eigen_conversions + tf2_eigen + liblapack-dev + roscpp + + moveit_core + pluginlib + liblapack-dev + roscpp + eigen_conversions + + + + + diff --git a/motoman_mpx3500_ikfast_plugin/src/motoman_mpx3500_ikfast_plugin.cpp b/motoman_mpx3500_ikfast_plugin/src/motoman_mpx3500_ikfast_plugin.cpp new file mode 100644 index 0000000..716f060 --- /dev/null +++ b/motoman_mpx3500_ikfast_plugin/src/motoman_mpx3500_ikfast_plugin.cpp @@ -0,0 +1,1398 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * 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 + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the all of the author's companies nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +/* + * IKFast Kinematics Solver Plugin for MoveIt! wrapping an ikfast solver from OpenRAVE. + * + * AUTO-GENERATED by create_ikfast_moveit_plugin.py in moveit_kinematics package. + * + * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt! kinematics plugin. + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace moveit::core; + +// 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 motoman_mpx3500_ikfast_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; + + bool operator<(const LimitObeyingSol& a) const + { + return dist_from_seed < a.dist_from_seed; + } +}; + +// Code generated by IKFast56/61 +#include "motoman_mpx3500_ikfast_solver.cpp" + +class IKFastKinematicsPlugin : public kinematics::KinematicsBase +{ + std::vector joint_names_; + std::vector joint_min_vector_; + std::vector joint_max_vector_; + std::vector joint_has_limits_vector_; + std::vector link_names_; + const size_t num_joints_; + std::vector free_params_; + + // The ikfast and base frame are the start and end of the kinematic chain for which the + // IKFast analytic solution was generated. + const std::string IKFAST_TIP_FRAME_ = "tool0"; + const std::string IKFAST_BASE_FRAME_ = "base_link"; + + // prefix added to tip- and baseframe to allow different namespaces or multi-robot setups + std::string link_prefix_; + + // The transform tip and base bool are set to true if this solver is used with a kinematic + // chain that extends beyond the ikfast tip and base frame. The solution will be valid so + // long as there are no active, passive, or mimic joints between either the ikfast_tip_frame + // and the tip_frame of the group or the ikfast_base_frame and the base_frame for the group. + bool tip_transform_required_; + bool base_transform_required_; + + // We store the transform from the ikfast_base_frame to the group base_frame as well as the + // ikfast_tip_frame to the group tip_frame to transform input poses into the solver frame. + Eigen::Isometry3d chain_base_to_group_base_; + Eigen::Isometry3d group_tip_to_chain_tip_; + + bool initialized_; // Internal variable that indicates whether solvers are configured and ready + const std::string name_{ "ikfast" }; + + const std::vector& getJointNames() const override + { + return joint_names_; + } + const std::vector& getLinkNames() const override + { + return link_names_; + } + +public: + /** @class + * @brief Interface for an IKFast kinematics plugin + */ + IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), initialized_(false) + { + srand(time(nullptr)); + 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 + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solution the solution vector + * @param error_code an error code that encodes the reason for failure or success + * @return True if a valid solution was found, false otherwise + */ + + // 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 override; + + /** + * @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 override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @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 override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @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 override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @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 override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). The consistency_limit specifies that only certain redundancy positions + * around those specified in the seed state are admissible and need to be searched. + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @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 override; + + /** + * @brief Given a set of joint angles and a set of links, compute their pose + * + * @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 override; + + /** + * @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); + + /** + * @brief Overrides the default method to prevent changing the redundant joints + */ + bool setRedundantJoints(const std::vector& redundant_joint_indices) override; + +private: + bool initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, + const std::string& base_frame, const std::vector& tip_frames, + double search_discretization) override; + + /** + * @brief Calls the IK solver from IKFast + * @return The number of solutions found + */ + size_t 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; + + /** + * @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; + + /** + * @brief If the value is outside of min/max then it tries to +/- 2 * pi to put the value into the range + */ + double enforceLimits(double val, double min, double max) 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; + + /// Validate that we can compute a fixed transform between from and to links. + bool computeRelativeTransform(const std::string& from, const std::string& to, Eigen::Isometry3d& transform, + bool& differs_from_identity); + /** + * @brief Transforms the input pose to the correct frame for the solver. This assumes that the group includes the + * entire solver chain and that any joints outside of the solver chain within the group are are fixed. + * @param ik_pose The pose to be transformed which should be in the correct frame for the group. + * @param ik_pose_chain The ik_pose to be populated with the apropriate pose for the solver + */ + void transformToChainFrame(const geometry_msgs::Pose& ik_pose, KDL::Frame& ik_pose_chain) const; +}; // end class + +bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, const std::string& to, + Eigen::Isometry3d& transform, bool& differs_from_identity) +{ + RobotStatePtr robot_state; + robot_state.reset(new RobotState(robot_model_)); + robot_state->setToDefaultValues(); + + auto* from_link = robot_model_->getLinkModel(from); + auto* to_link = robot_model_->getLinkModel(to); + if (!from_link || !to_link) + return false; + + if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) != + robot_model_->getRigidlyConnectedParentLinkModel(to_link)) + { + ROS_ERROR_STREAM_NAMED(name_, "Link frames " << from << " and " << to << " are not rigidly connected."); + return false; + } + + transform = robot_state->getGlobalLinkTransform(from_link).inverse() * robot_state->getGlobalLinkTransform(to_link); + differs_from_identity = !transform.matrix().isIdentity(); + return true; +} + +bool IKFastKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, + const std::string& base_frame, const std::vector& tip_frames, + double search_discretization) +{ + if (tip_frames.size() != 1) + { + ROS_ERROR_NAMED(name_, "Expecting exactly one tip frame."); + return false; + } + + storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); + if (!lookupParam("link_prefix", link_prefix_, std::string(""))) + { + ROS_INFO_NAMED(name_, "Using empty link_prefix."); + } + else + { + ROS_INFO_STREAM_NAMED(name_, "Using link_prefix: '" << link_prefix_ << "'"); + } + + // verbose error output. subsequent checks in computeRelativeTransform return false then + if (!robot_model.hasLinkModel(tip_frames_[0])) + ROS_ERROR_STREAM_NAMED(name_, "tip frame '" << tip_frames_[0] << "' does not exist."); + if (!robot_model.hasLinkModel(base_frame_)) + ROS_ERROR_STREAM_NAMED(name_, "base_frame '" << base_frame_ << "' does not exist."); + + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_TIP_FRAME_)) + ROS_ERROR_STREAM_NAMED(name_, "prefixed tip frame '" << link_prefix_ + IKFAST_TIP_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_BASE_FRAME_)) + ROS_ERROR_STREAM_NAMED(name_, "prefixed base frame '" << link_prefix_ + IKFAST_BASE_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); + // This IKFast solution was generated with IKFAST_TIP_FRAME_ and IKFAST_BASE_FRAME_. + // It is often the case that fixed joints are added to these links to model things like + // a robot mounted on a table or a robot with an end effector attached to the last link. + // To support these use cases, we store the transform from the IKFAST_BASE_FRAME_ to the + // base_frame_ and IKFAST_TIP_FRAME_ the tip_frame_ and transform to the input pose accordingly + if (!computeRelativeTransform(tip_frames_[0], link_prefix_ + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, + tip_transform_required_) || + !computeRelativeTransform(link_prefix_ + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, + base_transform_required_)) + { + return false; + } + + // IKFast56/61 + fillFreeParams(GetNumFreeParameters(), GetFreeParameters()); + + if (free_params_.size() > 1) + { + ROS_ERROR_NAMED(name_, "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(search_discretization); + } + + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); + if (!jmg) + { + ROS_ERROR_STREAM_NAMED(name_, "Unknown planning group: " << group_name); + return false; + } + + ROS_DEBUG_STREAM_NAMED(name_, "Registering joints and links"); + const moveit::core::LinkModel* link = robot_model_->getLinkModel(tip_frames_[0]); + const moveit::core::LinkModel* base_link = robot_model_->getLinkModel(base_frame_); + while (link && link != base_link) + { + ROS_DEBUG_STREAM_NAMED(name_, "Link " << link->getName()); + link_names_.push_back(link->getName()); + const moveit::core::JointModel* joint = link->getParentJointModel(); + if (joint->getType() != joint->UNKNOWN && joint->getType() != joint->FIXED && joint->getVariableCount() == 1) + { + ROS_DEBUG_STREAM_NAMED(name_, "Adding joint " << joint->getName()); + + joint_names_.push_back(joint->getName()); + const moveit::core::VariableBounds& bounds = joint->getVariableBounds()[0]; + joint_has_limits_vector_.push_back(bounds.position_bounded_); + joint_min_vector_.push_back(bounds.min_position_); + joint_max_vector_.push_back(bounds.max_position_); + } + link = link->getParentLinkModel(); + } + + if (joint_names_.size() != num_joints_) + { + ROS_FATAL_NAMED(name_, "Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match", + joint_names_.size(), 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(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end()); + + for (size_t joint_id = 0; joint_id < num_joints_; ++joint_id) + ROS_DEBUG_STREAM_NAMED(name_, joint_names_[joint_id] << " " << joint_min_vector_[joint_id] << " " + << joint_max_vector_[joint_id] << " " + << joint_has_limits_vector_[joint_id]); + + initialized_ = true; + return true; +} + +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_NAMED(name_, "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_NAMED(name_, "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_NAMED(name_, "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_NAMED(name_, "Changing the redundant joints isn't permitted by this group's solver "); + return false; +} + +size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector& vfree, + IkSolutionList& solutions) const +{ + // IKFast56/61 + solutions.Clear(); + + double trans[3]; + trans[0] = pose_frame.p[0]; //-.18; + trans[1] = pose_frame.p[1]; + trans[2] = pose_frame.p[2]; + + 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] : nullptr, 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] : nullptr, solutions); + return solutions.GetNumSolutions(); + + 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: + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_TranslationXAxisAngle4D: + // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin + // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the + // manipulator base link's coordinate system) + case IKP_TranslationXAxisAngleZNorm4D: + double roll, pitch, yaw; + // For **TranslationXAxisAngleZNorm4D** - 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) + pose_frame.M.GetRPY(roll, pitch, yaw); + ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationYAxisAngle4D: + case IKP_TranslationYAxisAngleXNorm4D: + // For **TranslationYAxisAngleXNorm4D** - 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) + pose_frame.M.GetRPY(roll, pitch, yaw); + ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationZAxisAngle4D: + case IKP_TranslationZAxisAngleYNorm4D: + // For **TranslationZAxisAngleYNorm4D** - 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) + pose_frame.M.GetRPY(roll, pitch, yaw); + ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + 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 +{ + 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] : nullptr); + + for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) + { + if (joint_has_limits_vector_[joint_id]) + { + solution[joint_id] = enforceLimits(solution[joint_id], joint_min_vector_[joint_id], joint_max_vector_[joint_id]); + } + } +} + +void IKFastKinematicsPlugin::getSolution(const IkSolutionList& 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] : nullptr); + + // 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]) + { + solution[i] = enforceLimits(solution[i], joint_min_vector_[i], joint_max_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::enforceLimits(double joint_value, double min, double max) const +{ + // If the joint_value is greater than max subtract 2 * PI until it is less than the max + while (joint_value > max) + { + joint_value -= 2 * M_PI; + } + + // If the joint_value is less than the min, add 2 * PI until it is more than the min + while (joint_value < min) + { + joint_value += 2 * M_PI; + } + return joint_value; +} + +void IKFastKinematicsPlugin::fillFreeParams(int count, int* array) +{ + free_params_.clear(); + for (int i = 0; i < count; ++i) + free_params_.push_back(array[i]); +} + +bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const +{ + if (count > 0) + { + if (-count >= min_count) + { + count = -count; + return true; + } + else if (count + 1 <= max_count) + { + count = count + 1; + return true; + } + else + { + return false; + } + } + else + { + if (1 - count <= max_count) + { + count = 1 - count; + return true; + } + else if (count - 1 >= min_count) + { + count = count - 1; + return true; + } + else + return false; + } +} + +bool IKFastKinematicsPlugin::getPositionFK(const std::vector& link_names, + const std::vector& joint_angles, + std::vector& poses) const +{ + if (GetIkType() != IKP_Transform6D) + { + // ComputeFk() is the inverse function of ComputeIk(), so the format of + // eerot differs depending on IK type. The Transform6D IK type is the only + // one for which a 3x3 rotation matrix is returned, which means we can only + // compute FK for that IK type. + ROS_ERROR_NAMED(name_, "Can only compute FK for Transform6D IK type!"); + return false; + } + + KDL::Frame p_out; + if (link_names.size() == 0) + { + ROS_WARN_STREAM_NAMED(name_, "Link names with nothing"); + return false; + } + + if (link_names.size() != 1 || link_names[0] != getTipFrame()) + { + ROS_ERROR_NAMED(name_, "Can compute FK for %s only", getTipFrame().c_str()); + return false; + } + + bool valid = true; + + IkReal eerot[9], eetrans[3]; + + if (joint_angles.size() != num_joints_) + { + ROS_ERROR_NAMED(name_, "Unexpected number of joint angles"); + return false; + } + + IkReal angles[num_joints_]; + for (unsigned char i = 0; i < num_joints_; i++) + angles[i] = joint_angles[i]; + + // IKFast56/61 + ComputeFk(angles, eetrans, eerot); + + for (int i = 0; i < 3; ++i) + p_out.p.data[i] = eetrans[i]; + + for (int i = 0; i < 9; ++i) + p_out.M.data[i] = eerot[i]; + + poses.resize(1); + poses[0] = tf2::toMsg(p_out); + + return valid; +} + +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 +{ + std::vector consistency_limits; + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), 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 +{ + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), 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 +{ + std::vector consistency_limits; + 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 +{ + // "SEARCH_MODE" is fixed during code generation + SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT; + + // Check if there are no redundant joints + if (free_params_.size() == 0) + { + ROS_DEBUG_STREAM_NAMED(name_, "No need to search since no free params/redundant joints"); + + std::vector ik_poses(1, ik_pose); + std::vector> solutions; + kinematics::KinematicsResult kinematic_result; + // Find all IK solutions within joint limits + if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options)) + { + ROS_DEBUG_STREAM_NAMED(name_, "No solution whatsoever"); + error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + return false; + } + + // sort solutions by their distance to the seed + std::vector solutions_obey_limits; + for (std::size_t i = 0; i < solutions.size(); ++i) + { + double dist_from_seed = 0.0; + for (std::size_t j = 0; j < ik_seed_state.size(); ++j) + { + dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]); + } + + 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) + { + 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 + { + solution = solutions_obey_limits[0].value; + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; // no collision check callback provided + } + } + + // ------------------------------------------------------------------------------------------------- + // Error Checking + if (!initialized_) + { + 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_) + { + 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_) + { + 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; + transformToChainFrame(ik_pose, frame); + + std::vector vfree(free_params_.size()); + + int counter = 0; + + double initial_guess = ik_seed_state[free_params_[0]]; + vfree[0] = initial_guess; + + // ------------------------------------------------------------------------------------------------- + // Handle consitency limits if needed + int num_positive_increments; + int num_negative_increments; + double search_discretization = redundant_joint_discretization_.at(free_params_[0]); + + 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]]); + + num_positive_increments = static_cast((max_limit - initial_guess) / search_discretization); + num_negative_increments = static_cast((initial_guess - min_limit) / search_discretization); + } + else // no consistency 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; + } + + // ------------------------------------------------------------------------------------------------- + // Begin searching + + 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) + { + IkSolutionList solutions; + size_t numsol = solve(frame, vfree, solutions); + + ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast"); + + if (numsol > 0) + { + for (size_t s = 0; s < numsol; ++s) + { + nattempts++; + std::vector sol; + getSolution(solutions, ik_seed_state, s, sol); + + bool obeys_limits = true; + for (size_t i = 0; i < sol.size(); i++) + { + if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) + { + obeys_limits = false; + break; + } + // ROS_INFO_STREAM_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) + { + 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()) + { + solution_callback(ik_pose, solution, error_code); + } + else + { + error_code.val = error_code.SUCCESS; + } + + if (error_code.val == error_code.SUCCESS) + { + 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)) + { + // Everything searched + error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + break; + } + + 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; + } + + // 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 +{ + ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK"); + + if (!initialized_) + { + ROS_ERROR_NAMED(name_, "kinematics not active"); + return false; + } + + if (ik_seed_state.size() < num_joints_) + { + ROS_ERROR_STREAM_NAMED(name_, "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(name_, "IK seed not in limits! " << static_cast(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) + { + int p = free_params_[i]; + ROS_ERROR("%u is %f", p, ik_seed_state[p]); // DTC + vfree[i] = ik_seed_state[p]; + } + + KDL::Frame frame; + transformToChainFrame(ik_pose, frame); + + IkSolutionList solutions; + size_t numsol = solve(frame, vfree, solutions); + ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast"); + + std::vector solutions_obey_limits; + + if (numsol) + { + std::vector solution_obey_limits; + for (std::size_t s = 0; s < numsol; ++s) + { + std::vector sol; + getSolution(solutions, ik_seed_state, s, sol); + ROS_DEBUG_NAMED(name_, "Sol %d: %e %e %e %e %e %e", static_cast(s), sol[0], sol[1], sol[2], sol[3], + sol[4], sol[5]); + + bool obeys_limits = true; + 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)))) + { + // One element of solution is not within limits + obeys_limits = false; + ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << static_cast(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 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(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 (!initialized_) + { + ROS_ERROR_NAMED(name_, "kinematics not active"); + result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE; + return false; + } + + if (ik_poses.empty()) + { + ROS_ERROR_NAMED(name_, "ik_poses is empty"); + result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES; + return false; + } + + if (ik_poses.size() > 1) + { + ROS_ERROR_NAMED(name_, "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_NAMED(name_, "ik_seed_state only has " << ik_seed_state.size() + << " entries, this ikfast solver requires " << num_joints_); + return false; + } + + KDL::Frame frame; + transformToChainFrame(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_NAMED(name_, "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 +{ + int index = redundant_joint_indices_.front(); + double joint_dscrt = redundant_joint_discretization_.at(index); + double joint_min = joint_min_vector_[index]; + double joint_max = joint_max_vector_[index]; + + switch (method) + { + case kinematics::DiscretizationMethods::ALL_DISCRETIZED: + { + size_t steps = std::ceil((joint_max - joint_min) / joint_dscrt); + for (size_t 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_NAMED(name_, "Discretization method " << method << " is not supported"); + return false; + } + + return true; +} + +void IKFastKinematicsPlugin::transformToChainFrame(const geometry_msgs::Pose& ik_pose, KDL::Frame& ik_pose_chain) const +{ + if (tip_transform_required_ || base_transform_required_) + { + Eigen::Isometry3d ik_eigen_pose; + tf2::fromMsg(ik_pose, ik_eigen_pose); + if (tip_transform_required_) + ik_eigen_pose = ik_eigen_pose * group_tip_to_chain_tip_; + + if (base_transform_required_) + ik_eigen_pose = chain_base_to_group_base_ * ik_eigen_pose; + + tf::transformEigenToKDL(ik_eigen_pose, ik_pose_chain); + } + else + { + tf2::fromMsg(ik_pose, ik_pose_chain); + } +} + +} // namespace motoman_mpx3500_ikfast_plugin + +// register IKFastKinematicsPlugin as a KinematicsBase implementation +#include +PLUGINLIB_EXPORT_CLASS(motoman_mpx3500_ikfast_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase); diff --git a/motoman_mpx3500_ikfast_plugin/src/motoman_mpx3500_ikfast_solver.cpp b/motoman_mpx3500_ikfast_plugin/src/motoman_mpx3500_ikfast_solver.cpp new file mode 100644 index 0000000..7d7adb3 --- /dev/null +++ b/motoman_mpx3500_ikfast_plugin/src/motoman_mpx3500_ikfast_solver.cpp @@ -0,0 +1,17846 @@ +/// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE +/// \author Rosen Diankov +/// +/// Licensed under the Apache License, Version 2.0 (the "License"); +/// you may not use this file except in compliance with the License. +/// You may obtain a copy of the License at +/// http://www.apache.org/licenses/LICENSE-2.0 +/// +/// Unless required by applicable law or agreed to in writing, software +/// distributed under the License is distributed on an "AS IS" BASIS, +/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +/// See the License for the specific language governing permissions and +/// limitations under the License. +/// +/// ikfast version 0x1000004a generated on 2021-05-06 22:37:03.870214 +/// Generated using solver transform6d +/// To compile with gcc: +/// gcc -lstdc++ ik.cpp +/// To compile without any main function as a shared object (might need -llapack): +/// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp +#define IKFAST_HAS_LIBRARY +#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h +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==0x1000004a); + +#include +#include +#include +#include +#include + +#ifndef IKFAST_ASSERT +#include +#include +#include + +#ifdef _MSC_VER +#ifndef __PRETTY_FUNCTION__ +#define __PRETTY_FUNCTION__ __FUNCDNAME__ +#endif +#endif + +#ifndef __PRETTY_FUNCTION__ +#define __PRETTY_FUNCTION__ __func__ +#endif + +#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } } + +#endif + +#if defined(_MSC_VER) +#define IKFAST_ALIGNED16(x) __declspec(align(16)) x +#else +#define IKFAST_ALIGNED16(x) x __attribute((aligned(16))) +#endif + +#define IK2PI ((IkReal)6.28318530717959) +#define IKPI ((IkReal)3.14159265358979) +#define IKPI_2 ((IkReal)1.57079632679490) + +#ifdef _MSC_VER +#ifndef isnan +#define isnan _isnan +#endif +#ifndef isinf +#define isinf _isinf +#endif +//#ifndef isfinite +//#define isfinite _isfinite +//#endif +#endif // _MSC_VER + +// lapack routines +extern "C" { + void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info); + void zgetrf_ (const int* m, const int* n, std::complex* a, const int* lda, int* ipiv, int* info); + void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info); + void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info); + void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info); + void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info); +} + +using namespace std; // necessary to get std math routines + +#ifdef IKFAST_NAMESPACE +namespace IKFAST_NAMESPACE { +#endif + +inline float IKabs(float f) { return fabsf(f); } +inline double IKabs(double f) { return fabs(f); } + +inline float IKsqr(float f) { return f*f; } +inline double IKsqr(double f) { return f*f; } + +inline float IKlog(float f) { return logf(f); } +inline double IKlog(double f) { return log(f); } + +// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation +#ifndef IKFAST_SINCOS_THRESH +#define IKFAST_SINCOS_THRESH ((IkReal)1e-7) +#endif + +// 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)1e-7) +#endif + +// minimum distance of separate solutions +#ifndef IKFAST_SOLUTION_THRESH +#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 +if( f <= -1 ) return float(-IKPI_2); +else if( f >= 1 ) return float(IKPI_2); +return asinf(f); +} +inline double IKasin(double f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return -IKPI_2; +else if( f >= 1 ) return IKPI_2; +return asin(f); +} + +// return positive value in [0,y) +inline float IKfmod(float x, float y) +{ + while(x < 0) { + x += y; + } + return fmodf(x,y); +} + +// return positive value in [0,y) +inline double IKfmod(double x, double y) +{ + while(x < 0) { + x += y; + } + return fmod(x,y); +} + +inline float IKacos(float f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return float(IKPI); +else if( f >= 1 ) return float(0); +return acosf(f); +} +inline double IKacos(double f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return IKPI; +else if( f >= 1 ) return 0; +return acos(f); +} +inline float IKsin(float f) { return sinf(f); } +inline double IKsin(double f) { return sin(f); } +inline float IKcos(float f) { return cosf(f); } +inline double IKcos(double f) { return cos(f); } +inline float IKtan(float f) { return tanf(f); } +inline double IKtan(double f) { return tan(f); } +inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); } +inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); } +inline float IKatan2Simple(float fy, float fx) { + return atan2f(fy,fx); +} +inline float IKatan2(float fy, float fx) { + if( isnan(fy) ) { + IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned + return float(IKPI_2); + } + 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( isnan(fy) ) { + IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned + return IKPI_2; + } + else if( isnan(fx) ) { + return 0; + } + return atan2(fy,fx); +} + +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); + } + else if( f < 0 ) { + return float(-1); + } + return 0; +} + +inline double IKsign(double f) { + if( f > 0 ) { + return 1.0; + } + else if( f < 0 ) { + return -1.0; + } + 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,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63,x64,x65,x66,x67,x68,x69,x70,x71,x72,x73,x74,x75,x76,x77,x78,x79,x80,x81,x82,x83,x84,x85,x86,x87,x88,x89,x90,x91,x92,x93,x94,x95,x96,x97,x98,x99; +x0=IKcos(j[0]); +x1=IKcos(j[3]); +x2=IKcos(j[2]); +x3=IKsin(j[1]); +x4=IKsin(j[2]); +x5=IKcos(j[1]); +x6=IKsin(j[0]); +x7=IKsin(j[3]); +x8=IKsin(j[4]); +x9=IKcos(j[4]); +x10=IKsin(j[5]); +x11=IKcos(j[5]); +x12=((1.0)*x9); +x13=((0.25)*x0); +x14=((1.0)*x1); +x15=((1.48775)*x4); +x16=((0.866025403784439)*x8); +x17=((0.5)*x7); +x18=((1.3)*x3); +x19=((0.106521124665486)*x8); +x20=((0.106521124665486)*x9); +x21=((0.433012701892219)*x1); +x22=((0.25)*x4); +x23=((0.75)*x1); +x24=((0.5)*x1); +x25=((0.151987458364169)*x1); +x26=((0.433012701892219)*x4); +x27=((1.0)*x2); +x28=((0.866025403784439)*x0); +x29=((1.0)*x8); +x30=((0.866025403784439)*x9); +x31=((0.5)*x2); +x32=((0.866025403784439)*x4); +x33=(x2*x5); +x34=(x0*x7); +x35=(x6*x7); +x36=(x3*x6); +x37=((-0.866025403784439)*x0); +x38=((-0.433012701892219)*x0); +x39=(x3*x4); +x40=(x0*x3); +x41=(x2*x3); +x42=(x1*x6); +x43=((-1.0)*x1); +x44=(x17*x6); +x45=((0.75)*x34); +x46=(x0*x17); +x47=((0.433012701892219)*x41); +x48=(x26*x5); +x49=(x32*x5); +x50=((0.866025403784439)*x41); +x51=((0.5)*x4*x5); +x52=((1.0)*x4*x5); +x53=(x0*x52); +x54=(x26*x40); +x55=((0.433012701892219)*x0*x33); +x56=(x28*x33); +x57=(x28*x39); +x58=(x52*x6); +x59=(x26*x36); +x60=((0.433012701892219)*x33*x6); +x61=((0.866025403784439)*x33*x6); +x62=(x32*x36); +x63=(((x31*x5))+(((0.5)*x39))); +x64=((((-1.0)*x39))+(((-1.0)*x27*x5))); +x65=(((x2*x40))+(((-1.0)*x53))); +x66=(((x2*x36))+(((-1.0)*x58))); +x67=(x64*x7); +x68=(x1*x63); +x69=((((-1.0)*x0*x51))+((x31*x40))); +x70=((((-1.0)*x27*x40))+x53); +x71=((((-1.0)*x51*x6))+((x31*x36))); +x72=((((-1.0)*x27*x36))+x58); +x73=(x57+x56); +x74=(x59+x60); +x75=(x1*x65); +x76=(x1*x66); +x77=(x23*x65); +x78=(x7*x70); +x79=(x23*x66); +x80=(x7*x72); +x81=(x1*x71); +x82=(x1*x69); +x83=(x24*x69); +x84=(x17*x70); +x85=((((-1.0)*x14*x6))+x78); +x86=(((x0*x1))+x80); +x87=(x46+x61+x62); +x88=(x49+x68+(((-1.0)*x50))); +x89=(x8*x85); +x90=(x86*x9); +x91=((((-1.0)*x49))+x50+(((-1.0)*x14*x63))); +x92=(x8*((((x0*x24))+((x17*x72))))); +x93=(x8*x91); +x94=(x55+x54+x83); +x95=((((-1.0)*x44))+x73+x82); +x96=(x81+x87); +x97=(x9*((x74+((x24*x71))+((x13*x7))))); +x98=((((-1.0)*x48))+((x9*(((((-1.0)*x47))+((x24*x63))+x48))))+((x1*(((((0.75)*x33))+(((0.866025403784439)*x3*x32))))))+x47+(((-1.0)*x63*x7*x8))); +x99=(x79+x45+x92+x97); +eerot[0]=(((x11*((((x8*(((((-1.0)*x24*x6))+x84))))+x77+(((-0.75)*x35))+((x9*(((((-0.25)*x35))+x94))))+(((-1.0)*x55))+(((-1.0)*x54))))))+((x10*((((x8*((((x37*x39))+x44+((x43*x69))+((x33*x37))))))+((x9*(((((-1.0)*x42))+x78))))))))); +eerot[1]=(((x10*((((x38*x39))+x77+((x8*(((((-0.5)*x42))+x84))))+(((-0.75)*x35))+((x9*(((((-0.25)*x35))+x94))))+((x33*x38))))))+(((-1.0)*x11*((((x29*((x44+(((-1.0)*x73))+(((-1.0)*x14*x69))))))+((x12*x85))))))); +eerot[2]=((((0.433012701892219)*x35))+((x30*x95))+((x16*x85))+((x13*x33))+((x13*x39))+(((-1.0)*x21*x65))); +eetrans[0]=((((1.48775)*x0*x33))+(((-1.0)*x25*x65))+((x0*x18))+((x15*x40))+((x20*x95))+((x19*x85))+(((0.151987458364169)*x35))); +eerot[3]=(((x10*((((x8*(((((-1.0)*x87))+(((-1.0)*x14*x71))))))+x90))))+((x11*(((((-0.433012701892219)*x33*x6))+(((-0.433012701892219)*x36*x4))+x99))))); +eerot[4]=((((-1.0)*x11*(((((1.0)*x8*(((((-0.5)*x34))+(((-0.866025403784439)*x33*x6))+((x43*x71))+(((-0.866025403784439)*x36*x4))))))+(((1.0)*x90))))))+((x10*((x99+(((-1.0)*x74))))))); +eerot[5]=(((x30*x96))+((x16*x86))+(((0.25)*x33*x6))+((x22*x36))+(((-0.433012701892219)*x34))+(((-1.0)*x21*x66))); +eetrans[1]=(((x18*x6))+(((-1.0)*x25*x66))+(((1.48775)*x33*x6))+((x15*x36))+((x20*x96))+((x19*x86))+(((-0.151987458364169)*x34))); +eerot[6]=(((x10*((((x67*x9))+x93))))+((x11*x98))); +eerot[7]=(((x11*(((((-1.0)*x29*x91))+(((-1.0)*x12*x67))))))+((x10*x98))); +eerot[8]=(((x30*x88))+(((-0.25)*x41))+((x22*x5))+((x16*x67))+((x1*(((((-0.433012701892219)*x33))+(((-1.0)*x26*x3))))))); +eetrans[2]=((0.8)+((x19*x67))+((x1*(((((-0.151987458364169)*x33))+(((-0.151987458364169)*x39))))))+((x20*x88))+(((1.3)*x5))+(((-1.48775)*x41))+((x15*x5))); +} + +IKFAST_API int GetNumFreeParameters() { return 0; } +IKFAST_API int* GetFreeParameters() { return NULL; } +IKFAST_API int GetNumJoints() { return 6; } + +IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } + +IKFAST_API int GetIkType() { return 0x67000001; } + +class IKSolver { +public: +IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp; +unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5; + +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; j4=numeric_limits::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; +for(int dummyiter = 0; dummyiter < 1; ++dummyiter) { + solutions.Clear(); +r00 = eerot[0*3+0]; +r01 = eerot[0*3+1]; +r02 = eerot[0*3+2]; +r10 = eerot[1*3+0]; +r11 = eerot[1*3+1]; +r12 = eerot[1*3+2]; +r20 = eerot[2*3+0]; +r21 = eerot[2*3+1]; +r22 = eerot[2*3+2]; +px = eetrans[0]; py = eetrans[1]; pz = eetrans[2]; + +new_r00=r00; +new_r01=((-1.0)*r01); +new_r02=((-1.0)*r02); +new_px=((((-0.18)*r02))+px); +new_r10=r10; +new_r11=((-1.0)*r11); +new_r12=((-1.0)*r12); +new_py=((((-0.18)*r12))+py); +new_r20=r20; +new_r21=((-1.0)*r21); +new_r22=((-1.0)*r22); +new_pz=((-0.8)+pz+(((-0.18)*r22))); +r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz; +IkReal x100=((1.0)*px); +IkReal x101=((1.0)*pz); +IkReal x102=((1.0)*py); +pp=((px*px)+(py*py)+(pz*pz)); +npx=(((px*r00))+((py*r10))+((pz*r20))); +npy=(((px*r01))+((py*r11))+((pz*r21))); +npz=(((px*r02))+((py*r12))+((pz*r22))); +rxp0_0=((((-1.0)*r20*x102))+((pz*r10))); +rxp0_1=(((px*r20))+(((-1.0)*r00*x101))); +rxp0_2=((((-1.0)*r10*x100))+((py*r00))); +rxp1_0=((((-1.0)*r21*x102))+((pz*r11))); +rxp1_1=(((px*r21))+(((-1.0)*r01*x101))); +rxp1_2=((((-1.0)*r11*x100))+((py*r01))); +rxp2_0=((((-1.0)*r22*x102))+((pz*r12))); +rxp2_1=(((px*r22))+(((-1.0)*r02*x101))); +rxp2_2=((((-1.0)*r12*x100))+((py*r02))); +IkReal op[72], zeror[48]; +int numroots;; +IkReal x103=((0.098726896031426)*r10); +IkReal x104=((((-1.0)*x103))+py); +IkReal x105=((2905339.26377693)*rxp2_1); +IkReal x106=((220642.405716096)*rxp1_0); +IkReal x107=((850338.412492541)*r00); +IkReal x108=((5032195.2180864)*rxp0_1); +IkReal x109=((290816.638236822)*r02); +IkReal x110=((286835.127430925)*r11); +IkReal x111=((3510990.75645658)*px); +IkReal x112=((290816.638236822)*r22); +IkReal x113=((3510990.75645658)*pz); +IkReal x114=((850338.412492541)*r20); +IkReal x115=((220642.405716096)*rxp1_2); +IkReal x116=((573670.25486185)*r10); +IkReal x117=((441284.811432192)*rxp0_0); +IkReal x118=((10064390.4361728)*rxp1_1); +IkReal x119=((1700676.82498508)*r01); +IkReal x120=((0.394907584125704)*r01); +IkReal x121=((0.197453792062852)*r00); +IkReal x122=((2.0)*px); +IkReal x123=((573670.25486185)*r01); +IkReal x124=((10064390.4361728)*rxp0_0); +IkReal x125=((1700676.82498508)*r10); +IkReal x126=((441284.811432192)*rxp1_1); +IkReal x127=((581633.276473644)*r12); +IkReal x128=((7021981.51291317)*py); +IkReal x129=((5810678.52755386)*rxp2_0); +IkReal x130=((882569.622864384)*rxp0_1); +IkReal x131=((20128780.8723456)*rxp1_0); +IkReal x132=((1147340.5097237)*r00); +IkReal x133=((3401353.64997016)*r11); +IkReal x134=((0.197453792062852)*r11); +IkReal x135=((1.0)*py); +IkReal x136=((3870919.398528)*npx); +IkReal x137=((1935459.699264)*pp); +IkReal x138=((7741838.797056)*npy); +IkReal x139=((2234876.35675148)*npz); +IkReal x140=((3870919.398528)*pp); +IkReal x141=((1117438.17837574)*pp); +IkReal x142=(x103+py); +IkReal x143=((-0.197453792062852)*r11); +IkReal x144=(px*x136); +IkReal x145=(r02*x141); +IkReal x146=(r00*x137); +IkReal x147=(px*x139); +IkReal x148=(r20*x137); +IkReal x149=(pz*x139); +IkReal x150=(pz*x136); +IkReal x151=(r22*x141); +IkReal x152=(r01*x140); +IkReal x153=(px*x138); +IkReal x154=((((-1.0)*x122))+x121); +IkReal x155=((2234876.35675148)*pp*r12); +IkReal x156=((7741838.797056)*npx*py); +IkReal x157=(r10*x140); +IkReal x158=((4469752.71350297)*npz*py); +IkReal x159=((7741838.797056)*pp*r11); +IkReal x160=((15483677.594112)*npy*py); +IkReal x161=(x103+(((-1.0)*x135))); +IkReal x162=((((-1.0)*x121))+(((-1.0)*x122))); +IkReal x163=((((-1.0)*x103))+(((-1.0)*x135))); +IkReal x164=(x110+x108); +IkReal x165=(x152+x117); +IkReal x166=(x159+x130); +IkReal x167=(x160+x133); +IkReal x168=(x153+x119); +IkReal x169=(x146+x105); +IkReal x170=(x155+x129); +IkReal x171=(x145+x146); +IkReal x172=(x165+x118); +IkReal x173=(x168+x116); +IkReal x174=(x168+x118); +IkReal x175=(x165+x116); +IkReal x176=(x144+x106+x107); +IkReal x177=(x164+x145); +IkReal x178=(x147+x111+x109); +IkReal x179=(x149+x113+x112); +IkReal x180=(x150+x115+x114); +IkReal x181=(x157+x124+x123); +IkReal x182=(x158+x127+x128); +IkReal x183=(x156+x126+x125); +IkReal x184=((((-1.0)*pz*x138))+(((-1700676.82498508)*r21))+((r21*x140))+(((441284.811432192)*rxp0_2))); +IkReal x185=(x129+x182); +IkReal x186=(x124+x123+x183); +IkReal x187=(x177+x169); +IkReal x188=(x176+x145+x105); +IkReal x189=(x178+x169); +IkReal x190=(x178+x176); +IkReal x191=(x178+x164+x146); +IkReal x192=(x177+x176); +IkReal x193=((((-1.0)*x148))+(((-1.0)*x151))+x179+x180); +IkReal x194=((((-1.0)*x151))+(((-1.0)*x180))+x179+x148); +IkReal x195=(x164+x105+x190); +op[0]=x142; +op[1]=x142; +op[2]=x143; +op[3]=x143; +op[4]=x104; +op[5]=x104; +op[6]=((((-1.0)*x190))+x187); +op[7]=x193; +op[8]=((((-1.0)*x172))+x173); +op[9]=x184; +op[10]=((((-1.0)*x191))+x188); +op[11]=x194; +op[12]=x193; +op[13]=((((-1.0)*x171))+x195); +op[14]=x184; +op[15]=((((-1.0)*x174))+x175); +op[16]=x194; +op[17]=((((-1.0)*x192))+x189); +op[18]=0; +op[19]=x142; +op[20]=0; +op[21]=x143; +op[22]=0; +op[23]=x104; +op[24]=x162; +op[25]=x162; +op[26]=x120; +op[27]=x120; +op[28]=x154; +op[29]=x154; +op[30]=((((-1.0)*x186))+(((-1.0)*x185))+x157+x155); +op[31]=0; +op[32]=((((-1.0)*x166))+x167+x131+(((-1.0)*x132))); +op[33]=0; +op[34]=((((-1.0)*x157))+(((-1.0)*x185))+x155+x186); +op[35]=0; +op[36]=0; +op[37]=((((-1.0)*x170))+(((-1.0)*x181))+x182+x183); +op[38]=0; +op[39]=((((-1.0)*x167))+x166+x131+(((-1.0)*x132))); +op[40]=0; +op[41]=((((-1.0)*x170))+(((-1.0)*x183))+x181+x182); +op[42]=0; +op[43]=x162; +op[44]=0; +op[45]=x120; +op[46]=0; +op[47]=x154; +op[48]=x163; +op[49]=x163; +op[50]=x134; +op[51]=x134; +op[52]=x161; +op[53]=x161; +op[54]=((((-1.0)*x187))+x190); +op[55]=x193; +op[56]=((((-1.0)*x173))+x172); +op[57]=x184; +op[58]=((((-1.0)*x188))+x191); +op[59]=x194; +op[60]=x193; +op[61]=((((-1.0)*x195))+x171); +op[62]=x184; +op[63]=((((-1.0)*x175))+x174); +op[64]=x194; +op[65]=((((-1.0)*x189))+x192); +op[66]=0; +op[67]=x163; +op[68]=0; +op[69]=x134; +op[70]=0; +op[71]=x161; +solvedialyticpoly8qep(op,zeror,numroots); +IkReal j0array[16], cj0array[16], sj0array[16], j5array[16], cj5array[16], sj5array[16], j1array[16], cj1array[16], sj1array[16]; +int numsolutions = 0; +for(int ij0 = 0; ij0 < numroots; ij0 += 3) +{ +IkReal htj0 = zeror[ij0+0], htj5 = zeror[ij0+1], htj1 = zeror[ij0+2]; +if(isnan(htj0)||isnan(htj5)||isnan(htj1)){ +continue; +} +j0array[numsolutions]=((2.0)*(atan(htj0))); +j5array[numsolutions]=((2.0)*(atan(htj5))); +j1array[numsolutions]=((2.0)*(atan(htj1))); +if(isinf(htj0)){ +cj0array[numsolutions] = IKcos(j0array[numsolutions]); +sj0array[numsolutions] = IKsin(j0array[numsolutions]); +} +else{ +IkReal x196=htj0*htj0; +CheckValue x197=IKPowWithIntegerCheck(((1.0)+x196),-1); +if(!x197.valid){ +continue; +} +cj0array[numsolutions]=((x197.value)*(((1.0)+(((-1.0)*x196))))); +CheckValue x198=IKPowWithIntegerCheck(((1.0)+(htj0*htj0)),-1); +if(!x198.valid){ +continue; +} +sj0array[numsolutions]=((2.0)*htj0*(x198.value)); +} +if(isinf(htj5)){ +cj5array[numsolutions] = IKcos(j5array[numsolutions]); +sj5array[numsolutions] = IKsin(j5array[numsolutions]); +} +else{ +IkReal x199=htj5*htj5; +CheckValue x200=IKPowWithIntegerCheck(((1.0)+x199),-1); +if(!x200.valid){ +continue; +} +cj5array[numsolutions]=((x200.value)*(((1.0)+(((-1.0)*x199))))); +CheckValue x201=IKPowWithIntegerCheck(((1.0)+(htj5*htj5)),-1); +if(!x201.valid){ +continue; +} +sj5array[numsolutions]=((2.0)*htj5*(x201.value)); +} +if(isinf(htj1)){ +cj1array[numsolutions] = IKcos(j1array[numsolutions]); +sj1array[numsolutions] = IKsin(j1array[numsolutions]); +} +else{ +IkReal x202=htj1*htj1; +CheckValue x203=IKPowWithIntegerCheck(((1.0)+x202),-1); +if(!x203.valid){ +continue; +} +cj1array[numsolutions]=((x203.value)*(((1.0)+(((-1.0)*x202))))); +CheckValue x204=IKPowWithIntegerCheck(((1.0)+(htj1*htj1)),-1); +if(!x204.valid){ +continue; +} +sj1array[numsolutions]=((2.0)*htj1*(x204.value)); +} +if( j0array[numsolutions] > IKPI ) +{ + j0array[numsolutions]-=IK2PI; +} +else if( j0array[numsolutions] < -IKPI ) +{ + j0array[numsolutions]+=IK2PI; +} +if( j5array[numsolutions] > IKPI ) +{ + j5array[numsolutions]-=IK2PI; +} +else if( j5array[numsolutions] < -IKPI ) +{ + j5array[numsolutions]+=IK2PI; +} +if( j1array[numsolutions] > IKPI ) +{ + j1array[numsolutions]-=IK2PI; +} +else if( j1array[numsolutions] < -IKPI ) +{ + j1array[numsolutions]+=IK2PI; +} +numsolutions++; +} +bool j0valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true}; +_nj0 = 16; +_nj5 = 1; +_nj1 = 1; +for(int ij0 = 0; ij0 < numsolutions; ++ij0) + { +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +_ij5[0] = 0; _ij5[1] = -1; +_ij1[0] = 0; _ij1[1] = -1; +for(int iij0 = ij0+1; iij0 < numsolutions; ++iij0) +{ +if( !j0valid[iij0] ) { continue; } +if( IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(cj5array[ij0]-cj5array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij0]-sj5array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(cj1array[ij0]-cj1array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij0]-sj1array[iij0]) < IKFAST_SOLUTION_THRESH && 1 ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; _ij5[1] = 0; _ij1[1] = 0; break; +} +} + j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + + j5 = j5array[ij0]; cj5 = cj5array[ij0]; sj5 = sj5array[ij0]; + + j1 = j1array[ij0]; cj1 = cj1array[ij0]; sj1 = sj1array[ij0]; + +innerfn(solutions); + } +} +return solutions.GetNumSolutions()>0; +} +inline void innerfn(IkSolutionListBase& solutions) { +for(int fniter = 0; fniter < 1; ++fniter) { +{ +IkReal j2array[1], cj2array[1], sj2array[1]; +bool j2valid[1]={false}; +_nj2 = 1; +IkReal x205=(r10*sj0); +IkReal x206=(cj0*r00); +IkReal x207=(r11*sj0); +IkReal x208=((0.0705192114510186)*cj1); +IkReal x209=((0.714285714285714)*sj1); +IkReal x210=(py*sj0); +IkReal x211=(cj0*px); +IkReal x212=((0.714285714285714)*cj1); +IkReal x213=(cj0*r01); +IkReal x214=((0.0705192114510186)*cj5*sj1); +IkReal x215=((0.0705192114510186)*sj1*sj5); +if( IKabs(((-0.928571428571429)+(((-1.0)*r21*sj5*x208))+((x206*x214))+((cj5*r20*x208))+((pz*x212))+(((-1.0)*x207*x215))+((x205*x214))+((x209*x210))+((x209*x211))+(((-1.0)*x213*x215)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*r20*x214))+((r21*x215))+(((-1.0)*sj5*x208*x213))+((cj5*x206*x208))+((x210*x212))+(((-1.0)*pz*x209))+((x211*x212))+(((-1.0)*sj5*x207*x208))+((cj5*x205*x208)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-0.928571428571429)+(((-1.0)*r21*sj5*x208))+((x206*x214))+((cj5*r20*x208))+((pz*x212))+(((-1.0)*x207*x215))+((x205*x214))+((x209*x210))+((x209*x211))+(((-1.0)*x213*x215))))+IKsqr(((((-1.0)*r20*x214))+((r21*x215))+(((-1.0)*sj5*x208*x213))+((cj5*x206*x208))+((x210*x212))+(((-1.0)*pz*x209))+((x211*x212))+(((-1.0)*sj5*x207*x208))+((cj5*x205*x208))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j2array[0]=IKatan2(((-0.928571428571429)+(((-1.0)*r21*sj5*x208))+((x206*x214))+((cj5*r20*x208))+((pz*x212))+(((-1.0)*x207*x215))+((x205*x214))+((x209*x210))+((x209*x211))+(((-1.0)*x213*x215))), ((((-1.0)*r20*x214))+((r21*x215))+(((-1.0)*sj5*x208*x213))+((cj5*x206*x208))+((x210*x212))+(((-1.0)*pz*x209))+((x211*x212))+(((-1.0)*sj5*x207*x208))+((cj5*x205*x208)))); +sj2array[0]=IKsin(j2array[0]); +cj2array[0]=IKcos(j2array[0]); +if( j2array[0] > IKPI ) +{ + j2array[0]-=IK2PI; +} +else if( j2array[0] < -IKPI ) +{ j2array[0]+=IK2PI; +} +j2valid[0] = true; +for(int ij2 = 0; ij2 < 1; ++ij2) +{ +if( !j2valid[ij2] ) +{ + continue; +} +_ij2[0] = ij2; _ij2[1] = -1; +for(int iij2 = ij2+1; iij2 < 1; ++iij2) +{ +if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH ) +{ + j2valid[iij2]=false; _ij2[1] = iij2; break; +} +} +j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; +{ +IkReal evalcond[2]; +IkReal x216=IKsin(j2); +IkReal x217=IKcos(j2); +IkReal x218=((0.098726896031426)*cj5); +IkReal x219=((1.4)*sj1); +IkReal x220=((0.098726896031426)*sj5); +IkReal x221=((1.4)*cj1); +evalcond[0]=((((-1.0)*x216*x221))+(((-1.3)*cj1))+(((-1.0)*r21*x220))+pz+((x217*x219))+((r20*x218))); +evalcond[1]=(((cj0*r01*x220))+((x216*x219))+(((1.3)*sj1))+(((-1.0)*r10*sj0*x218))+(((-1.0)*py*sj0))+((x217*x221))+(((-1.0)*cj0*r00*x218))+((r11*sj0*x220))+(((-1.0)*cj0*px))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j3eval[1]; +j3eval[0]=((((-1.0)*sj1*sj2))+(((-1.0)*cj1*cj2))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +j3eval[0]=(((cj2*sj1))+(((-1.0)*cj1*sj2))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j1), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j2), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x222=((1.0)*sj5); +if( IKabs(((((-1.0)*cj0*r11*x222))+((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj5*r20))+(((0.577350269189626)*r22))+(((-1.0)*r21*x222)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj0*r11*x222))+((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))))+IKsqr((((cj5*r20))+(((0.577350269189626)*r22))+(((-1.0)*r21*x222))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*cj0*r11*x222))+((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))), (((cj5*r20))+(((0.577350269189626)*r22))+(((-1.0)*r21*x222)))); +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[2]; +IkReal x223=((0.866025403784439)*sj5); +IkReal x224=((0.866025403784439)*cj5); +evalcond[0]=(((r20*x224))+(((-0.866025403784439)*(IKcos(j3))))+(((-1.0)*r21*x223))+(((0.5)*r22))); +evalcond[1]=((((-1.0)*cj0*r10*x224))+((cj0*r11*x223))+((r00*sj0*x224))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((-1.0)*r01*sj0*x223))+(((0.866025403784439)*(IKsin(j3))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x225=r20*r20; +IkReal x226=cj5*cj5; +IkReal x227=r21*r21; +IkReal x228=r22*r22; +IkReal x229=(cj5*r20); +IkReal x230=((0.5)*cj3); +IkReal x231=(r21*sj5); +IkReal x232=(r20*sj5); +IkReal x233=((2.0)*sj3); +IkReal x234=(cj5*r21); +IkReal x235=((1.73205080756888)*r22); +IkReal x236=((1.5)*x226); +IkReal x237=(r22*x231); +IkReal x238=((3.0)*x226); +j4eval[0]=((((-4.0)*x225))+(((-1.0)*x227*x238))+(((-3.0)*x228))+(((-3.46410161513775)*x237))+((x225*x238))+(((-6.0)*x229*x231))+(((3.46410161513775)*r22*x229))+(((-1.0)*x227))); +j4eval[1]=((IKabs((((cj3*x232))+((cj3*x234))+(((-1.0)*sj3*x231))+((sj3*x229))+(((-1.0)*sj3*x235)))))+(IKabs((((x232*x233))+((x233*x234))+((x230*x231))+(((0.866025403784439)*cj3*r22))+(((-1.0)*x229*x230)))))); +j4eval[2]=IKsign(((((-2.0)*x225))+((x229*x235))+(((-1.0)*x227*x236))+(((-1.5)*x228))+((x225*x236))+(((-1.0)*x231*x235))+(((-0.5)*x227))+(((-3.0)*x229*x231)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x239=((0.866025403784439)*sj5); +IkReal x240=(r01*sj0); +IkReal x241=(r02*sj0); +IkReal x242=(cj0*r11); +IkReal x243=(cj0*r12); +IkReal x244=((1.73205080756888)*sj5); +IkReal x245=(cj0*cj5*r10); +IkReal x246=(cj5*r00*sj0); +j4eval[0]=((((-1.0)*x242*x244))+((x240*x244))+(((-1.0)*x241))+x243+(((1.73205080756888)*x245))+(((-1.73205080756888)*x246))); +j4eval[1]=IKsign(((((0.866025403784439)*x245))+(((-0.866025403784439)*x246))+((x239*x240))+(((-1.0)*x239*x242))+(((-0.5)*x241))+(((0.5)*x243)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x247=(r12*sj0); +IkReal x248=(cj0*r02); +IkReal x249=(cj0*cj5*r00); +IkReal x250=(r11*sj0*sj5); +IkReal x251=(cj0*r01*sj5); +IkReal x252=(cj5*r10*sj0); +j4eval[0]=((((-1.0)*x248))+(((-1.0)*x247))+(((-1.73205080756888)*x252))+(((1.73205080756888)*x251))+(((1.73205080756888)*x250))+(((-1.73205080756888)*x249))); +j4eval[1]=IKsign(((((-0.866025403784439)*x252))+(((-0.866025403784439)*x249))+(((0.866025403784439)*x251))+(((0.866025403784439)*x250))+(((-0.5)*x248))+(((-0.5)*x247)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x253=((0.5)*sj0); +IkReal x254=((0.5)*cj5); +IkReal x255=(cj0*sj3); +IkReal x256=(cj3*r20); +IkReal x257=((1.0)*cj5); +IkReal x258=((1.0)*sj5); +IkReal x259=(cj5*sj3); +IkReal x260=((0.5)*sj5); +IkReal x261=(cj3*r21); +IkReal x262=(r01*sj5); +IkReal x263=((0.866025403784439)*cj0); +IkReal x264=((0.866025403784439)*sj0); +CheckValue x265 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+(((-1.0)*x256*x258))+(((-1.0)*r11*x255*x257))+(((-1.0)*r10*x255*x258))+((r01*sj0*x259))+(((-1.0)*x257*x261)))),IkReal(((((-0.866025403784439)*r12*x255))+(((-1.0)*r00*x253*x259))+((r10*x254*x255))+(((-0.866025403784439)*cj3*r22))+((sj3*x253*x262))+((x254*x256))+((r02*sj3*x264))+(((-1.0)*x260*x261))+(((-1.0)*r11*x255*x260)))),IKFAST_ATAN2_MAGTHRESH); +if(!x265.valid){ +continue; +} +CheckValue x266=IKPowWithIntegerCheck(IKsign(((((-0.5)*cj0*r02))+((x262*x263))+((r11*sj5*x264))+(((-1.0)*cj5*r10*x264))+(((-1.0)*cj5*r00*x263))+(((-1.0)*r12*x253)))),-1); +if(!x266.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x265.value)+(((1.5707963267949)*(x266.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x267=IKcos(j4); +IkReal x268=IKsin(j4); +IkReal x269=(r21*sj5); +IkReal x270=(cj5*sj0); +IkReal x271=(sj0*sj5); +IkReal x272=(cj0*r01); +IkReal x273=(cj5*r20); +IkReal x274=(r20*sj5); +IkReal x275=(cj0*sj5); +IkReal x276=(r02*sj0); +IkReal x277=(cj0*r12); +IkReal x278=(cj0*r02); +IkReal x279=(r12*sj0); +IkReal x280=(cj0*cj5); +IkReal x281=((0.5)*x268); +IkReal x282=((1.0)*x267); +IkReal x283=((0.5)*x267); +IkReal x284=(r10*x268); +IkReal x285=(cj5*x268); +IkReal x286=((1.0)*x268); +IkReal x287=((0.866025403784439)*x268); +IkReal x288=((0.866025403784439)*x267); +evalcond[0]=(sj3+(((-1.0)*r22*x287))+((x273*x281))+((cj5*r21*x267))+(((-1.0)*x269*x281))+((x267*x274))); +evalcond[1]=((((-1.0)*r22*x288))+((x273*x283))+(((-1.0)*r21*x285))+(((-1.0)*x269*x283))+(((-0.5)*cj3))+(((-1.0)*x274*x286))); +evalcond[2]=((((-1.0)*r11*x280*x282))+cj3+((r00*x267*x271))+(((-1.0)*x276*x287))+(((-1.0)*r10*x275*x282))+((r01*x267*x270))+((r11*x275*x281))+(((-1.0)*r01*x271*x281))+((r00*x270*x281))+(((-1.0)*r10*x280*x281))+((x277*x287))); +evalcond[3]=(((x279*x287))+((x278*x287))+(((-1.0)*r10*x270*x281))+(((-1.0)*r10*x271*x282))+(((-1.0)*cj5*x272*x282))+((r11*x271*x281))+(((-1.0)*r11*x270*x282))+((sj5*x272*x281))+(((-1.0)*r00*x280*x281))+(((-1.0)*r00*x275*x282))); +evalcond[4]=((((-1.0)*r01*x270*x286))+(((-1.0)*x276*x288))+(((0.5)*sj3))+((r11*x275*x283))+(((-1.0)*r00*x271*x286))+((x275*x284))+(((-1.0)*r01*x271*x283))+((r00*x270*x283))+(((-1.0)*r10*x280*x283))+((x277*x288))+((r11*x268*x280))); +evalcond[5]=((0.866025403784439)+((x279*x288))+((r11*x268*x270))+((x278*x288))+(((-1.0)*r10*x270*x283))+((x271*x284))+((r11*x271*x283))+((x272*x285))+((sj5*x272*x283))+(((-1.0)*r00*x280*x283))+((r00*x268*x275))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x289=((0.866025403784439)*sj5); +IkReal x290=((1.0)*sj3); +IkReal x291=(cj0*sj5); +IkReal x292=((0.5)*sj0); +IkReal x293=(cj5*sj0); +IkReal x294=((0.866025403784439)*cj0); +IkReal x295=(cj5*r10); +IkReal x296=((0.5)*sj3); +IkReal x297=(cj0*cj5); +CheckValue x298 = IKatan2WithCheck(IkReal(((((-1.0)*r11*x290*x293))+(((-1.0)*r00*x290*x291))+(((-1.0)*r01*x290*x297))+(((-1.0)*r10*sj0*sj5*x290)))),IkReal(((((-0.866025403784439)*r12*sj0*sj3))+((sj3*x292*x295))+((r00*x296*x297))+(((-1.0)*r01*x291*x296))+(((-1.0)*r11*sj3*sj5*x292))+(((-1.0)*r02*sj3*x294)))),IKFAST_ATAN2_MAGTHRESH); +if(!x298.valid){ +continue; +} +CheckValue x299=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj0*r11*x289))+(((-0.866025403784439)*r00*x293))+((r01*sj0*x289))+(((-1.0)*r02*x292))+((x294*x295))+(((0.5)*cj0*r12)))),-1); +if(!x299.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x298.value)+(((1.5707963267949)*(x299.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x300=IKcos(j4); +IkReal x301=IKsin(j4); +IkReal x302=(r21*sj5); +IkReal x303=(cj5*sj0); +IkReal x304=(sj0*sj5); +IkReal x305=(cj0*r01); +IkReal x306=(cj5*r20); +IkReal x307=(r20*sj5); +IkReal x308=(cj0*sj5); +IkReal x309=(r02*sj0); +IkReal x310=(cj0*r12); +IkReal x311=(cj0*r02); +IkReal x312=(r12*sj0); +IkReal x313=(cj0*cj5); +IkReal x314=((0.5)*x301); +IkReal x315=((1.0)*x300); +IkReal x316=((0.5)*x300); +IkReal x317=(r10*x301); +IkReal x318=(cj5*x301); +IkReal x319=((1.0)*x301); +IkReal x320=((0.866025403784439)*x301); +IkReal x321=((0.866025403784439)*x300); +evalcond[0]=(sj3+((cj5*r21*x300))+(((-1.0)*r22*x320))+(((-1.0)*x302*x314))+((x306*x314))+((x300*x307))); +evalcond[1]=((((-1.0)*r22*x321))+(((-1.0)*x302*x316))+((x306*x316))+(((-1.0)*x307*x319))+(((-0.5)*cj3))+(((-1.0)*r21*x318))); +evalcond[2]=((((-1.0)*r10*x313*x314))+((r00*x303*x314))+((r11*x308*x314))+cj3+(((-1.0)*x309*x320))+((x310*x320))+(((-1.0)*r11*x313*x315))+(((-1.0)*r01*x304*x314))+((r01*x300*x303))+((r00*x300*x304))+(((-1.0)*r10*x308*x315))); +evalcond[3]=((((-1.0)*r00*x308*x315))+(((-1.0)*r10*x304*x315))+((x312*x320))+((sj5*x305*x314))+(((-1.0)*r10*x303*x314))+(((-1.0)*cj5*x305*x315))+(((-1.0)*r00*x313*x314))+((x311*x320))+((r11*x304*x314))+(((-1.0)*r11*x303*x315))); +evalcond[4]=((((-1.0)*r00*x304*x319))+(((-1.0)*r10*x313*x316))+((r00*x303*x316))+((r11*x308*x316))+(((-1.0)*r01*x303*x319))+((r11*x301*x313))+(((-1.0)*x309*x321))+((x310*x321))+(((0.5)*sj3))+((x308*x317))+(((-1.0)*r01*x304*x316))); +evalcond[5]=((0.866025403784439)+((x305*x318))+((x304*x317))+((r11*x301*x303))+((x312*x321))+((sj5*x305*x316))+(((-1.0)*r10*x303*x316))+(((-1.0)*r00*x313*x316))+((r00*x301*x308))+((x311*x321))+((r11*x304*x316))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x322=cj5*cj5; +IkReal x323=r20*r20; +IkReal x324=r21*r21; +IkReal x325=(r21*sj5); +IkReal x326=(cj3*cj5); +IkReal x327=(r20*sj5); +IkReal x328=(cj5*sj3); +IkReal x329=(cj5*r20); +IkReal x330=((1.73205080756888)*r22); +IkReal x331=((1.5)*x322); +CheckValue x332 = IKatan2WithCheck(IkReal(((((-1.0)*sj3*x330))+((r20*x328))+(((-1.0)*sj3*x325))+((r21*x326))+((cj3*x327)))),IkReal(((((2.0)*r21*x328))+(((0.5)*cj3*x325))+(((-0.5)*r20*x326))+(((0.866025403784439)*cj3*r22))+(((2.0)*sj3*x327)))),IKFAST_ATAN2_MAGTHRESH); +if(!x332.valid){ +continue; +} +CheckValue x333=IKPowWithIntegerCheck(IKsign(((((-1.5)*(r22*r22)))+(((-2.0)*x323))+(((-1.0)*x324*x331))+(((-0.5)*x324))+((x329*x330))+(((-3.0)*x325*x329))+((x323*x331))+(((-1.0)*x325*x330)))),-1); +if(!x333.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x332.value)+(((1.5707963267949)*(x333.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x334=IKcos(j4); +IkReal x335=IKsin(j4); +IkReal x336=(r21*sj5); +IkReal x337=(cj5*sj0); +IkReal x338=(sj0*sj5); +IkReal x339=(cj0*r01); +IkReal x340=(cj5*r20); +IkReal x341=(r20*sj5); +IkReal x342=(cj0*sj5); +IkReal x343=(r02*sj0); +IkReal x344=(cj0*r12); +IkReal x345=(cj0*r02); +IkReal x346=(r12*sj0); +IkReal x347=(cj0*cj5); +IkReal x348=((0.5)*x335); +IkReal x349=((1.0)*x334); +IkReal x350=((0.5)*x334); +IkReal x351=(r10*x335); +IkReal x352=(cj5*x335); +IkReal x353=((1.0)*x335); +IkReal x354=((0.866025403784439)*x335); +IkReal x355=((0.866025403784439)*x334); +evalcond[0]=(sj3+((cj5*r21*x334))+(((-1.0)*r22*x354))+(((-1.0)*x336*x348))+((x340*x348))+((x334*x341))); +evalcond[1]=((((-1.0)*r22*x355))+(((-1.0)*x341*x353))+(((-1.0)*r21*x352))+((x340*x350))+(((-1.0)*x336*x350))+(((-0.5)*cj3))); +evalcond[2]=(((r00*x337*x348))+(((-1.0)*r10*x347*x348))+(((-1.0)*r01*x338*x348))+cj3+(((-1.0)*r11*x347*x349))+(((-1.0)*r10*x342*x349))+((x344*x354))+((r00*x334*x338))+(((-1.0)*x343*x354))+((r11*x342*x348))+((r01*x334*x337))); +evalcond[3]=((((-1.0)*cj5*x339*x349))+(((-1.0)*r00*x347*x348))+((x345*x354))+((x346*x354))+(((-1.0)*r11*x337*x349))+(((-1.0)*r10*x338*x349))+((sj5*x339*x348))+((r11*x338*x348))+(((-1.0)*r00*x342*x349))+(((-1.0)*r10*x337*x348))); +evalcond[4]=(((r00*x337*x350))+(((-1.0)*r00*x338*x353))+(((0.5)*sj3))+(((-1.0)*r01*x338*x350))+((x344*x355))+(((-1.0)*x343*x355))+((x342*x351))+(((-1.0)*r01*x337*x353))+(((-1.0)*r10*x347*x350))+((r11*x342*x350))+((r11*x335*x347))); +evalcond[5]=((0.866025403784439)+((sj5*x339*x350))+((x338*x351))+((x345*x355))+(((-1.0)*r00*x347*x350))+(((-1.0)*r10*x337*x350))+((x346*x355))+((r11*x338*x350))+((r11*x335*x337))+((x339*x352))+((r00*x335*x342))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(j2, 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x356=((1.0)*cj5); +if( IKabs((((cj0*cj5*r10))+(((-1.0)*r00*sj0*x356))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x356)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-1.0)*r00*sj0*x356))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))))+IKsqr(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x356))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-1.0)*r00*sj0*x356))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))), ((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x356)))); +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[2]; +IkReal x357=((0.866025403784439)*sj5); +IkReal x358=((0.866025403784439)*cj5); +evalcond[0]=((((0.5)*r22))+(((0.866025403784439)*(IKcos(j3))))+(((-1.0)*r21*x357))+((r20*x358))); +evalcond[1]=(((cj0*r11*x357))+(((-1.0)*r01*sj0*x357))+(((-1.0)*cj0*r10*x358))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((0.866025403784439)*(IKsin(j3))))+((r00*sj0*x358))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x359=r20*r20; +IkReal x360=cj5*cj5; +IkReal x361=r21*r21; +IkReal x362=r22*r22; +IkReal x363=(cj5*r20); +IkReal x364=((0.5)*cj3); +IkReal x365=(r21*sj5); +IkReal x366=((3.46410161513775)*r22); +IkReal x367=((1.73205080756888)*r22); +IkReal x368=(r20*sj5); +IkReal x369=((1.0)*cj3); +IkReal x370=((2.0)*sj3); +IkReal x371=(cj5*r21); +IkReal x372=((1.5)*x360); +IkReal x373=((3.0)*x360); +j4eval[0]=((((-1.0)*x361))+(((-1.0)*x361*x373))+(((-3.0)*x362))+(((-4.0)*x359))+((x359*x373))+(((-6.0)*x363*x365))+(((-1.0)*x365*x366))+((x363*x366))); +j4eval[1]=((IKabs(((((-1.0)*x368*x370))+(((-1.0)*x370*x371))+(((-0.866025403784439)*cj3*r22))+(((-1.0)*x364*x365))+((x363*x364)))))+(IKabs(((((-1.0)*sj3*x363))+((sj3*x367))+((sj3*x365))+(((-1.0)*x369*x371))+(((-1.0)*x368*x369)))))); +j4eval[2]=IKsign(((((-2.0)*x359))+(((-1.0)*x361*x372))+(((-1.5)*x362))+(((-0.5)*x361))+((x359*x372))+(((-3.0)*x363*x365))+(((-1.0)*x365*x367))+((x363*x367)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x374=(cj0*r12); +IkReal x375=((0.866025403784439)*sj5); +IkReal x376=(r01*sj0); +IkReal x377=(r02*sj0); +IkReal x378=(cj0*r11); +IkReal x379=((1.73205080756888)*sj5); +IkReal x380=(cj0*cj5*r10); +IkReal x381=(cj5*r00*sj0); +j4eval[0]=((((-1.0)*x374))+(((1.73205080756888)*x381))+(((-1.73205080756888)*x380))+x377+((x378*x379))+(((-1.0)*x376*x379))); +j4eval[1]=IKsign((((x375*x378))+(((-0.5)*x374))+(((0.866025403784439)*x381))+(((-0.866025403784439)*x380))+(((-1.0)*x375*x376))+(((0.5)*x377)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x382=(r12*sj0); +IkReal x383=(cj0*r02); +IkReal x384=(cj0*cj5*r00); +IkReal x385=(r11*sj0*sj5); +IkReal x386=(cj0*r01*sj5); +IkReal x387=(cj5*r10*sj0); +j4eval[0]=((((1.73205080756888)*x384))+(((1.73205080756888)*x387))+(((-1.73205080756888)*x385))+(((-1.73205080756888)*x386))+x382+x383); +j4eval[1]=IKsign(((((0.5)*x383))+(((0.5)*x382))+(((0.866025403784439)*x384))+(((0.866025403784439)*x387))+(((-0.866025403784439)*x385))+(((-0.866025403784439)*x386)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x388=((0.5)*sj0); +IkReal x389=((0.5)*cj5); +IkReal x390=(cj0*sj3); +IkReal x391=(cj3*r20); +IkReal x392=(cj5*sj3); +IkReal x393=((0.5)*sj5); +IkReal x394=(cj3*r21); +IkReal x395=((0.866025403784439)*cj0); +IkReal x396=(r01*sj5); +IkReal x397=((0.866025403784439)*sj0); +CheckValue x398 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+(((-1.0)*cj5*r11*x390))+((cj5*x394))+((sj5*x391))+((r01*sj0*x392))+(((-1.0)*r10*sj5*x390)))),IkReal(((((-0.866025403784439)*r12*x390))+(((-1.0)*r00*x388*x392))+(((-1.0)*x389*x391))+((r02*sj3*x397))+(((0.866025403784439)*cj3*r22))+((sj3*x388*x396))+((x393*x394))+((r10*x389*x390))+(((-1.0)*r11*x390*x393)))),IKFAST_ATAN2_MAGTHRESH); +if(!x398.valid){ +continue; +} +CheckValue x399=IKPowWithIntegerCheck(IKsign((((cj5*r10*x397))+((r12*x388))+(((-1.0)*x395*x396))+(((0.5)*cj0*r02))+(((-1.0)*r11*sj5*x397))+((cj5*r00*x395)))),-1); +if(!x399.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x398.value)+(((1.5707963267949)*(x399.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x400=IKcos(j4); +IkReal x401=IKsin(j4); +IkReal x402=(r21*sj5); +IkReal x403=(cj5*sj0); +IkReal x404=(sj0*sj5); +IkReal x405=(cj0*r01); +IkReal x406=(cj5*r20); +IkReal x407=(r20*sj5); +IkReal x408=(cj0*sj5); +IkReal x409=(r02*sj0); +IkReal x410=(cj0*r12); +IkReal x411=(cj0*r02); +IkReal x412=(r12*sj0); +IkReal x413=(cj0*cj5); +IkReal x414=((0.5)*x401); +IkReal x415=((1.0)*x400); +IkReal x416=((0.5)*x400); +IkReal x417=(r10*x401); +IkReal x418=(cj5*x401); +IkReal x419=((1.0)*x401); +IkReal x420=((0.866025403784439)*x401); +IkReal x421=((0.866025403784439)*x400); +evalcond[0]=((((-1.0)*sj3))+(((-1.0)*x402*x414))+((x406*x414))+((x400*x407))+((cj5*r21*x400))+(((-1.0)*r22*x420))); +evalcond[1]=((((-1.0)*x402*x416))+((x406*x416))+(((0.5)*cj3))+(((-1.0)*r21*x418))+(((-1.0)*x407*x419))+(((-1.0)*r22*x421))); +evalcond[2]=((((-1.0)*r11*x413*x415))+cj3+((x410*x420))+(((-1.0)*r01*x404*x414))+(((-1.0)*r10*x408*x415))+((r00*x400*x404))+((r00*x403*x414))+(((-1.0)*x409*x420))+((r01*x400*x403))+((r11*x408*x414))+(((-1.0)*r10*x413*x414))); +evalcond[3]=((((-1.0)*r00*x413*x414))+(((-1.0)*r10*x404*x415))+((x411*x420))+(((-1.0)*r11*x403*x415))+((sj5*x405*x414))+((x412*x420))+(((-1.0)*r00*x408*x415))+((r11*x404*x414))+(((-1.0)*r10*x403*x414))+(((-1.0)*cj5*x405*x415))); +evalcond[4]=(((x410*x421))+(((-1.0)*r01*x404*x416))+(((-1.0)*r00*x404*x419))+(((0.5)*sj3))+((x408*x417))+((r00*x403*x416))+((r11*x401*x413))+(((-1.0)*x409*x421))+((r11*x408*x416))+(((-1.0)*r01*x403*x419))+(((-1.0)*r10*x413*x416))); +evalcond[5]=((-0.866025403784439)+((x404*x417))+((x405*x418))+(((-1.0)*r00*x413*x416))+((x411*x421))+((sj5*x405*x416))+((x412*x421))+((r11*x401*x403))+((r00*x401*x408))+((r11*x404*x416))+(((-1.0)*r10*x403*x416))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x422=((0.866025403784439)*sj5); +IkReal x423=((0.5)*sj0); +IkReal x424=(r11*sj3); +IkReal x425=((1.0)*sj0); +IkReal x426=((0.866025403784439)*cj0); +IkReal x427=(cj5*r10); +IkReal x428=((0.5)*cj0); +IkReal x429=(sj3*sj5); +IkReal x430=((1.0)*cj0); +IkReal x431=(cj5*sj3); +IkReal x432=((0.866025403784439)*sj0); +CheckValue x433 = IKatan2WithCheck(IkReal(((((-1.0)*r00*x429*x430))+(((-1.0)*r10*x425*x429))+(((-1.0)*r01*x430*x431))+(((-1.0)*cj5*x424*x425)))),IkReal((((r00*x428*x431))+(((-1.0)*r02*sj3*x426))+(((-1.0)*sj5*x423*x424))+(((-1.0)*r12*sj3*x432))+((sj3*x423*x427))+(((-1.0)*r01*x428*x429)))),IKFAST_ATAN2_MAGTHRESH); +if(!x433.valid){ +continue; +} +CheckValue x434=IKPowWithIntegerCheck(IKsign(((((-1.0)*r12*x428))+((r02*x423))+(((-1.0)*r01*sj0*x422))+((cj5*r00*x432))+(((-1.0)*x426*x427))+((cj0*r11*x422)))),-1); +if(!x434.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x433.value)+(((1.5707963267949)*(x434.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x435=IKcos(j4); +IkReal x436=IKsin(j4); +IkReal x437=(r21*sj5); +IkReal x438=(cj5*sj0); +IkReal x439=(sj0*sj5); +IkReal x440=(cj0*r01); +IkReal x441=(cj5*r20); +IkReal x442=(r20*sj5); +IkReal x443=(cj0*sj5); +IkReal x444=(r02*sj0); +IkReal x445=(cj0*r12); +IkReal x446=(cj0*r02); +IkReal x447=(r12*sj0); +IkReal x448=(cj0*cj5); +IkReal x449=((0.5)*x436); +IkReal x450=((1.0)*x435); +IkReal x451=((0.5)*x435); +IkReal x452=(r10*x436); +IkReal x453=(cj5*x436); +IkReal x454=((1.0)*x436); +IkReal x455=((0.866025403784439)*x436); +IkReal x456=((0.866025403784439)*x435); +evalcond[0]=(((x435*x442))+(((-1.0)*x437*x449))+(((-1.0)*sj3))+((x441*x449))+(((-1.0)*r22*x455))+((cj5*r21*x435))); +evalcond[1]=(((x441*x451))+(((-1.0)*x437*x451))+(((-1.0)*r21*x453))+(((-1.0)*x442*x454))+(((0.5)*cj3))+(((-1.0)*r22*x456))); +evalcond[2]=(((r00*x438*x449))+((r00*x435*x439))+(((-1.0)*r01*x439*x449))+cj3+((r11*x443*x449))+(((-1.0)*r10*x443*x450))+(((-1.0)*r11*x448*x450))+((r01*x435*x438))+(((-1.0)*r10*x448*x449))+((x445*x455))+(((-1.0)*x444*x455))); +evalcond[3]=((((-1.0)*cj5*x440*x450))+((sj5*x440*x449))+(((-1.0)*r10*x439*x450))+(((-1.0)*r10*x438*x449))+(((-1.0)*r00*x443*x450))+(((-1.0)*r00*x448*x449))+((x447*x455))+((x446*x455))+((r11*x439*x449))+(((-1.0)*r11*x438*x450))); +evalcond[4]=(((r00*x438*x451))+(((-1.0)*r10*x448*x451))+(((-1.0)*r00*x439*x454))+(((0.5)*sj3))+((r11*x436*x448))+((r11*x443*x451))+((x443*x452))+((x445*x456))+(((-1.0)*r01*x438*x454))+(((-1.0)*x444*x456))+(((-1.0)*r01*x439*x451))); +evalcond[5]=((-0.866025403784439)+((r00*x436*x443))+((x440*x453))+((sj5*x440*x451))+((x447*x456))+((x446*x456))+((r11*x436*x438))+((r11*x439*x451))+(((-1.0)*r00*x448*x451))+((x439*x452))+(((-1.0)*r10*x438*x451))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x457=cj5*cj5; +IkReal x458=r20*r20; +IkReal x459=r21*r21; +IkReal x460=((1.0)*cj5); +IkReal x461=(r20*sj3); +IkReal x462=(cj3*r21); +IkReal x463=(cj5*r20); +IkReal x464=(r21*sj3); +IkReal x465=(r21*sj5); +IkReal x466=((1.73205080756888)*r22); +IkReal x467=((1.5)*x457); +CheckValue x468 = IKatan2WithCheck(IkReal((((sj3*x466))+(((-1.0)*x460*x461))+(((-1.0)*x460*x462))+(((-1.0)*cj3*r20*sj5))+((sj5*x464)))),IkReal(((((-0.5)*sj5*x462))+(((-2.0)*cj5*x464))+(((-0.866025403784439)*cj3*r22))+(((-2.0)*sj5*x461))+(((0.5)*cj3*x463)))),IKFAST_ATAN2_MAGTHRESH); +if(!x468.valid){ +continue; +} +CheckValue x469=IKPowWithIntegerCheck(IKsign(((((-1.0)*x465*x466))+(((-1.5)*(r22*r22)))+(((-0.5)*x459))+((x463*x466))+(((-2.0)*x458))+(((-1.0)*x459*x467))+(((-3.0)*x463*x465))+((x458*x467)))),-1); +if(!x469.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x468.value)+(((1.5707963267949)*(x469.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x470=IKcos(j4); +IkReal x471=IKsin(j4); +IkReal x472=(r21*sj5); +IkReal x473=(cj5*sj0); +IkReal x474=(sj0*sj5); +IkReal x475=(cj0*r01); +IkReal x476=(cj5*r20); +IkReal x477=(r20*sj5); +IkReal x478=(cj0*sj5); +IkReal x479=(r02*sj0); +IkReal x480=(cj0*r12); +IkReal x481=(cj0*r02); +IkReal x482=(r12*sj0); +IkReal x483=(cj0*cj5); +IkReal x484=((0.5)*x471); +IkReal x485=((1.0)*x470); +IkReal x486=((0.5)*x470); +IkReal x487=(r10*x471); +IkReal x488=(cj5*x471); +IkReal x489=((1.0)*x471); +IkReal x490=((0.866025403784439)*x471); +IkReal x491=((0.866025403784439)*x470); +evalcond[0]=((((-1.0)*sj3))+((x476*x484))+((x470*x477))+((cj5*r21*x470))+(((-1.0)*r22*x490))+(((-1.0)*x472*x484))); +evalcond[1]=(((x476*x486))+(((0.5)*cj3))+(((-1.0)*r21*x488))+(((-1.0)*r22*x491))+(((-1.0)*x477*x489))+(((-1.0)*x472*x486))); +evalcond[2]=((((-1.0)*r10*x483*x484))+((r11*x478*x484))+(((-1.0)*r01*x474*x484))+cj3+(((-1.0)*r10*x478*x485))+((r00*x473*x484))+(((-1.0)*r11*x483*x485))+((r01*x470*x473))+((x480*x490))+(((-1.0)*x479*x490))+((r00*x470*x474))); +evalcond[3]=((((-1.0)*r11*x473*x485))+(((-1.0)*cj5*x475*x485))+((r11*x474*x484))+((x481*x490))+((sj5*x475*x484))+(((-1.0)*r00*x483*x484))+(((-1.0)*r00*x478*x485))+(((-1.0)*r10*x473*x484))+((x482*x490))+(((-1.0)*r10*x474*x485))); +evalcond[4]=((((-1.0)*r10*x483*x486))+((r11*x478*x486))+((r11*x471*x483))+(((-1.0)*r01*x474*x486))+(((0.5)*sj3))+((r00*x473*x486))+(((-1.0)*r00*x474*x489))+((x480*x491))+((x478*x487))+(((-1.0)*x479*x491))+(((-1.0)*r01*x473*x489))); +evalcond[5]=((-0.866025403784439)+((x475*x488))+((r11*x471*x473))+((x474*x487))+((r11*x474*x486))+((x481*x491))+((sj5*x475*x486))+((r00*x471*x478))+(((-1.0)*r00*x483*x486))+(((-1.0)*r10*x473*x486))+((x482*x491))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x492=((1.0)*sj5); +if( IKabs((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*cj0*r11*x492))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*r21*x492))+((cj5*r20))+(((0.577350269189626)*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*cj0*r11*x492))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))))+IKsqr(((((-1.0)*r21*x492))+((cj5*r20))+(((0.577350269189626)*r22))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*cj0*r11*x492))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))), ((((-1.0)*r21*x492))+((cj5*r20))+(((0.577350269189626)*r22)))); +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[2]; +IkReal x493=((0.866025403784439)*sj5); +IkReal x494=((0.866025403784439)*cj5); +evalcond[0]=((((-1.0)*r21*x493))+(((-0.866025403784439)*(IKcos(j3))))+(((0.5)*r22))+((r20*x494))); +evalcond[1]=((((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((0.866025403784439)*(IKsin(j3))))+((cj0*r11*x493))+(((-1.0)*r01*sj0*x493))+((r00*sj0*x494))+(((-1.0)*cj0*r10*x494))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x495=r20*r20; +IkReal x496=cj5*cj5; +IkReal x497=r21*r21; +IkReal x498=r22*r22; +IkReal x499=(cj5*r20); +IkReal x500=((0.5)*cj3); +IkReal x501=(r21*sj5); +IkReal x502=(r20*sj5); +IkReal x503=((2.0)*sj3); +IkReal x504=(cj5*r21); +IkReal x505=((1.73205080756888)*r22); +IkReal x506=((1.5)*x496); +IkReal x507=(r22*x501); +IkReal x508=((3.0)*x496); +j4eval[0]=((((-1.0)*x497*x508))+(((-3.46410161513775)*x507))+((x495*x508))+(((3.46410161513775)*r22*x499))+(((-3.0)*x498))+(((-6.0)*x499*x501))+(((-1.0)*x497))+(((-4.0)*x495))); +j4eval[1]=((IKabs((((x500*x501))+(((-1.0)*x499*x500))+((x503*x504))+(((0.866025403784439)*cj3*r22))+((x502*x503)))))+(IKabs(((((-1.0)*sj3*x505))+((cj3*x502))+((cj3*x504))+(((-1.0)*sj3*x501))+((sj3*x499)))))); +j4eval[2]=IKsign(((((-1.5)*x498))+(((-1.0)*x497*x506))+(((-3.0)*x499*x501))+((x495*x506))+(((-1.0)*x501*x505))+(((-0.5)*x497))+((x499*x505))+(((-2.0)*x495)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x509=((0.866025403784439)*sj5); +IkReal x510=(r01*sj0); +IkReal x511=(r02*sj0); +IkReal x512=(cj0*r11); +IkReal x513=(cj0*r12); +IkReal x514=((1.73205080756888)*sj5); +IkReal x515=(cj0*cj5*r10); +IkReal x516=(cj5*r00*sj0); +j4eval[0]=(((x510*x514))+(((1.73205080756888)*x515))+(((-1.0)*x511))+(((-1.73205080756888)*x516))+(((-1.0)*x512*x514))+x513); +j4eval[1]=IKsign(((((-0.5)*x511))+(((0.5)*x513))+(((0.866025403784439)*x515))+(((-0.866025403784439)*x516))+(((-1.0)*x509*x512))+((x509*x510)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x517=(r12*sj0); +IkReal x518=(cj0*r02); +IkReal x519=(cj0*cj5*r00); +IkReal x520=(r11*sj0*sj5); +IkReal x521=(cj0*r01*sj5); +IkReal x522=(cj5*r10*sj0); +j4eval[0]=((((1.73205080756888)*x521))+(((1.73205080756888)*x520))+(((-1.0)*x518))+(((-1.0)*x517))+(((-1.73205080756888)*x519))+(((-1.73205080756888)*x522))); +j4eval[1]=IKsign(((((-0.5)*x517))+(((-0.5)*x518))+(((-0.866025403784439)*x522))+(((0.866025403784439)*x521))+(((0.866025403784439)*x520))+(((-0.866025403784439)*x519)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x523=((0.5)*sj0); +IkReal x524=((0.5)*cj5); +IkReal x525=(cj0*sj3); +IkReal x526=(cj3*r20); +IkReal x527=((1.0)*cj5); +IkReal x528=((1.0)*sj5); +IkReal x529=(cj5*sj3); +IkReal x530=((0.5)*sj5); +IkReal x531=(cj3*r21); +IkReal x532=(r01*sj5); +IkReal x533=((0.866025403784439)*cj0); +IkReal x534=((0.866025403784439)*sj0); +CheckValue x535=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*r00*x533))+(((-0.5)*cj0*r02))+((r11*sj5*x534))+(((-1.0)*cj5*r10*x534))+((x532*x533))+(((-1.0)*r12*x523)))),-1); +if(!x535.valid){ +continue; +} +CheckValue x536 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+(((-1.0)*r10*x525*x528))+(((-1.0)*x526*x528))+(((-1.0)*x527*x531))+((r01*sj0*x529))+(((-1.0)*r11*x525*x527)))),IkReal(((((-1.0)*r11*x525*x530))+(((-1.0)*r00*x523*x529))+(((-1.0)*x530*x531))+(((-0.866025403784439)*r12*x525))+((sj3*x523*x532))+(((-0.866025403784439)*cj3*r22))+((x524*x526))+((r10*x524*x525))+((r02*sj3*x534)))),IKFAST_ATAN2_MAGTHRESH); +if(!x536.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x535.value)))+(x536.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x537=IKcos(j4); +IkReal x538=IKsin(j4); +IkReal x539=(r21*sj5); +IkReal x540=(cj5*sj0); +IkReal x541=(sj0*sj5); +IkReal x542=(cj0*r01); +IkReal x543=(cj5*r20); +IkReal x544=(r20*sj5); +IkReal x545=(cj0*sj5); +IkReal x546=(r02*sj0); +IkReal x547=(cj0*r12); +IkReal x548=(cj0*r02); +IkReal x549=(r12*sj0); +IkReal x550=(cj0*cj5); +IkReal x551=((0.5)*x538); +IkReal x552=((1.0)*x537); +IkReal x553=((0.5)*x537); +IkReal x554=(r10*x538); +IkReal x555=(cj5*x538); +IkReal x556=((1.0)*x538); +IkReal x557=((0.866025403784439)*x538); +IkReal x558=((0.866025403784439)*x537); +evalcond[0]=(sj3+(((-1.0)*r22*x557))+(((-1.0)*x539*x551))+((cj5*r21*x537))+((x537*x544))+((x543*x551))); +evalcond[1]=((((-1.0)*r22*x558))+(((-1.0)*x544*x556))+(((-1.0)*x539*x553))+(((-1.0)*r21*x555))+((x543*x553))+(((-0.5)*cj3))); +evalcond[2]=((((-1.0)*r01*x541*x551))+(((-1.0)*r10*x550*x551))+cj3+((r11*x545*x551))+((r00*x537*x541))+((x547*x557))+(((-1.0)*r10*x545*x552))+(((-1.0)*r11*x550*x552))+((r00*x540*x551))+((r01*x537*x540))+(((-1.0)*x546*x557))); +evalcond[3]=(((x548*x557))+(((-1.0)*r11*x540*x552))+((r11*x541*x551))+((sj5*x542*x551))+(((-1.0)*r10*x541*x552))+((x549*x557))+(((-1.0)*cj5*x542*x552))+(((-1.0)*r10*x540*x551))+(((-1.0)*r00*x550*x551))+(((-1.0)*r00*x545*x552))); +evalcond[4]=((((-1.0)*r01*x541*x553))+(((-1.0)*r10*x550*x553))+((r11*x545*x553))+(((-1.0)*r01*x540*x556))+(((0.5)*sj3))+((r11*x538*x550))+((x547*x558))+((r00*x540*x553))+(((-1.0)*r00*x541*x556))+(((-1.0)*x546*x558))+((x545*x554))); +evalcond[5]=((0.866025403784439)+((x548*x558))+((x542*x555))+((x541*x554))+((r11*x541*x553))+((r11*x538*x540))+((sj5*x542*x553))+((x549*x558))+(((-1.0)*r10*x540*x553))+(((-1.0)*r00*x550*x553))+((r00*x538*x545))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x559=((0.866025403784439)*sj5); +IkReal x560=((1.0)*sj3); +IkReal x561=(cj0*sj5); +IkReal x562=((0.5)*sj0); +IkReal x563=(cj5*sj0); +IkReal x564=((0.866025403784439)*cj0); +IkReal x565=(cj5*r10); +IkReal x566=((0.5)*sj3); +IkReal x567=(cj0*cj5); +CheckValue x568=IKPowWithIntegerCheck(IKsign((((x564*x565))+((r01*sj0*x559))+(((-0.866025403784439)*r00*x563))+(((-1.0)*cj0*r11*x559))+(((0.5)*cj0*r12))+(((-1.0)*r02*x562)))),-1); +if(!x568.valid){ +continue; +} +CheckValue x569 = IKatan2WithCheck(IkReal(((((-1.0)*r10*sj0*sj5*x560))+(((-1.0)*r00*x560*x561))+(((-1.0)*r11*x560*x563))+(((-1.0)*r01*x560*x567)))),IkReal(((((-1.0)*r01*x561*x566))+(((-0.866025403784439)*r12*sj0*sj3))+(((-1.0)*r11*sj3*sj5*x562))+((r00*x566*x567))+(((-1.0)*r02*sj3*x564))+((sj3*x562*x565)))),IKFAST_ATAN2_MAGTHRESH); +if(!x569.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x568.value)))+(x569.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x570=IKcos(j4); +IkReal x571=IKsin(j4); +IkReal x572=(r21*sj5); +IkReal x573=(cj5*sj0); +IkReal x574=(sj0*sj5); +IkReal x575=(cj0*r01); +IkReal x576=(cj5*r20); +IkReal x577=(r20*sj5); +IkReal x578=(cj0*sj5); +IkReal x579=(r02*sj0); +IkReal x580=(cj0*r12); +IkReal x581=(cj0*r02); +IkReal x582=(r12*sj0); +IkReal x583=(cj0*cj5); +IkReal x584=((0.5)*x571); +IkReal x585=((1.0)*x570); +IkReal x586=((0.5)*x570); +IkReal x587=(r10*x571); +IkReal x588=(cj5*x571); +IkReal x589=((1.0)*x571); +IkReal x590=((0.866025403784439)*x571); +IkReal x591=((0.866025403784439)*x570); +evalcond[0]=(sj3+(((-1.0)*x572*x584))+((cj5*r21*x570))+(((-1.0)*r22*x590))+((x576*x584))+((x570*x577))); +evalcond[1]=((((-1.0)*x572*x586))+(((-1.0)*r21*x588))+(((-1.0)*x577*x589))+(((-1.0)*r22*x591))+(((-0.5)*cj3))+((x576*x586))); +evalcond[2]=(((r11*x578*x584))+cj3+(((-1.0)*r11*x583*x585))+(((-1.0)*r10*x583*x584))+((r00*x570*x574))+((r00*x573*x584))+((r01*x570*x573))+((x580*x590))+(((-1.0)*r10*x578*x585))+(((-1.0)*x579*x590))+(((-1.0)*r01*x574*x584))); +evalcond[3]=(((r11*x574*x584))+(((-1.0)*cj5*x575*x585))+(((-1.0)*r11*x573*x585))+(((-1.0)*r10*x574*x585))+((sj5*x575*x584))+(((-1.0)*r00*x583*x584))+(((-1.0)*r10*x573*x584))+((x581*x590))+((x582*x590))+(((-1.0)*r00*x578*x585))); +evalcond[4]=(((x578*x587))+((r11*x578*x586))+(((-1.0)*r10*x583*x586))+(((-1.0)*r01*x573*x589))+(((-1.0)*r00*x574*x589))+(((0.5)*sj3))+((r11*x571*x583))+((r00*x573*x586))+((x580*x591))+(((-1.0)*x579*x591))+(((-1.0)*r01*x574*x586))); +evalcond[5]=((0.866025403784439)+((r11*x574*x586))+((sj5*x575*x586))+((r00*x571*x578))+(((-1.0)*r00*x583*x586))+(((-1.0)*r10*x573*x586))+((x574*x587))+((r11*x571*x573))+((x575*x588))+((x581*x591))+((x582*x591))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x592=cj5*cj5; +IkReal x593=r20*r20; +IkReal x594=r21*r21; +IkReal x595=(r21*sj5); +IkReal x596=(cj3*cj5); +IkReal x597=(r20*sj5); +IkReal x598=(cj5*sj3); +IkReal x599=(cj5*r20); +IkReal x600=((1.73205080756888)*r22); +IkReal x601=((1.5)*x592); +CheckValue x602 = IKatan2WithCheck(IkReal(((((-1.0)*sj3*x600))+(((-1.0)*sj3*x595))+((r21*x596))+((cj3*x597))+((r20*x598)))),IkReal(((((-0.5)*r20*x596))+(((2.0)*sj3*x597))+(((0.866025403784439)*cj3*r22))+(((2.0)*r21*x598))+(((0.5)*cj3*x595)))),IKFAST_ATAN2_MAGTHRESH); +if(!x602.valid){ +continue; +} +CheckValue x603=IKPowWithIntegerCheck(IKsign(((((-1.5)*(r22*r22)))+((x593*x601))+(((-2.0)*x593))+(((-0.5)*x594))+(((-3.0)*x595*x599))+((x599*x600))+(((-1.0)*x594*x601))+(((-1.0)*x595*x600)))),-1); +if(!x603.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x602.value)+(((1.5707963267949)*(x603.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x604=IKcos(j4); +IkReal x605=IKsin(j4); +IkReal x606=(r21*sj5); +IkReal x607=(cj5*sj0); +IkReal x608=(sj0*sj5); +IkReal x609=(cj0*r01); +IkReal x610=(cj5*r20); +IkReal x611=(r20*sj5); +IkReal x612=(cj0*sj5); +IkReal x613=(r02*sj0); +IkReal x614=(cj0*r12); +IkReal x615=(cj0*r02); +IkReal x616=(r12*sj0); +IkReal x617=(cj0*cj5); +IkReal x618=((0.5)*x605); +IkReal x619=((1.0)*x604); +IkReal x620=((0.5)*x604); +IkReal x621=(r10*x605); +IkReal x622=(cj5*x605); +IkReal x623=((1.0)*x605); +IkReal x624=((0.866025403784439)*x605); +IkReal x625=((0.866025403784439)*x604); +evalcond[0]=(sj3+((cj5*r21*x604))+(((-1.0)*x606*x618))+((x610*x618))+(((-1.0)*r22*x624))+((x604*x611))); +evalcond[1]=((((-1.0)*x606*x620))+(((-1.0)*r21*x622))+(((-1.0)*x611*x623))+((x610*x620))+(((-0.5)*cj3))+(((-1.0)*r22*x625))); +evalcond[2]=(((x614*x624))+((r01*x604*x607))+((r00*x604*x608))+cj3+(((-1.0)*r10*x617*x618))+(((-1.0)*r01*x608*x618))+((r11*x612*x618))+((r00*x607*x618))+(((-1.0)*r10*x612*x619))+(((-1.0)*r11*x617*x619))+(((-1.0)*x613*x624))); +evalcond[3]=(((x616*x624))+(((-1.0)*cj5*x609*x619))+((r11*x608*x618))+((sj5*x609*x618))+(((-1.0)*r00*x612*x619))+(((-1.0)*r10*x608*x619))+(((-1.0)*r11*x607*x619))+(((-1.0)*r10*x607*x618))+(((-1.0)*r00*x617*x618))+((x615*x624))); +evalcond[4]=(((x614*x625))+((r11*x605*x617))+(((0.5)*sj3))+(((-1.0)*r01*x608*x620))+(((-1.0)*r00*x608*x623))+((x612*x621))+((r00*x607*x620))+(((-1.0)*r10*x617*x620))+((r11*x612*x620))+(((-1.0)*x613*x625))+(((-1.0)*r01*x607*x623))); +evalcond[5]=((0.866025403784439)+(((-1.0)*r10*x607*x620))+((r00*x605*x612))+((x616*x625))+((r11*x605*x607))+((x609*x622))+(((-1.0)*r00*x617*x620))+((x608*x621))+((r11*x608*x620))+((sj5*x609*x620))+((x615*x625))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x626=((1.0)*cj5); +if( IKabs((((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*r00*sj0*x626))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x626)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*r00*sj0*x626))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))))+IKsqr(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x626))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*r00*sj0*x626))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))), ((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x626)))); +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[2]; +IkReal x627=((0.866025403784439)*sj5); +IkReal x628=((0.866025403784439)*cj5); +evalcond[0]=((((0.5)*r22))+(((-1.0)*r21*x627))+(((0.866025403784439)*(IKcos(j3))))+((r20*x628))); +evalcond[1]=((((-1.0)*cj0*r10*x628))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+((cj0*r11*x627))+((r00*sj0*x628))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r01*sj0*x627))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x629=r20*r20; +IkReal x630=cj5*cj5; +IkReal x631=r21*r21; +IkReal x632=r22*r22; +IkReal x633=(cj5*r20); +IkReal x634=((0.5)*cj3); +IkReal x635=(r21*sj5); +IkReal x636=((3.46410161513775)*r22); +IkReal x637=((1.73205080756888)*r22); +IkReal x638=(r20*sj5); +IkReal x639=((1.0)*cj3); +IkReal x640=((2.0)*sj3); +IkReal x641=(cj5*r21); +IkReal x642=((1.5)*x630); +IkReal x643=((3.0)*x630); +j4eval[0]=((((-1.0)*x631*x643))+((x629*x643))+(((-3.0)*x632))+(((-1.0)*x635*x636))+(((-6.0)*x633*x635))+(((-1.0)*x631))+(((-4.0)*x629))+((x633*x636))); +j4eval[1]=((IKabs(((((-1.0)*x640*x641))+(((-1.0)*x634*x635))+(((-0.866025403784439)*cj3*r22))+((x633*x634))+(((-1.0)*x638*x640)))))+(IKabs(((((-1.0)*x639*x641))+(((-1.0)*sj3*x633))+(((-1.0)*x638*x639))+((sj3*x635))+((sj3*x637)))))); +j4eval[2]=IKsign(((((-2.0)*x629))+(((-1.0)*x631*x642))+((x629*x642))+(((-1.5)*x632))+(((-1.0)*x635*x637))+(((-0.5)*x631))+(((-3.0)*x633*x635))+((x633*x637)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x644=(cj0*r12); +IkReal x645=((0.866025403784439)*sj5); +IkReal x646=(r01*sj0); +IkReal x647=(r02*sj0); +IkReal x648=(cj0*r11); +IkReal x649=((1.73205080756888)*sj5); +IkReal x650=(cj0*cj5*r10); +IkReal x651=(cj5*r00*sj0); +j4eval[0]=((((-1.73205080756888)*x650))+(((-1.0)*x644))+((x648*x649))+(((-1.0)*x646*x649))+(((1.73205080756888)*x651))+x647); +j4eval[1]=IKsign(((((0.5)*x647))+(((-0.866025403784439)*x650))+((x645*x648))+(((-0.5)*x644))+(((-1.0)*x645*x646))+(((0.866025403784439)*x651)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x652=(r12*sj0); +IkReal x653=(cj0*r02); +IkReal x654=(cj0*cj5*r00); +IkReal x655=(r11*sj0*sj5); +IkReal x656=(cj0*r01*sj5); +IkReal x657=(cj5*r10*sj0); +j4eval[0]=((((-1.73205080756888)*x656))+(((-1.73205080756888)*x655))+(((1.73205080756888)*x657))+(((1.73205080756888)*x654))+x652+x653); +j4eval[1]=IKsign(((((-0.866025403784439)*x656))+(((-0.866025403784439)*x655))+(((0.5)*x653))+(((0.5)*x652))+(((0.866025403784439)*x657))+(((0.866025403784439)*x654)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x658=((0.5)*sj0); +IkReal x659=((0.5)*cj5); +IkReal x660=(cj0*sj3); +IkReal x661=(cj3*r20); +IkReal x662=(cj5*sj3); +IkReal x663=((0.5)*sj5); +IkReal x664=(cj3*r21); +IkReal x665=((0.866025403784439)*cj0); +IkReal x666=(r01*sj5); +IkReal x667=((0.866025403784439)*sj0); +CheckValue x668 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+(((-1.0)*r10*sj5*x660))+((r01*sj0*x662))+(((-1.0)*cj5*r11*x660))+((sj5*x661))+((cj5*x664)))),IkReal(((((-1.0)*r11*x660*x663))+(((-1.0)*x659*x661))+(((-0.866025403784439)*r12*x660))+((x663*x664))+((sj3*x658*x666))+((r02*sj3*x667))+((r10*x659*x660))+(((0.866025403784439)*cj3*r22))+(((-1.0)*r00*x658*x662)))),IKFAST_ATAN2_MAGTHRESH); +if(!x668.valid){ +continue; +} +CheckValue x669=IKPowWithIntegerCheck(IKsign(((((-1.0)*x665*x666))+((cj5*r10*x667))+((cj5*r00*x665))+(((-1.0)*r11*sj5*x667))+(((0.5)*cj0*r02))+((r12*x658)))),-1); +if(!x669.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x668.value)+(((1.5707963267949)*(x669.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x670=IKcos(j4); +IkReal x671=IKsin(j4); +IkReal x672=(r21*sj5); +IkReal x673=(cj5*sj0); +IkReal x674=(sj0*sj5); +IkReal x675=(cj0*r01); +IkReal x676=(cj5*r20); +IkReal x677=(r20*sj5); +IkReal x678=(cj0*sj5); +IkReal x679=(r02*sj0); +IkReal x680=(cj0*r12); +IkReal x681=(cj0*r02); +IkReal x682=(r12*sj0); +IkReal x683=(cj0*cj5); +IkReal x684=((0.5)*x671); +IkReal x685=((1.0)*x670); +IkReal x686=((0.5)*x670); +IkReal x687=(r10*x671); +IkReal x688=(cj5*x671); +IkReal x689=((1.0)*x671); +IkReal x690=((0.866025403784439)*x671); +IkReal x691=((0.866025403784439)*x670); +evalcond[0]=((((-1.0)*sj3))+((x670*x677))+((x676*x684))+(((-1.0)*r22*x690))+(((-1.0)*x672*x684))+((cj5*r21*x670))); +evalcond[1]=((((-1.0)*r21*x688))+((x676*x686))+(((0.5)*cj3))+(((-1.0)*r22*x691))+(((-1.0)*x677*x689))+(((-1.0)*x672*x686))); +evalcond[2]=((((-1.0)*r10*x678*x685))+cj3+(((-1.0)*r01*x674*x684))+((r11*x678*x684))+(((-1.0)*r10*x683*x684))+((r00*x673*x684))+((x680*x690))+(((-1.0)*r11*x683*x685))+((r00*x670*x674))+((r01*x670*x673))+(((-1.0)*x679*x690))); +evalcond[3]=(((x682*x690))+(((-1.0)*cj5*x675*x685))+((r11*x674*x684))+(((-1.0)*r00*x678*x685))+(((-1.0)*r10*x673*x684))+((sj5*x675*x684))+(((-1.0)*r10*x674*x685))+((x681*x690))+(((-1.0)*r00*x683*x684))+(((-1.0)*r11*x673*x685))); +evalcond[4]=(((x678*x687))+(((-1.0)*r01*x674*x686))+((r11*x678*x686))+(((0.5)*sj3))+(((-1.0)*r10*x683*x686))+(((-1.0)*r00*x674*x689))+((r00*x673*x686))+((x680*x691))+(((-1.0)*r01*x673*x689))+((r11*x671*x683))+(((-1.0)*x679*x691))); +evalcond[5]=((-0.866025403784439)+((x682*x691))+((r11*x674*x686))+(((-1.0)*r10*x673*x686))+((sj5*x675*x686))+((x674*x687))+((x681*x691))+((x675*x688))+(((-1.0)*r00*x683*x686))+((r00*x671*x678))+((r11*x671*x673))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x692=((0.866025403784439)*sj5); +IkReal x693=((0.5)*sj0); +IkReal x694=(r11*sj3); +IkReal x695=((1.0)*sj0); +IkReal x696=((0.866025403784439)*cj0); +IkReal x697=(cj5*r10); +IkReal x698=((0.5)*cj0); +IkReal x699=(sj3*sj5); +IkReal x700=((1.0)*cj0); +IkReal x701=(cj5*sj3); +IkReal x702=((0.866025403784439)*sj0); +CheckValue x703=IKPowWithIntegerCheck(IKsign(((((-1.0)*r01*sj0*x692))+(((-1.0)*x696*x697))+((cj0*r11*x692))+(((-1.0)*r12*x698))+((r02*x693))+((cj5*r00*x702)))),-1); +if(!x703.valid){ +continue; +} +CheckValue x704 = IKatan2WithCheck(IkReal(((((-1.0)*r00*x699*x700))+(((-1.0)*r01*x700*x701))+(((-1.0)*cj5*x694*x695))+(((-1.0)*r10*x695*x699)))),IkReal(((((-1.0)*r12*sj3*x702))+((sj3*x693*x697))+((r00*x698*x701))+(((-1.0)*sj5*x693*x694))+(((-1.0)*r02*sj3*x696))+(((-1.0)*r01*x698*x699)))),IKFAST_ATAN2_MAGTHRESH); +if(!x704.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x703.value)))+(x704.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x705=IKcos(j4); +IkReal x706=IKsin(j4); +IkReal x707=(r21*sj5); +IkReal x708=(cj5*sj0); +IkReal x709=(sj0*sj5); +IkReal x710=(cj0*r01); +IkReal x711=(cj5*r20); +IkReal x712=(r20*sj5); +IkReal x713=(cj0*sj5); +IkReal x714=(r02*sj0); +IkReal x715=(cj0*r12); +IkReal x716=(cj0*r02); +IkReal x717=(r12*sj0); +IkReal x718=(cj0*cj5); +IkReal x719=((0.5)*x706); +IkReal x720=((1.0)*x705); +IkReal x721=((0.5)*x705); +IkReal x722=(r10*x706); +IkReal x723=(cj5*x706); +IkReal x724=((1.0)*x706); +IkReal x725=((0.866025403784439)*x706); +IkReal x726=((0.866025403784439)*x705); +evalcond[0]=((((-1.0)*sj3))+(((-1.0)*x707*x719))+((cj5*r21*x705))+((x711*x719))+(((-1.0)*r22*x725))+((x705*x712))); +evalcond[1]=((((-1.0)*r21*x723))+((x711*x721))+(((0.5)*cj3))+(((-1.0)*r22*x726))+(((-1.0)*x707*x721))+(((-1.0)*x712*x724))); +evalcond[2]=((((-1.0)*r01*x709*x719))+cj3+(((-1.0)*r10*x713*x720))+((r00*x705*x709))+((r11*x713*x719))+(((-1.0)*r10*x718*x719))+((r00*x708*x719))+(((-1.0)*r11*x718*x720))+(((-1.0)*x714*x725))+((x715*x725))+((r01*x705*x708))); +evalcond[3]=(((r11*x709*x719))+(((-1.0)*r00*x713*x720))+((x717*x725))+(((-1.0)*r10*x709*x720))+(((-1.0)*r10*x708*x719))+(((-1.0)*r11*x708*x720))+((sj5*x710*x719))+(((-1.0)*cj5*x710*x720))+((x716*x725))+(((-1.0)*r00*x718*x719))); +evalcond[4]=(((x713*x722))+(((-1.0)*r01*x709*x721))+(((-1.0)*r01*x708*x724))+(((0.5)*sj3))+((r11*x713*x721))+(((-1.0)*r00*x709*x724))+(((-1.0)*r10*x718*x721))+((r11*x706*x718))+(((-1.0)*x714*x726))+((r00*x708*x721))+((x715*x726))); +evalcond[5]=((-0.866025403784439)+((sj5*x710*x721))+((r00*x706*x713))+((x710*x723))+((x709*x722))+((x717*x726))+((r11*x706*x708))+(((-1.0)*r00*x718*x721))+((r11*x709*x721))+((x716*x726))+(((-1.0)*r10*x708*x721))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x727=cj5*cj5; +IkReal x728=r20*r20; +IkReal x729=r21*r21; +IkReal x730=((1.0)*cj5); +IkReal x731=(r20*sj3); +IkReal x732=(cj3*r21); +IkReal x733=(cj5*r20); +IkReal x734=(r21*sj3); +IkReal x735=(r21*sj5); +IkReal x736=((1.73205080756888)*r22); +IkReal x737=((1.5)*x727); +CheckValue x738 = IKatan2WithCheck(IkReal((((sj3*x736))+((sj5*x734))+(((-1.0)*x730*x731))+(((-1.0)*x730*x732))+(((-1.0)*cj3*r20*sj5)))),IkReal(((((-0.866025403784439)*cj3*r22))+(((-0.5)*sj5*x732))+(((-2.0)*sj5*x731))+(((0.5)*cj3*x733))+(((-2.0)*cj5*x734)))),IKFAST_ATAN2_MAGTHRESH); +if(!x738.valid){ +continue; +} +CheckValue x739=IKPowWithIntegerCheck(IKsign((((x728*x737))+(((-1.5)*(r22*r22)))+(((-1.0)*x735*x736))+(((-0.5)*x729))+(((-2.0)*x728))+((x733*x736))+(((-3.0)*x733*x735))+(((-1.0)*x729*x737)))),-1); +if(!x739.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x738.value)+(((1.5707963267949)*(x739.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x740=IKcos(j4); +IkReal x741=IKsin(j4); +IkReal x742=(r21*sj5); +IkReal x743=(cj5*sj0); +IkReal x744=(sj0*sj5); +IkReal x745=(cj0*r01); +IkReal x746=(cj5*r20); +IkReal x747=(r20*sj5); +IkReal x748=(cj0*sj5); +IkReal x749=(r02*sj0); +IkReal x750=(cj0*r12); +IkReal x751=(cj0*r02); +IkReal x752=(r12*sj0); +IkReal x753=(cj0*cj5); +IkReal x754=((0.5)*x741); +IkReal x755=((1.0)*x740); +IkReal x756=((0.5)*x740); +IkReal x757=(r10*x741); +IkReal x758=(cj5*x741); +IkReal x759=((1.0)*x741); +IkReal x760=((0.866025403784439)*x741); +IkReal x761=((0.866025403784439)*x740); +evalcond[0]=((((-1.0)*x742*x754))+(((-1.0)*sj3))+((x740*x747))+((x746*x754))+((cj5*r21*x740))+(((-1.0)*r22*x760))); +evalcond[1]=((((-1.0)*x742*x756))+(((-1.0)*r21*x758))+((x746*x756))+(((0.5)*cj3))+(((-1.0)*x747*x759))+(((-1.0)*r22*x761))); +evalcond[2]=((((-1.0)*r10*x753*x754))+(((-1.0)*r11*x753*x755))+((r00*x740*x744))+cj3+(((-1.0)*r01*x744*x754))+((r11*x748*x754))+((r00*x743*x754))+((r01*x740*x743))+(((-1.0)*r10*x748*x755))+((x750*x760))+(((-1.0)*x749*x760))); +evalcond[3]=((((-1.0)*r00*x753*x754))+((r11*x744*x754))+(((-1.0)*r10*x744*x755))+((x752*x760))+((sj5*x745*x754))+(((-1.0)*cj5*x745*x755))+(((-1.0)*r10*x743*x754))+(((-1.0)*r11*x743*x755))+((x751*x760))+(((-1.0)*r00*x748*x755))); +evalcond[4]=((((-1.0)*r10*x753*x756))+(((-1.0)*r01*x744*x756))+((r11*x748*x756))+((r00*x743*x756))+(((0.5)*sj3))+((r11*x741*x753))+(((-1.0)*r00*x744*x759))+((x750*x761))+((x748*x757))+(((-1.0)*r01*x743*x759))+(((-1.0)*x749*x761))); +evalcond[5]=((-0.866025403784439)+((x745*x758))+(((-1.0)*r00*x753*x756))+((r11*x744*x756))+((x744*x757))+((x752*x761))+((sj5*x745*x756))+(((-1.0)*r10*x743*x756))+((x751*x761))+((r11*x741*x743))+((r00*x741*x748))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(j1, 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j2), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x762=((1.0)*cj5); +if( IKabs(((((-1.0)*r00*sj0*x762))+((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x762)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*r00*sj0*x762))+((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))))+IKsqr(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x762))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*r00*sj0*x762))+((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))), ((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x762)))); +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[2]; +IkReal x763=((0.866025403784439)*sj5); +IkReal x764=((0.866025403784439)*cj5); +evalcond[0]=(((r20*x764))+(((0.5)*r22))+(((-1.0)*r21*x763))+(((0.866025403784439)*(IKcos(j3))))); +evalcond[1]=(((cj0*r11*x763))+(((-1.0)*cj0*r10*x764))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((0.866025403784439)*(IKsin(j3))))+((r00*sj0*x764))+(((-1.0)*r01*sj0*x763))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x765=r20*r20; +IkReal x766=cj5*cj5; +IkReal x767=r21*r21; +IkReal x768=r22*r22; +IkReal x769=(cj5*r20); +IkReal x770=((0.5)*cj3); +IkReal x771=(r21*sj5); +IkReal x772=((3.46410161513775)*r22); +IkReal x773=((1.73205080756888)*r22); +IkReal x774=(r20*sj5); +IkReal x775=((1.0)*cj3); +IkReal x776=((2.0)*sj3); +IkReal x777=(cj5*r21); +IkReal x778=((1.5)*x766); +IkReal x779=((3.0)*x766); +j4eval[0]=((((-6.0)*x769*x771))+(((-1.0)*x767*x779))+((x769*x772))+(((-1.0)*x771*x772))+((x765*x779))+(((-1.0)*x767))+(((-4.0)*x765))+(((-3.0)*x768))); +j4eval[1]=((IKabs(((((-1.0)*x774*x775))+(((-1.0)*sj3*x769))+(((-1.0)*x775*x777))+((sj3*x771))+((sj3*x773)))))+(IKabs(((((-1.0)*x774*x776))+((x769*x770))+(((-0.866025403784439)*cj3*r22))+(((-1.0)*x776*x777))+(((-1.0)*x770*x771)))))); +j4eval[2]=IKsign(((((-2.0)*x765))+(((-1.0)*x767*x778))+((x769*x773))+(((-1.0)*x771*x773))+((x765*x778))+(((-0.5)*x767))+(((-3.0)*x769*x771))+(((-1.5)*x768)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x780=(cj0*r12); +IkReal x781=((0.866025403784439)*sj5); +IkReal x782=(r01*sj0); +IkReal x783=(r02*sj0); +IkReal x784=(cj0*r11); +IkReal x785=((1.73205080756888)*sj5); +IkReal x786=(cj0*cj5*r10); +IkReal x787=(cj5*r00*sj0); +j4eval[0]=(((x784*x785))+(((1.73205080756888)*x787))+(((-1.73205080756888)*x786))+(((-1.0)*x782*x785))+x783+(((-1.0)*x780))); +j4eval[1]=IKsign((((x781*x784))+(((-1.0)*x781*x782))+(((0.5)*x783))+(((-0.866025403784439)*x786))+(((0.866025403784439)*x787))+(((-0.5)*x780)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x788=(r12*sj0); +IkReal x789=(cj0*r02); +IkReal x790=(cj0*cj5*r00); +IkReal x791=(r11*sj0*sj5); +IkReal x792=(cj0*r01*sj5); +IkReal x793=(cj5*r10*sj0); +j4eval[0]=((((1.73205080756888)*x793))+(((1.73205080756888)*x790))+(((-1.73205080756888)*x792))+(((-1.73205080756888)*x791))+x788+x789); +j4eval[1]=IKsign(((((0.5)*x789))+(((0.5)*x788))+(((-0.866025403784439)*x792))+(((-0.866025403784439)*x791))+(((0.866025403784439)*x793))+(((0.866025403784439)*x790)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x794=((0.5)*sj0); +IkReal x795=((0.5)*cj5); +IkReal x796=(cj0*sj3); +IkReal x797=(cj3*r20); +IkReal x798=(cj5*sj3); +IkReal x799=((0.5)*sj5); +IkReal x800=(cj3*r21); +IkReal x801=((0.866025403784439)*cj0); +IkReal x802=(r01*sj5); +IkReal x803=((0.866025403784439)*sj0); +CheckValue x804=IKPowWithIntegerCheck(IKsign(((((-1.0)*r11*sj5*x803))+((cj5*r10*x803))+(((0.5)*cj0*r02))+((cj5*r00*x801))+(((-1.0)*x801*x802))+((r12*x794)))),-1); +if(!x804.valid){ +continue; +} +CheckValue x805 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+(((-1.0)*r10*sj5*x796))+((sj5*x797))+((cj5*x800))+((r01*sj0*x798))+(((-1.0)*cj5*r11*x796)))),IkReal(((((-1.0)*r00*x794*x798))+((r02*sj3*x803))+(((-1.0)*x795*x797))+((sj3*x794*x802))+((x799*x800))+(((0.866025403784439)*cj3*r22))+(((-1.0)*r11*x796*x799))+((r10*x795*x796))+(((-0.866025403784439)*r12*x796)))),IKFAST_ATAN2_MAGTHRESH); +if(!x805.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x804.value)))+(x805.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x806=IKcos(j4); +IkReal x807=IKsin(j4); +IkReal x808=(r21*sj5); +IkReal x809=(cj5*sj0); +IkReal x810=(sj0*sj5); +IkReal x811=(cj0*r01); +IkReal x812=(cj5*r20); +IkReal x813=(r20*sj5); +IkReal x814=(cj0*sj5); +IkReal x815=(r02*sj0); +IkReal x816=(cj0*r12); +IkReal x817=(cj0*r02); +IkReal x818=(r12*sj0); +IkReal x819=(cj0*cj5); +IkReal x820=((0.5)*x807); +IkReal x821=((1.0)*x806); +IkReal x822=((0.5)*x806); +IkReal x823=(r10*x807); +IkReal x824=(cj5*x807); +IkReal x825=((1.0)*x807); +IkReal x826=((0.866025403784439)*x807); +IkReal x827=((0.866025403784439)*x806); +evalcond[0]=(((x812*x820))+(((-1.0)*sj3))+(((-1.0)*x808*x820))+((cj5*r21*x806))+(((-1.0)*r22*x826))+((x806*x813))); +evalcond[1]=(((x812*x822))+(((-1.0)*x808*x822))+(((-1.0)*r21*x824))+(((-1.0)*x813*x825))+(((0.5)*cj3))+(((-1.0)*r22*x827))); +evalcond[2]=((((-1.0)*r10*x814*x821))+((r00*x809*x820))+((r01*x806*x809))+cj3+(((-1.0)*r11*x819*x821))+((r11*x814*x820))+(((-1.0)*x815*x826))+(((-1.0)*r10*x819*x820))+((x816*x826))+((r00*x806*x810))+(((-1.0)*r01*x810*x820))); +evalcond[3]=((((-1.0)*r10*x809*x820))+((r11*x810*x820))+(((-1.0)*r00*x814*x821))+(((-1.0)*r11*x809*x821))+((x817*x826))+((sj5*x811*x820))+(((-1.0)*r00*x819*x820))+(((-1.0)*cj5*x811*x821))+((x818*x826))+(((-1.0)*r10*x810*x821))); +evalcond[4]=(((r11*x807*x819))+((r00*x809*x822))+(((-1.0)*r00*x810*x825))+((r11*x814*x822))+(((0.5)*sj3))+(((-1.0)*x815*x827))+((x814*x823))+(((-1.0)*r10*x819*x822))+((x816*x827))+(((-1.0)*r01*x809*x825))+(((-1.0)*r01*x810*x822))); +evalcond[5]=((-0.866025403784439)+(((-1.0)*r10*x809*x822))+((r11*x810*x822))+((x817*x827))+((x811*x824))+((sj5*x811*x822))+(((-1.0)*r00*x819*x822))+((x810*x823))+((r11*x807*x809))+((x818*x827))+((r00*x807*x814))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x828=((0.866025403784439)*sj5); +IkReal x829=((0.5)*sj0); +IkReal x830=(r11*sj3); +IkReal x831=((1.0)*sj0); +IkReal x832=((0.866025403784439)*cj0); +IkReal x833=(cj5*r10); +IkReal x834=((0.5)*cj0); +IkReal x835=(sj3*sj5); +IkReal x836=((1.0)*cj0); +IkReal x837=(cj5*sj3); +IkReal x838=((0.866025403784439)*sj0); +CheckValue x839 = IKatan2WithCheck(IkReal(((((-1.0)*r10*x831*x835))+(((-1.0)*r00*x835*x836))+(((-1.0)*cj5*x830*x831))+(((-1.0)*r01*x836*x837)))),IkReal((((r00*x834*x837))+((sj3*x829*x833))+(((-1.0)*r01*x834*x835))+(((-1.0)*r12*sj3*x838))+(((-1.0)*sj5*x829*x830))+(((-1.0)*r02*sj3*x832)))),IKFAST_ATAN2_MAGTHRESH); +if(!x839.valid){ +continue; +} +CheckValue x840=IKPowWithIntegerCheck(IKsign(((((-1.0)*r01*sj0*x828))+(((-1.0)*x832*x833))+((cj0*r11*x828))+((cj5*r00*x838))+(((-1.0)*r12*x834))+((r02*x829)))),-1); +if(!x840.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x839.value)+(((1.5707963267949)*(x840.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x841=IKcos(j4); +IkReal x842=IKsin(j4); +IkReal x843=(r21*sj5); +IkReal x844=(cj5*sj0); +IkReal x845=(sj0*sj5); +IkReal x846=(cj0*r01); +IkReal x847=(cj5*r20); +IkReal x848=(r20*sj5); +IkReal x849=(cj0*sj5); +IkReal x850=(r02*sj0); +IkReal x851=(cj0*r12); +IkReal x852=(cj0*r02); +IkReal x853=(r12*sj0); +IkReal x854=(cj0*cj5); +IkReal x855=((0.5)*x842); +IkReal x856=((1.0)*x841); +IkReal x857=((0.5)*x841); +IkReal x858=(r10*x842); +IkReal x859=(cj5*x842); +IkReal x860=((1.0)*x842); +IkReal x861=((0.866025403784439)*x842); +IkReal x862=((0.866025403784439)*x841); +evalcond[0]=((((-1.0)*sj3))+((x847*x855))+((cj5*r21*x841))+(((-1.0)*x843*x855))+(((-1.0)*r22*x861))+((x841*x848))); +evalcond[1]=((((-1.0)*x848*x860))+(((-1.0)*r21*x859))+((x847*x857))+(((0.5)*cj3))+(((-1.0)*x843*x857))+(((-1.0)*r22*x862))); +evalcond[2]=(((r00*x841*x845))+cj3+(((-1.0)*r01*x845*x855))+(((-1.0)*x850*x861))+((r11*x849*x855))+((x851*x861))+(((-1.0)*r10*x854*x855))+(((-1.0)*r11*x854*x856))+((r01*x841*x844))+(((-1.0)*r10*x849*x856))+((r00*x844*x855))); +evalcond[3]=((((-1.0)*cj5*x846*x856))+((sj5*x846*x855))+(((-1.0)*r10*x845*x856))+(((-1.0)*r11*x844*x856))+(((-1.0)*r00*x854*x855))+(((-1.0)*r00*x849*x856))+((x853*x861))+((r11*x845*x855))+(((-1.0)*r10*x844*x855))+((x852*x861))); +evalcond[4]=((((-1.0)*r01*x844*x860))+(((-1.0)*r01*x845*x857))+(((-1.0)*x850*x862))+(((0.5)*sj3))+((r11*x849*x857))+((x851*x862))+(((-1.0)*r10*x854*x857))+(((-1.0)*r00*x845*x860))+((r11*x842*x854))+((x849*x858))+((r00*x844*x857))); +evalcond[5]=((-0.866025403784439)+((r00*x842*x849))+((x845*x858))+((sj5*x846*x857))+(((-1.0)*r00*x854*x857))+((x853*x862))+((x846*x859))+((r11*x845*x857))+(((-1.0)*r10*x844*x857))+((r11*x842*x844))+((x852*x862))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x863=cj5*cj5; +IkReal x864=r20*r20; +IkReal x865=r21*r21; +IkReal x866=((1.0)*cj5); +IkReal x867=(r20*sj3); +IkReal x868=(cj3*r21); +IkReal x869=(cj5*r20); +IkReal x870=(r21*sj3); +IkReal x871=(r21*sj5); +IkReal x872=((1.73205080756888)*r22); +IkReal x873=((1.5)*x863); +CheckValue x874 = IKatan2WithCheck(IkReal((((sj3*x872))+((sj5*x870))+(((-1.0)*x866*x867))+(((-1.0)*x866*x868))+(((-1.0)*cj3*r20*sj5)))),IkReal(((((-0.5)*sj5*x868))+(((-2.0)*cj5*x870))+(((-0.866025403784439)*cj3*r22))+(((-2.0)*sj5*x867))+(((0.5)*cj3*x869)))),IKFAST_ATAN2_MAGTHRESH); +if(!x874.valid){ +continue; +} +CheckValue x875=IKPowWithIntegerCheck(IKsign(((((-1.5)*(r22*r22)))+(((-1.0)*x871*x872))+(((-2.0)*x864))+(((-3.0)*x869*x871))+(((-0.5)*x865))+((x869*x872))+((x864*x873))+(((-1.0)*x865*x873)))),-1); +if(!x875.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x874.value)+(((1.5707963267949)*(x875.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x876=IKcos(j4); +IkReal x877=IKsin(j4); +IkReal x878=(r21*sj5); +IkReal x879=(cj5*sj0); +IkReal x880=(sj0*sj5); +IkReal x881=(cj0*r01); +IkReal x882=(cj5*r20); +IkReal x883=(r20*sj5); +IkReal x884=(cj0*sj5); +IkReal x885=(r02*sj0); +IkReal x886=(cj0*r12); +IkReal x887=(cj0*r02); +IkReal x888=(r12*sj0); +IkReal x889=(cj0*cj5); +IkReal x890=((0.5)*x877); +IkReal x891=((1.0)*x876); +IkReal x892=((0.5)*x876); +IkReal x893=(r10*x877); +IkReal x894=(cj5*x877); +IkReal x895=((1.0)*x877); +IkReal x896=((0.866025403784439)*x877); +IkReal x897=((0.866025403784439)*x876); +evalcond[0]=((((-1.0)*sj3))+((x876*x883))+((x882*x890))+(((-1.0)*r22*x896))+(((-1.0)*x878*x890))+((cj5*r21*x876))); +evalcond[1]=((((-1.0)*x883*x895))+((x882*x892))+(((0.5)*cj3))+(((-1.0)*r21*x894))+(((-1.0)*r22*x897))+(((-1.0)*x878*x892))); +evalcond[2]=(((x886*x896))+((r11*x884*x890))+(((-1.0)*r01*x880*x890))+((r01*x876*x879))+cj3+(((-1.0)*r11*x889*x891))+((r00*x879*x890))+(((-1.0)*r10*x889*x890))+(((-1.0)*r10*x884*x891))+(((-1.0)*x885*x896))+((r00*x876*x880))); +evalcond[3]=((((-1.0)*cj5*x881*x891))+(((-1.0)*r00*x889*x890))+((sj5*x881*x890))+(((-1.0)*r00*x884*x891))+(((-1.0)*r11*x879*x891))+(((-1.0)*r10*x879*x890))+(((-1.0)*r10*x880*x891))+((x888*x896))+((r11*x880*x890))+((x887*x896))); +evalcond[4]=(((x886*x897))+((r11*x884*x892))+((x884*x893))+(((-1.0)*r01*x880*x892))+(((-1.0)*r00*x880*x895))+(((-1.0)*r01*x879*x895))+(((0.5)*sj3))+((r00*x879*x892))+(((-1.0)*r10*x889*x892))+(((-1.0)*x885*x897))+((r11*x877*x889))); +evalcond[5]=((-0.866025403784439)+((x880*x893))+((x881*x894))+(((-1.0)*r00*x889*x892))+((sj5*x881*x892))+((r11*x877*x879))+(((-1.0)*r10*x879*x892))+((x888*x897))+((r11*x880*x892))+((r00*x877*x884))+((x887*x897))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(j2, 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(j1, 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x898=((1.0)*sj5); +if( IKabs((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*x898)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj5*r20))+(((0.577350269189626)*r22))+(((-1.0)*r21*x898)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*x898))))+IKsqr((((cj5*r20))+(((0.577350269189626)*r22))+(((-1.0)*r21*x898))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*x898))), (((cj5*r20))+(((0.577350269189626)*r22))+(((-1.0)*r21*x898)))); +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[2]; +IkReal x899=((0.866025403784439)*sj5); +IkReal x900=((0.866025403784439)*cj5); +evalcond[0]=(((r20*x900))+(((-0.866025403784439)*(IKcos(j3))))+(((0.5)*r22))+(((-1.0)*r21*x899))); +evalcond[1]=((((-1.0)*r01*sj0*x899))+((r00*sj0*x900))+(((-0.5)*cj0*r12))+((cj0*r11*x899))+(((0.5)*r02*sj0))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*cj0*r10*x900))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x901=r20*r20; +IkReal x902=cj5*cj5; +IkReal x903=r21*r21; +IkReal x904=r22*r22; +IkReal x905=(cj5*r20); +IkReal x906=((0.5)*cj3); +IkReal x907=(r21*sj5); +IkReal x908=(r20*sj5); +IkReal x909=((2.0)*sj3); +IkReal x910=(cj5*r21); +IkReal x911=((1.73205080756888)*r22); +IkReal x912=((1.5)*x902); +IkReal x913=(r22*x907); +IkReal x914=((3.0)*x902); +j4eval[0]=(((x901*x914))+(((-3.0)*x904))+(((-6.0)*x905*x907))+(((-3.46410161513775)*x913))+(((-1.0)*x903))+(((3.46410161513775)*r22*x905))+(((-4.0)*x901))+(((-1.0)*x903*x914))); +j4eval[1]=((IKabs((((sj3*x905))+((cj3*x908))+((cj3*x910))+(((-1.0)*sj3*x911))+(((-1.0)*sj3*x907)))))+(IKabs((((x909*x910))+((x906*x907))+(((0.866025403784439)*cj3*r22))+((x908*x909))+(((-1.0)*x905*x906)))))); +j4eval[2]=IKsign((((x901*x912))+(((-3.0)*x905*x907))+(((-1.5)*x904))+(((-2.0)*x901))+(((-0.5)*x903))+(((-1.0)*x907*x911))+((x905*x911))+(((-1.0)*x903*x912)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x915=((0.866025403784439)*sj5); +IkReal x916=(r01*sj0); +IkReal x917=(r02*sj0); +IkReal x918=(cj0*r11); +IkReal x919=(cj0*r12); +IkReal x920=((1.73205080756888)*sj5); +IkReal x921=(cj0*cj5*r10); +IkReal x922=(cj5*r00*sj0); +j4eval[0]=(((x916*x920))+(((1.73205080756888)*x921))+(((-1.0)*x918*x920))+(((-1.0)*x917))+(((-1.73205080756888)*x922))+x919); +j4eval[1]=IKsign(((((0.866025403784439)*x921))+(((-0.866025403784439)*x922))+(((-0.5)*x917))+((x915*x916))+(((0.5)*x919))+(((-1.0)*x915*x918)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x923=(r12*sj0); +IkReal x924=(cj0*r02); +IkReal x925=(cj0*cj5*r00); +IkReal x926=(r11*sj0*sj5); +IkReal x927=(cj0*r01*sj5); +IkReal x928=(cj5*r10*sj0); +j4eval[0]=((((1.73205080756888)*x927))+(((1.73205080756888)*x926))+(((-1.0)*x923))+(((-1.0)*x924))+(((-1.73205080756888)*x925))+(((-1.73205080756888)*x928))); +j4eval[1]=IKsign(((((0.866025403784439)*x927))+(((0.866025403784439)*x926))+(((-0.866025403784439)*x925))+(((-0.866025403784439)*x928))+(((-0.5)*x923))+(((-0.5)*x924)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x929=((0.5)*sj0); +IkReal x930=((0.5)*cj5); +IkReal x931=(cj0*sj3); +IkReal x932=(cj3*r20); +IkReal x933=((1.0)*cj5); +IkReal x934=((1.0)*sj5); +IkReal x935=(cj5*sj3); +IkReal x936=((0.5)*sj5); +IkReal x937=(cj3*r21); +IkReal x938=(r01*sj5); +IkReal x939=((0.866025403784439)*cj0); +IkReal x940=((0.866025403784439)*sj0); +CheckValue x941=IKPowWithIntegerCheck(IKsign((((r11*sj5*x940))+(((-1.0)*r12*x929))+(((-1.0)*cj5*r10*x940))+(((-0.5)*cj0*r02))+((x938*x939))+(((-1.0)*cj5*r00*x939)))),-1); +if(!x941.valid){ +continue; +} +CheckValue x942 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+(((-1.0)*r11*x931*x933))+((r01*sj0*x935))+(((-1.0)*x932*x934))+(((-1.0)*x933*x937))+(((-1.0)*r10*x931*x934)))),IkReal(((((-1.0)*r00*x929*x935))+((r02*sj3*x940))+(((-1.0)*r11*x931*x936))+((r10*x930*x931))+((x930*x932))+(((-0.866025403784439)*cj3*r22))+(((-0.866025403784439)*r12*x931))+(((-1.0)*x936*x937))+((sj3*x929*x938)))),IKFAST_ATAN2_MAGTHRESH); +if(!x942.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x941.value)))+(x942.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x943=IKcos(j4); +IkReal x944=IKsin(j4); +IkReal x945=(r21*sj5); +IkReal x946=(cj5*sj0); +IkReal x947=(sj0*sj5); +IkReal x948=(cj0*r01); +IkReal x949=(cj5*r20); +IkReal x950=(r20*sj5); +IkReal x951=(cj0*sj5); +IkReal x952=(r02*sj0); +IkReal x953=(cj0*r12); +IkReal x954=(cj0*r02); +IkReal x955=(r12*sj0); +IkReal x956=(cj0*cj5); +IkReal x957=((0.5)*x944); +IkReal x958=((1.0)*x943); +IkReal x959=((0.5)*x943); +IkReal x960=(r10*x944); +IkReal x961=(cj5*x944); +IkReal x962=((1.0)*x944); +IkReal x963=((0.866025403784439)*x944); +IkReal x964=((0.866025403784439)*x943); +evalcond[0]=(sj3+((x949*x957))+((x943*x950))+((cj5*r21*x943))+(((-1.0)*r22*x963))+(((-1.0)*x945*x957))); +evalcond[1]=(((x949*x959))+(((-1.0)*r21*x961))+(((-1.0)*x950*x962))+(((-1.0)*r22*x964))+(((-1.0)*x945*x959))+(((-0.5)*cj3))); +evalcond[2]=(((r11*x951*x957))+cj3+((r00*x946*x957))+((r01*x943*x946))+((r00*x943*x947))+(((-1.0)*r11*x956*x958))+(((-1.0)*r01*x947*x957))+(((-1.0)*r10*x956*x957))+(((-1.0)*r10*x951*x958))+(((-1.0)*x952*x963))+((x953*x963))); +evalcond[3]=(((x954*x963))+(((-1.0)*r11*x946*x958))+(((-1.0)*r10*x946*x957))+(((-1.0)*cj5*x948*x958))+(((-1.0)*r00*x951*x958))+(((-1.0)*r10*x947*x958))+((sj5*x948*x957))+(((-1.0)*r00*x956*x957))+((r11*x947*x957))+((x955*x963))); +evalcond[4]=((((-1.0)*r00*x947*x962))+((x951*x960))+((r11*x951*x959))+((r00*x946*x959))+(((0.5)*sj3))+(((-1.0)*r01*x947*x959))+((r11*x944*x956))+(((-1.0)*r01*x946*x962))+(((-1.0)*r10*x956*x959))+(((-1.0)*x952*x964))+((x953*x964))); +evalcond[5]=((0.866025403784439)+((r11*x944*x946))+((x954*x964))+((r00*x944*x951))+(((-1.0)*r10*x946*x959))+((x948*x961))+((x947*x960))+((sj5*x948*x959))+(((-1.0)*r00*x956*x959))+((r11*x947*x959))+((x955*x964))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x965=((0.866025403784439)*sj5); +IkReal x966=((1.0)*sj3); +IkReal x967=(cj0*sj5); +IkReal x968=((0.5)*sj0); +IkReal x969=(cj5*sj0); +IkReal x970=((0.866025403784439)*cj0); +IkReal x971=(cj5*r10); +IkReal x972=((0.5)*sj3); +IkReal x973=(cj0*cj5); +CheckValue x974 = IKatan2WithCheck(IkReal(((((-1.0)*r11*x966*x969))+(((-1.0)*r00*x966*x967))+(((-1.0)*r01*x966*x973))+(((-1.0)*r10*sj0*sj5*x966)))),IkReal(((((-0.866025403784439)*r12*sj0*sj3))+(((-1.0)*r11*sj3*sj5*x968))+(((-1.0)*r02*sj3*x970))+((r00*x972*x973))+((sj3*x968*x971))+(((-1.0)*r01*x967*x972)))),IKFAST_ATAN2_MAGTHRESH); +if(!x974.valid){ +continue; +} +CheckValue x975=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj0*r11*x965))+((r01*sj0*x965))+(((-0.866025403784439)*r00*x969))+((x970*x971))+(((0.5)*cj0*r12))+(((-1.0)*r02*x968)))),-1); +if(!x975.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x974.value)+(((1.5707963267949)*(x975.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x976=IKcos(j4); +IkReal x977=IKsin(j4); +IkReal x978=(r21*sj5); +IkReal x979=(cj5*sj0); +IkReal x980=(sj0*sj5); +IkReal x981=(cj0*r01); +IkReal x982=(cj5*r20); +IkReal x983=(r20*sj5); +IkReal x984=(cj0*sj5); +IkReal x985=(r02*sj0); +IkReal x986=(cj0*r12); +IkReal x987=(cj0*r02); +IkReal x988=(r12*sj0); +IkReal x989=(cj0*cj5); +IkReal x990=((0.5)*x977); +IkReal x991=((1.0)*x976); +IkReal x992=((0.5)*x976); +IkReal x993=(r10*x977); +IkReal x994=(cj5*x977); +IkReal x995=((1.0)*x977); +IkReal x996=((0.866025403784439)*x977); +IkReal x997=((0.866025403784439)*x976); +evalcond[0]=((((-1.0)*r22*x996))+sj3+((x976*x983))+(((-1.0)*x978*x990))+((cj5*r21*x976))+((x982*x990))); +evalcond[1]=((((-1.0)*r22*x997))+(((-1.0)*x978*x992))+(((-1.0)*r21*x994))+(((-1.0)*x983*x995))+(((-0.5)*cj3))+((x982*x992))); +evalcond[2]=((((-1.0)*r01*x980*x990))+((r00*x979*x990))+((x986*x996))+cj3+(((-1.0)*r10*x989*x990))+((r00*x976*x980))+(((-1.0)*x985*x996))+(((-1.0)*r10*x984*x991))+(((-1.0)*r11*x989*x991))+((r11*x984*x990))+((r01*x976*x979))); +evalcond[3]=(((sj5*x981*x990))+((x988*x996))+(((-1.0)*r00*x989*x990))+(((-1.0)*r11*x979*x991))+(((-1.0)*r10*x980*x991))+((r11*x980*x990))+(((-1.0)*r00*x984*x991))+(((-1.0)*r10*x979*x990))+((x987*x996))+(((-1.0)*cj5*x981*x991))); +evalcond[4]=((((-1.0)*r01*x980*x992))+((r00*x979*x992))+((x986*x997))+(((-1.0)*r10*x989*x992))+(((0.5)*sj3))+(((-1.0)*x985*x997))+(((-1.0)*r00*x980*x995))+((r11*x984*x992))+(((-1.0)*r01*x979*x995))+((r11*x977*x989))+((x984*x993))); +evalcond[5]=((0.866025403784439)+((sj5*x981*x992))+((x988*x997))+(((-1.0)*r00*x989*x992))+((r00*x977*x984))+((r11*x980*x992))+((x981*x994))+(((-1.0)*r10*x979*x992))+((x980*x993))+((x987*x997))+((r11*x977*x979))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x998=cj5*cj5; +IkReal x999=r20*r20; +IkReal x1000=r21*r21; +IkReal x1001=(r21*sj5); +IkReal x1002=(cj3*cj5); +IkReal x1003=(r20*sj5); +IkReal x1004=(cj5*sj3); +IkReal x1005=(cj5*r20); +IkReal x1006=((1.73205080756888)*r22); +IkReal x1007=((1.5)*x998); +CheckValue x1008 = IKatan2WithCheck(IkReal((((cj3*x1003))+(((-1.0)*sj3*x1001))+((r21*x1002))+(((-1.0)*sj3*x1006))+((r20*x1004)))),IkReal(((((0.5)*cj3*x1001))+(((0.866025403784439)*cj3*r22))+(((2.0)*sj3*x1003))+(((2.0)*r21*x1004))+(((-0.5)*r20*x1002)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1008.valid){ +continue; +} +CheckValue x1009=IKPowWithIntegerCheck(IKsign(((((-3.0)*x1001*x1005))+(((-0.5)*x1000))+(((-1.5)*(r22*r22)))+(((-2.0)*x999))+((x1007*x999))+((x1005*x1006))+(((-1.0)*x1001*x1006))+(((-1.0)*x1000*x1007)))),-1); +if(!x1009.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1008.value)+(((1.5707963267949)*(x1009.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1010=IKcos(j4); +IkReal x1011=IKsin(j4); +IkReal x1012=(r21*sj5); +IkReal x1013=(cj5*sj0); +IkReal x1014=(sj0*sj5); +IkReal x1015=(cj0*r01); +IkReal x1016=(cj5*r20); +IkReal x1017=(r20*sj5); +IkReal x1018=(cj0*sj5); +IkReal x1019=(r02*sj0); +IkReal x1020=(cj0*r12); +IkReal x1021=(cj0*r02); +IkReal x1022=(r12*sj0); +IkReal x1023=(cj0*cj5); +IkReal x1024=((0.5)*x1011); +IkReal x1025=((1.0)*x1010); +IkReal x1026=((0.5)*x1010); +IkReal x1027=(r10*x1011); +IkReal x1028=(cj5*x1011); +IkReal x1029=((1.0)*x1011); +IkReal x1030=((0.866025403784439)*x1011); +IkReal x1031=((0.866025403784439)*x1010); +evalcond[0]=(sj3+(((-1.0)*r22*x1030))+((x1010*x1017))+((x1016*x1024))+((cj5*r21*x1010))+(((-1.0)*x1012*x1024))); +evalcond[1]=((((-1.0)*r21*x1028))+(((-0.5)*cj3))+(((-1.0)*x1017*x1029))+(((-1.0)*r22*x1031))+((x1016*x1026))+(((-1.0)*x1012*x1026))); +evalcond[2]=(((x1020*x1030))+(((-1.0)*r10*x1023*x1024))+cj3+(((-1.0)*r01*x1014*x1024))+((r00*x1010*x1014))+((r00*x1013*x1024))+((r01*x1010*x1013))+(((-1.0)*x1019*x1030))+(((-1.0)*r10*x1018*x1025))+(((-1.0)*r11*x1023*x1025))+((r11*x1018*x1024))); +evalcond[3]=(((x1021*x1030))+(((-1.0)*r10*x1014*x1025))+((x1022*x1030))+((sj5*x1015*x1024))+(((-1.0)*r00*x1023*x1024))+((r11*x1014*x1024))+(((-1.0)*cj5*x1015*x1025))+(((-1.0)*r00*x1018*x1025))+(((-1.0)*r10*x1013*x1024))+(((-1.0)*r11*x1013*x1025))); +evalcond[4]=(((x1020*x1031))+(((-1.0)*r10*x1023*x1026))+(((-1.0)*r01*x1014*x1026))+(((0.5)*sj3))+((r00*x1013*x1026))+((x1018*x1027))+(((-1.0)*r00*x1014*x1029))+((r11*x1011*x1023))+(((-1.0)*r01*x1013*x1029))+(((-1.0)*x1019*x1031))+((r11*x1018*x1026))); +evalcond[5]=((0.866025403784439)+((r00*x1011*x1018))+((x1021*x1031))+((x1014*x1027))+((x1015*x1028))+((r11*x1011*x1013))+((x1022*x1031))+((sj5*x1015*x1026))+(((-1.0)*r00*x1023*x1026))+((r11*x1014*x1026))+(((-1.0)*r10*x1013*x1026))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x1032=((1.0)*cj5); +if( IKabs((((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*r00*sj0*x1032))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x1032)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*r00*sj0*x1032))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))))+IKsqr(((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x1032))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*r00*sj0*x1032))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))), ((((-0.577350269189626)*r22))+((r21*sj5))+(((-1.0)*r20*x1032)))); +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[2]; +IkReal x1033=((0.866025403784439)*sj5); +IkReal x1034=((0.866025403784439)*cj5); +evalcond[0]=((((-1.0)*r21*x1033))+(((0.5)*r22))+(((0.866025403784439)*(IKcos(j3))))+((r20*x1034))); +evalcond[1]=((((-1.0)*cj0*r10*x1034))+((cj0*r11*x1033))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((0.866025403784439)*(IKsin(j3))))+((r00*sj0*x1034))+(((-1.0)*r01*sj0*x1033))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x1035=r20*r20; +IkReal x1036=cj5*cj5; +IkReal x1037=r21*r21; +IkReal x1038=r22*r22; +IkReal x1039=(cj5*r20); +IkReal x1040=((0.5)*cj3); +IkReal x1041=(r21*sj5); +IkReal x1042=((3.46410161513775)*r22); +IkReal x1043=((1.73205080756888)*r22); +IkReal x1044=(r20*sj5); +IkReal x1045=((1.0)*cj3); +IkReal x1046=((2.0)*sj3); +IkReal x1047=(cj5*r21); +IkReal x1048=((1.5)*x1036); +IkReal x1049=((3.0)*x1036); +j4eval[0]=((((-1.0)*x1037))+(((-4.0)*x1035))+((x1039*x1042))+((x1035*x1049))+(((-6.0)*x1039*x1041))+(((-1.0)*x1041*x1042))+(((-1.0)*x1037*x1049))+(((-3.0)*x1038))); +j4eval[1]=((IKabs(((((-1.0)*x1046*x1047))+(((-1.0)*x1044*x1046))+((x1039*x1040))+(((-0.866025403784439)*cj3*r22))+(((-1.0)*x1040*x1041)))))+(IKabs(((((-1.0)*sj3*x1039))+(((-1.0)*x1044*x1045))+(((-1.0)*x1045*x1047))+((sj3*x1041))+((sj3*x1043)))))); +j4eval[2]=IKsign(((((-0.5)*x1037))+((x1039*x1043))+((x1035*x1048))+(((-1.0)*x1041*x1043))+(((-1.0)*x1037*x1048))+(((-1.5)*x1038))+(((-2.0)*x1035))+(((-3.0)*x1039*x1041)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x1050=(cj0*r12); +IkReal x1051=((0.866025403784439)*sj5); +IkReal x1052=(r01*sj0); +IkReal x1053=(r02*sj0); +IkReal x1054=(cj0*r11); +IkReal x1055=((1.73205080756888)*sj5); +IkReal x1056=(cj0*cj5*r10); +IkReal x1057=(cj5*r00*sj0); +j4eval[0]=((((-1.0)*x1052*x1055))+((x1054*x1055))+x1053+(((1.73205080756888)*x1057))+(((-1.73205080756888)*x1056))+(((-1.0)*x1050))); +j4eval[1]=IKsign(((((0.5)*x1053))+(((-0.5)*x1050))+((x1051*x1054))+(((0.866025403784439)*x1057))+(((-0.866025403784439)*x1056))+(((-1.0)*x1051*x1052)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x1058=(r12*sj0); +IkReal x1059=(cj0*r02); +IkReal x1060=(cj0*cj5*r00); +IkReal x1061=(r11*sj0*sj5); +IkReal x1062=(cj0*r01*sj5); +IkReal x1063=(cj5*r10*sj0); +j4eval[0]=(x1058+x1059+(((-1.73205080756888)*x1062))+(((-1.73205080756888)*x1061))+(((1.73205080756888)*x1063))+(((1.73205080756888)*x1060))); +j4eval[1]=IKsign(((((0.5)*x1059))+(((0.5)*x1058))+(((-0.866025403784439)*x1062))+(((-0.866025403784439)*x1061))+(((0.866025403784439)*x1063))+(((0.866025403784439)*x1060)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1064=((0.5)*sj0); +IkReal x1065=((0.5)*cj5); +IkReal x1066=(cj0*sj3); +IkReal x1067=(cj3*r20); +IkReal x1068=(cj5*sj3); +IkReal x1069=((0.5)*sj5); +IkReal x1070=(cj3*r21); +IkReal x1071=((0.866025403784439)*cj0); +IkReal x1072=(r01*sj5); +IkReal x1073=((0.866025403784439)*sj0); +CheckValue x1074 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+((r01*sj0*x1068))+((sj5*x1067))+(((-1.0)*cj5*r11*x1066))+((cj5*x1070))+(((-1.0)*r10*sj5*x1066)))),IkReal((((x1069*x1070))+((sj3*x1064*x1072))+(((-0.866025403784439)*r12*x1066))+((r02*sj3*x1073))+(((0.866025403784439)*cj3*r22))+(((-1.0)*x1065*x1067))+((r10*x1065*x1066))+(((-1.0)*r00*x1064*x1068))+(((-1.0)*r11*x1066*x1069)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1074.valid){ +continue; +} +CheckValue x1075=IKPowWithIntegerCheck(IKsign((((r12*x1064))+(((-1.0)*x1071*x1072))+(((-1.0)*r11*sj5*x1073))+((cj5*r10*x1073))+((cj5*r00*x1071))+(((0.5)*cj0*r02)))),-1); +if(!x1075.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1074.value)+(((1.5707963267949)*(x1075.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1076=IKcos(j4); +IkReal x1077=IKsin(j4); +IkReal x1078=(r21*sj5); +IkReal x1079=(cj5*sj0); +IkReal x1080=(sj0*sj5); +IkReal x1081=(cj0*r01); +IkReal x1082=(cj5*r20); +IkReal x1083=(r20*sj5); +IkReal x1084=(cj0*sj5); +IkReal x1085=(r02*sj0); +IkReal x1086=(cj0*r12); +IkReal x1087=(cj0*r02); +IkReal x1088=(r12*sj0); +IkReal x1089=(cj0*cj5); +IkReal x1090=((0.5)*x1077); +IkReal x1091=((1.0)*x1076); +IkReal x1092=((0.5)*x1076); +IkReal x1093=(r10*x1077); +IkReal x1094=(cj5*x1077); +IkReal x1095=((1.0)*x1077); +IkReal x1096=((0.866025403784439)*x1077); +IkReal x1097=((0.866025403784439)*x1076); +evalcond[0]=(((cj5*r21*x1076))+(((-1.0)*sj3))+((x1076*x1083))+((x1082*x1090))+(((-1.0)*r22*x1096))+(((-1.0)*x1078*x1090))); +evalcond[1]=((((-1.0)*x1083*x1095))+(((0.5)*cj3))+((x1082*x1092))+(((-1.0)*r21*x1094))+(((-1.0)*r22*x1097))+(((-1.0)*x1078*x1092))); +evalcond[2]=(((r11*x1084*x1090))+((r00*x1076*x1080))+cj3+((x1086*x1096))+((r00*x1079*x1090))+(((-1.0)*r11*x1089*x1091))+(((-1.0)*r01*x1080*x1090))+(((-1.0)*x1085*x1096))+(((-1.0)*r10*x1084*x1091))+((r01*x1076*x1079))+(((-1.0)*r10*x1089*x1090))); +evalcond[3]=(((x1087*x1096))+(((-1.0)*cj5*x1081*x1091))+(((-1.0)*r10*x1079*x1090))+(((-1.0)*r11*x1079*x1091))+(((-1.0)*r10*x1080*x1091))+((x1088*x1096))+(((-1.0)*r00*x1084*x1091))+((r11*x1080*x1090))+((sj5*x1081*x1090))+(((-1.0)*r00*x1089*x1090))); +evalcond[4]=(((r11*x1084*x1092))+((x1084*x1093))+(((-1.0)*r01*x1079*x1095))+((x1086*x1097))+(((-1.0)*r00*x1080*x1095))+((r00*x1079*x1092))+(((0.5)*sj3))+(((-1.0)*r01*x1080*x1092))+(((-1.0)*x1085*x1097))+((r11*x1077*x1089))+(((-1.0)*r10*x1089*x1092))); +evalcond[5]=((-0.866025403784439)+((r00*x1077*x1084))+((x1087*x1097))+(((-1.0)*r10*x1079*x1092))+((x1080*x1093))+((x1088*x1097))+((r11*x1077*x1079))+((r11*x1080*x1092))+((x1081*x1094))+((sj5*x1081*x1092))+(((-1.0)*r00*x1089*x1092))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1098=((0.866025403784439)*sj5); +IkReal x1099=((0.5)*sj0); +IkReal x1100=(r11*sj3); +IkReal x1101=((1.0)*sj0); +IkReal x1102=((0.866025403784439)*cj0); +IkReal x1103=(cj5*r10); +IkReal x1104=((0.5)*cj0); +IkReal x1105=(sj3*sj5); +IkReal x1106=((1.0)*cj0); +IkReal x1107=(cj5*sj3); +IkReal x1108=((0.866025403784439)*sj0); +CheckValue x1109 = IKatan2WithCheck(IkReal(((((-1.0)*r01*x1106*x1107))+(((-1.0)*r00*x1105*x1106))+(((-1.0)*cj5*x1100*x1101))+(((-1.0)*r10*x1101*x1105)))),IkReal((((sj3*x1099*x1103))+(((-1.0)*r01*x1104*x1105))+(((-1.0)*r12*sj3*x1108))+(((-1.0)*sj5*x1099*x1100))+(((-1.0)*r02*sj3*x1102))+((r00*x1104*x1107)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1109.valid){ +continue; +} +CheckValue x1110=IKPowWithIntegerCheck(IKsign(((((-1.0)*r12*x1104))+((cj5*r00*x1108))+((r02*x1099))+((cj0*r11*x1098))+(((-1.0)*r01*sj0*x1098))+(((-1.0)*x1102*x1103)))),-1); +if(!x1110.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1109.value)+(((1.5707963267949)*(x1110.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1111=IKcos(j4); +IkReal x1112=IKsin(j4); +IkReal x1113=(r21*sj5); +IkReal x1114=(cj5*sj0); +IkReal x1115=(sj0*sj5); +IkReal x1116=(cj0*r01); +IkReal x1117=(cj5*r20); +IkReal x1118=(r20*sj5); +IkReal x1119=(cj0*sj5); +IkReal x1120=(r02*sj0); +IkReal x1121=(cj0*r12); +IkReal x1122=(cj0*r02); +IkReal x1123=(r12*sj0); +IkReal x1124=(cj0*cj5); +IkReal x1125=((0.5)*x1112); +IkReal x1126=((1.0)*x1111); +IkReal x1127=((0.5)*x1111); +IkReal x1128=(r10*x1112); +IkReal x1129=(cj5*x1112); +IkReal x1130=((1.0)*x1112); +IkReal x1131=((0.866025403784439)*x1112); +IkReal x1132=((0.866025403784439)*x1111); +evalcond[0]=(((x1111*x1118))+(((-1.0)*sj3))+((cj5*r21*x1111))+(((-1.0)*x1113*x1125))+(((-1.0)*r22*x1131))+((x1117*x1125))); +evalcond[1]=((((-1.0)*x1113*x1127))+(((0.5)*cj3))+(((-1.0)*r22*x1132))+((x1117*x1127))+(((-1.0)*x1118*x1130))+(((-1.0)*r21*x1129))); +evalcond[2]=(((x1121*x1131))+cj3+(((-1.0)*r10*x1124*x1125))+((r00*x1114*x1125))+((r11*x1119*x1125))+(((-1.0)*r01*x1115*x1125))+(((-1.0)*r10*x1119*x1126))+((r00*x1111*x1115))+((r01*x1111*x1114))+(((-1.0)*r11*x1124*x1126))+(((-1.0)*x1120*x1131))); +evalcond[3]=((((-1.0)*r10*x1114*x1125))+((r11*x1115*x1125))+((sj5*x1116*x1125))+(((-1.0)*r00*x1119*x1126))+(((-1.0)*r00*x1124*x1125))+(((-1.0)*cj5*x1116*x1126))+(((-1.0)*r11*x1114*x1126))+((x1123*x1131))+(((-1.0)*r10*x1115*x1126))+((x1122*x1131))); +evalcond[4]=(((x1121*x1132))+(((-1.0)*r10*x1124*x1127))+(((0.5)*sj3))+((x1119*x1128))+((r00*x1114*x1127))+((r11*x1119*x1127))+(((-1.0)*r01*x1115*x1127))+(((-1.0)*r00*x1115*x1130))+((r11*x1112*x1124))+(((-1.0)*r01*x1114*x1130))+(((-1.0)*x1120*x1132))); +evalcond[5]=((-0.866025403784439)+(((-1.0)*r10*x1114*x1127))+((r11*x1115*x1127))+((r00*x1112*x1119))+((sj5*x1116*x1127))+(((-1.0)*r00*x1124*x1127))+((x1123*x1132))+((r11*x1112*x1114))+((x1116*x1129))+((x1122*x1132))+((x1115*x1128))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1133=cj5*cj5; +IkReal x1134=r20*r20; +IkReal x1135=r21*r21; +IkReal x1136=((1.0)*cj5); +IkReal x1137=(r20*sj3); +IkReal x1138=(cj3*r21); +IkReal x1139=(cj5*r20); +IkReal x1140=(r21*sj3); +IkReal x1141=(r21*sj5); +IkReal x1142=((1.73205080756888)*r22); +IkReal x1143=((1.5)*x1133); +CheckValue x1144=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1135*x1143))+((x1134*x1143))+(((-1.5)*(r22*r22)))+(((-0.5)*x1135))+(((-1.0)*x1141*x1142))+(((-2.0)*x1134))+(((-3.0)*x1139*x1141))+((x1139*x1142)))),-1); +if(!x1144.valid){ +continue; +} +CheckValue x1145 = IKatan2WithCheck(IkReal((((sj5*x1140))+((sj3*x1142))+(((-1.0)*x1136*x1138))+(((-1.0)*x1136*x1137))+(((-1.0)*cj3*r20*sj5)))),IkReal(((((0.5)*cj3*x1139))+(((-0.866025403784439)*cj3*r22))+(((-2.0)*cj5*x1140))+(((-0.5)*sj5*x1138))+(((-2.0)*sj5*x1137)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1145.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1144.value)))+(x1145.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1146=IKcos(j4); +IkReal x1147=IKsin(j4); +IkReal x1148=(r21*sj5); +IkReal x1149=(cj5*sj0); +IkReal x1150=(sj0*sj5); +IkReal x1151=(cj0*r01); +IkReal x1152=(cj5*r20); +IkReal x1153=(r20*sj5); +IkReal x1154=(cj0*sj5); +IkReal x1155=(r02*sj0); +IkReal x1156=(cj0*r12); +IkReal x1157=(cj0*r02); +IkReal x1158=(r12*sj0); +IkReal x1159=(cj0*cj5); +IkReal x1160=((0.5)*x1147); +IkReal x1161=((1.0)*x1146); +IkReal x1162=((0.5)*x1146); +IkReal x1163=(r10*x1147); +IkReal x1164=(cj5*x1147); +IkReal x1165=((1.0)*x1147); +IkReal x1166=((0.866025403784439)*x1147); +IkReal x1167=((0.866025403784439)*x1146); +evalcond[0]=((((-1.0)*sj3))+((x1146*x1153))+(((-1.0)*x1148*x1160))+(((-1.0)*r22*x1166))+((x1152*x1160))+((cj5*r21*x1146))); +evalcond[1]=((((-1.0)*r21*x1164))+(((-1.0)*x1148*x1162))+(((-1.0)*x1153*x1165))+(((0.5)*cj3))+(((-1.0)*r22*x1167))+((x1152*x1162))); +evalcond[2]=((((-1.0)*r10*x1159*x1160))+((r11*x1154*x1160))+cj3+(((-1.0)*r01*x1150*x1160))+((r01*x1146*x1149))+((r00*x1149*x1160))+((r00*x1146*x1150))+((x1156*x1166))+(((-1.0)*x1155*x1166))+(((-1.0)*r11*x1159*x1161))+(((-1.0)*r10*x1154*x1161))); +evalcond[3]=((((-1.0)*r10*x1150*x1161))+(((-1.0)*cj5*x1151*x1161))+((x1157*x1166))+((sj5*x1151*x1160))+(((-1.0)*r00*x1159*x1160))+((r11*x1150*x1160))+((x1158*x1166))+(((-1.0)*r11*x1149*x1161))+(((-1.0)*r10*x1149*x1160))+(((-1.0)*r00*x1154*x1161))); +evalcond[4]=((((-1.0)*r10*x1159*x1162))+((r11*x1154*x1162))+(((-1.0)*r01*x1150*x1162))+((r00*x1149*x1162))+(((0.5)*sj3))+((x1154*x1163))+(((-1.0)*r01*x1149*x1165))+((x1156*x1167))+((r11*x1147*x1159))+(((-1.0)*x1155*x1167))+(((-1.0)*r00*x1150*x1165))); +evalcond[5]=((-0.866025403784439)+((r00*x1147*x1154))+((r11*x1147*x1149))+((x1157*x1167))+((sj5*x1151*x1162))+(((-1.0)*r00*x1159*x1162))+((r11*x1150*x1162))+((x1158*x1167))+(((-1.0)*r10*x1149*x1162))+((x1151*x1164))+((x1150*x1163))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x1168=((1.0)*sj5); +if( IKabs((((cj0*cj5*r10))+(((-1.0)*cj0*r11*x1168))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj5*r20))+(((-1.0)*r21*x1168))+(((0.577350269189626)*r22)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-1.0)*cj0*r11*x1168))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))))+IKsqr((((cj5*r20))+(((-1.0)*r21*x1168))+(((0.577350269189626)*r22))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-1.0)*cj0*r11*x1168))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))), (((cj5*r20))+(((-1.0)*r21*x1168))+(((0.577350269189626)*r22)))); +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[2]; +IkReal x1169=((0.866025403784439)*sj5); +IkReal x1170=((0.866025403784439)*cj5); +evalcond[0]=(((r20*x1170))+(((-1.0)*r21*x1169))+(((-0.866025403784439)*(IKcos(j3))))+(((0.5)*r22))); +evalcond[1]=((((-1.0)*r01*sj0*x1169))+((cj0*r11*x1169))+(((-0.5)*cj0*r12))+((r00*sj0*x1170))+(((0.5)*r02*sj0))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*cj0*r10*x1170))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x1171=r20*r20; +IkReal x1172=cj5*cj5; +IkReal x1173=r21*r21; +IkReal x1174=r22*r22; +IkReal x1175=(cj5*r20); +IkReal x1176=((0.5)*cj3); +IkReal x1177=(r21*sj5); +IkReal x1178=(r20*sj5); +IkReal x1179=((2.0)*sj3); +IkReal x1180=(cj5*r21); +IkReal x1181=((1.73205080756888)*r22); +IkReal x1182=((1.5)*x1172); +IkReal x1183=(r22*x1177); +IkReal x1184=((3.0)*x1172); +j4eval[0]=((((3.46410161513775)*r22*x1175))+(((-3.0)*x1174))+(((-4.0)*x1171))+((x1171*x1184))+(((-1.0)*x1173*x1184))+(((-6.0)*x1175*x1177))+(((-3.46410161513775)*x1183))+(((-1.0)*x1173))); +j4eval[1]=((IKabs((((sj3*x1175))+(((-1.0)*sj3*x1177))+((cj3*x1178))+((cj3*x1180))+(((-1.0)*sj3*x1181)))))+(IKabs((((x1179*x1180))+((x1176*x1177))+(((0.866025403784439)*cj3*r22))+(((-1.0)*x1175*x1176))+((x1178*x1179)))))); +j4eval[2]=IKsign(((((-0.5)*x1173))+(((-2.0)*x1171))+(((-3.0)*x1175*x1177))+((x1175*x1181))+(((-1.5)*x1174))+((x1171*x1182))+(((-1.0)*x1173*x1182))+(((-1.0)*x1177*x1181)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x1185=((0.866025403784439)*sj5); +IkReal x1186=(r01*sj0); +IkReal x1187=(r02*sj0); +IkReal x1188=(cj0*r11); +IkReal x1189=(cj0*r12); +IkReal x1190=((1.73205080756888)*sj5); +IkReal x1191=(cj0*cj5*r10); +IkReal x1192=(cj5*r00*sj0); +j4eval[0]=(x1189+((x1186*x1190))+(((-1.0)*x1188*x1190))+(((-1.73205080756888)*x1192))+(((1.73205080756888)*x1191))+(((-1.0)*x1187))); +j4eval[1]=IKsign(((((0.5)*x1189))+(((-0.866025403784439)*x1192))+((x1185*x1186))+(((-0.5)*x1187))+(((0.866025403784439)*x1191))+(((-1.0)*x1185*x1188)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x1193=(r12*sj0); +IkReal x1194=(cj0*r02); +IkReal x1195=(cj0*cj5*r00); +IkReal x1196=(r11*sj0*sj5); +IkReal x1197=(cj0*r01*sj5); +IkReal x1198=(cj5*r10*sj0); +j4eval[0]=((((-1.73205080756888)*x1195))+(((-1.73205080756888)*x1198))+(((1.73205080756888)*x1196))+(((1.73205080756888)*x1197))+(((-1.0)*x1193))+(((-1.0)*x1194))); +j4eval[1]=IKsign(((((-0.866025403784439)*x1195))+(((-0.866025403784439)*x1198))+(((-0.5)*x1194))+(((-0.5)*x1193))+(((0.866025403784439)*x1196))+(((0.866025403784439)*x1197)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1199=((0.5)*sj0); +IkReal x1200=((0.5)*cj5); +IkReal x1201=(cj0*sj3); +IkReal x1202=(cj3*r20); +IkReal x1203=((1.0)*cj5); +IkReal x1204=((1.0)*sj5); +IkReal x1205=(cj5*sj3); +IkReal x1206=((0.5)*sj5); +IkReal x1207=(cj3*r21); +IkReal x1208=(r01*sj5); +IkReal x1209=((0.866025403784439)*cj0); +IkReal x1210=((0.866025403784439)*sj0); +CheckValue x1211=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*r10*x1210))+(((-0.5)*cj0*r02))+((x1208*x1209))+(((-1.0)*cj5*r00*x1209))+((r11*sj5*x1210))+(((-1.0)*r12*x1199)))),-1); +if(!x1211.valid){ +continue; +} +CheckValue x1212 = IKatan2WithCheck(IkReal((((r00*sj0*sj3*sj5))+((r01*sj0*x1205))+(((-1.0)*x1202*x1204))+(((-1.0)*r11*x1201*x1203))+(((-1.0)*x1203*x1207))+(((-1.0)*r10*x1201*x1204)))),IkReal(((((-0.866025403784439)*r12*x1201))+((r10*x1200*x1201))+((r02*sj3*x1210))+((x1200*x1202))+(((-1.0)*r11*x1201*x1206))+(((-0.866025403784439)*cj3*r22))+(((-1.0)*x1206*x1207))+(((-1.0)*r00*x1199*x1205))+((sj3*x1199*x1208)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1212.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1211.value)))+(x1212.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1213=IKcos(j4); +IkReal x1214=IKsin(j4); +IkReal x1215=(r21*sj5); +IkReal x1216=(cj5*sj0); +IkReal x1217=(sj0*sj5); +IkReal x1218=(cj0*r01); +IkReal x1219=(cj5*r20); +IkReal x1220=(r20*sj5); +IkReal x1221=(cj0*sj5); +IkReal x1222=(r02*sj0); +IkReal x1223=(cj0*r12); +IkReal x1224=(cj0*r02); +IkReal x1225=(r12*sj0); +IkReal x1226=(cj0*cj5); +IkReal x1227=((0.5)*x1214); +IkReal x1228=((1.0)*x1213); +IkReal x1229=((0.5)*x1213); +IkReal x1230=(r10*x1214); +IkReal x1231=(cj5*x1214); +IkReal x1232=((1.0)*x1214); +IkReal x1233=((0.866025403784439)*x1214); +IkReal x1234=((0.866025403784439)*x1213); +evalcond[0]=(sj3+((x1219*x1227))+((cj5*r21*x1213))+(((-1.0)*x1215*x1227))+((x1213*x1220))+(((-1.0)*r22*x1233))); +evalcond[1]=(((x1219*x1229))+(((-1.0)*x1220*x1232))+(((-1.0)*x1215*x1229))+(((-1.0)*r21*x1231))+(((-0.5)*cj3))+(((-1.0)*r22*x1234))); +evalcond[2]=(((r01*x1213*x1216))+(((-1.0)*r10*x1226*x1227))+cj3+(((-1.0)*r01*x1217*x1227))+((x1223*x1233))+(((-1.0)*x1222*x1233))+(((-1.0)*r11*x1226*x1228))+(((-1.0)*r10*x1221*x1228))+((r00*x1216*x1227))+((r11*x1221*x1227))+((r00*x1213*x1217))); +evalcond[3]=(((r11*x1217*x1227))+(((-1.0)*cj5*x1218*x1228))+(((-1.0)*r00*x1226*x1227))+(((-1.0)*r10*x1216*x1227))+((x1225*x1233))+((x1224*x1233))+((sj5*x1218*x1227))+(((-1.0)*r10*x1217*x1228))+(((-1.0)*r11*x1216*x1228))+(((-1.0)*r00*x1221*x1228))); +evalcond[4]=(((r11*x1214*x1226))+(((-1.0)*r10*x1226*x1229))+(((-1.0)*r01*x1217*x1229))+((x1223*x1234))+(((0.5)*sj3))+(((-1.0)*r01*x1216*x1232))+(((-1.0)*x1222*x1234))+((r00*x1216*x1229))+((x1221*x1230))+((r11*x1221*x1229))+(((-1.0)*r00*x1217*x1232))); +evalcond[5]=((0.866025403784439)+((r11*x1214*x1216))+((x1217*x1230))+((r11*x1217*x1229))+(((-1.0)*r00*x1226*x1229))+(((-1.0)*r10*x1216*x1229))+((x1225*x1234))+((r00*x1214*x1221))+((x1224*x1234))+((sj5*x1218*x1229))+((x1218*x1231))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1235=((0.866025403784439)*sj5); +IkReal x1236=((1.0)*sj3); +IkReal x1237=(cj0*sj5); +IkReal x1238=((0.5)*sj0); +IkReal x1239=(cj5*sj0); +IkReal x1240=((0.866025403784439)*cj0); +IkReal x1241=(cj5*r10); +IkReal x1242=((0.5)*sj3); +IkReal x1243=(cj0*cj5); +CheckValue x1244=IKPowWithIntegerCheck(IKsign(((((-0.866025403784439)*r00*x1239))+((x1240*x1241))+((r01*sj0*x1235))+(((-1.0)*cj0*r11*x1235))+(((0.5)*cj0*r12))+(((-1.0)*r02*x1238)))),-1); +if(!x1244.valid){ +continue; +} +CheckValue x1245 = IKatan2WithCheck(IkReal(((((-1.0)*r01*x1236*x1243))+(((-1.0)*r10*sj0*sj5*x1236))+(((-1.0)*r11*x1236*x1239))+(((-1.0)*r00*x1236*x1237)))),IkReal((((sj3*x1238*x1241))+(((-0.866025403784439)*r12*sj0*sj3))+(((-1.0)*r01*x1237*x1242))+((r00*x1242*x1243))+(((-1.0)*r02*sj3*x1240))+(((-1.0)*r11*sj3*sj5*x1238)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1245.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1244.value)))+(x1245.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1246=IKcos(j4); +IkReal x1247=IKsin(j4); +IkReal x1248=(r21*sj5); +IkReal x1249=(cj5*sj0); +IkReal x1250=(sj0*sj5); +IkReal x1251=(cj0*r01); +IkReal x1252=(cj5*r20); +IkReal x1253=(r20*sj5); +IkReal x1254=(cj0*sj5); +IkReal x1255=(r02*sj0); +IkReal x1256=(cj0*r12); +IkReal x1257=(cj0*r02); +IkReal x1258=(r12*sj0); +IkReal x1259=(cj0*cj5); +IkReal x1260=((0.5)*x1247); +IkReal x1261=((1.0)*x1246); +IkReal x1262=((0.5)*x1246); +IkReal x1263=(r10*x1247); +IkReal x1264=(cj5*x1247); +IkReal x1265=((1.0)*x1247); +IkReal x1266=((0.866025403784439)*x1247); +IkReal x1267=((0.866025403784439)*x1246); +evalcond[0]=(((x1246*x1253))+sj3+(((-1.0)*r22*x1266))+(((-1.0)*x1248*x1260))+((x1252*x1260))+((cj5*r21*x1246))); +evalcond[1]=((((-1.0)*x1253*x1265))+(((-1.0)*r22*x1267))+(((-1.0)*r21*x1264))+(((-1.0)*x1248*x1262))+((x1252*x1262))+(((-0.5)*cj3))); +evalcond[2]=(cj3+((r11*x1254*x1260))+((x1256*x1266))+(((-1.0)*x1255*x1266))+(((-1.0)*r10*x1254*x1261))+(((-1.0)*r10*x1259*x1260))+(((-1.0)*r01*x1250*x1260))+((r01*x1246*x1249))+(((-1.0)*r11*x1259*x1261))+((r00*x1249*x1260))+((r00*x1246*x1250))); +evalcond[3]=((((-1.0)*r10*x1249*x1260))+((sj5*x1251*x1260))+((x1258*x1266))+(((-1.0)*r10*x1250*x1261))+(((-1.0)*r00*x1254*x1261))+(((-1.0)*r11*x1249*x1261))+(((-1.0)*cj5*x1251*x1261))+((x1257*x1266))+(((-1.0)*r00*x1259*x1260))+((r11*x1250*x1260))); +evalcond[4]=((((-1.0)*r01*x1249*x1265))+(((0.5)*sj3))+((r11*x1254*x1262))+((x1256*x1267))+(((-1.0)*x1255*x1267))+(((-1.0)*r00*x1250*x1265))+((r11*x1247*x1259))+((x1254*x1263))+(((-1.0)*r10*x1259*x1262))+(((-1.0)*r01*x1250*x1262))+((r00*x1249*x1262))); +evalcond[5]=((0.866025403784439)+((r11*x1247*x1249))+(((-1.0)*r10*x1249*x1262))+((sj5*x1251*x1262))+((x1258*x1267))+((x1250*x1263))+((x1257*x1267))+(((-1.0)*r00*x1259*x1262))+((x1251*x1264))+((r11*x1250*x1262))+((r00*x1247*x1254))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1268=cj5*cj5; +IkReal x1269=r20*r20; +IkReal x1270=r21*r21; +IkReal x1271=(r21*sj5); +IkReal x1272=(cj3*cj5); +IkReal x1273=(r20*sj5); +IkReal x1274=(cj5*sj3); +IkReal x1275=(cj5*r20); +IkReal x1276=((1.73205080756888)*r22); +IkReal x1277=((1.5)*x1268); +CheckValue x1278=IKPowWithIntegerCheck(IKsign((((x1269*x1277))+((x1275*x1276))+(((-3.0)*x1271*x1275))+(((-1.5)*(r22*r22)))+(((-1.0)*x1270*x1277))+(((-1.0)*x1271*x1276))+(((-2.0)*x1269))+(((-0.5)*x1270)))),-1); +if(!x1278.valid){ +continue; +} +CheckValue x1279 = IKatan2WithCheck(IkReal((((r21*x1272))+((cj3*x1273))+(((-1.0)*sj3*x1276))+(((-1.0)*sj3*x1271))+((r20*x1274)))),IkReal(((((2.0)*r21*x1274))+(((-0.5)*r20*x1272))+(((0.866025403784439)*cj3*r22))+(((2.0)*sj3*x1273))+(((0.5)*cj3*x1271)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1279.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1278.value)))+(x1279.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1280=IKcos(j4); +IkReal x1281=IKsin(j4); +IkReal x1282=(r21*sj5); +IkReal x1283=(cj5*sj0); +IkReal x1284=(sj0*sj5); +IkReal x1285=(cj0*r01); +IkReal x1286=(cj5*r20); +IkReal x1287=(r20*sj5); +IkReal x1288=(cj0*sj5); +IkReal x1289=(r02*sj0); +IkReal x1290=(cj0*r12); +IkReal x1291=(cj0*r02); +IkReal x1292=(r12*sj0); +IkReal x1293=(cj0*cj5); +IkReal x1294=((0.5)*x1281); +IkReal x1295=((1.0)*x1280); +IkReal x1296=((0.5)*x1280); +IkReal x1297=(r10*x1281); +IkReal x1298=(cj5*x1281); +IkReal x1299=((1.0)*x1281); +IkReal x1300=((0.866025403784439)*x1281); +IkReal x1301=((0.866025403784439)*x1280); +evalcond[0]=(sj3+((x1286*x1294))+(((-1.0)*r22*x1300))+(((-1.0)*x1282*x1294))+((cj5*r21*x1280))+((x1280*x1287))); +evalcond[1]=((((-1.0)*x1287*x1299))+((x1286*x1296))+(((-1.0)*r22*x1301))+(((-1.0)*x1282*x1296))+(((-1.0)*r21*x1298))+(((-0.5)*cj3))); +evalcond[2]=((((-1.0)*r11*x1293*x1295))+((r00*x1280*x1284))+cj3+(((-1.0)*r10*x1293*x1294))+((x1290*x1300))+((r11*x1288*x1294))+((r00*x1283*x1294))+((r01*x1280*x1283))+(((-1.0)*r01*x1284*x1294))+(((-1.0)*x1289*x1300))+(((-1.0)*r10*x1288*x1295))); +evalcond[3]=(((x1291*x1300))+(((-1.0)*r10*x1283*x1294))+(((-1.0)*r11*x1283*x1295))+((x1292*x1300))+(((-1.0)*r00*x1293*x1294))+(((-1.0)*r00*x1288*x1295))+((r11*x1284*x1294))+((sj5*x1285*x1294))+(((-1.0)*cj5*x1285*x1295))+(((-1.0)*r10*x1284*x1295))); +evalcond[4]=((((-1.0)*r00*x1284*x1299))+((r11*x1281*x1293))+(((-1.0)*r10*x1293*x1296))+(((0.5)*sj3))+((x1290*x1301))+((r11*x1288*x1296))+(((-1.0)*r01*x1283*x1299))+((x1288*x1297))+((r00*x1283*x1296))+(((-1.0)*r01*x1284*x1296))+(((-1.0)*x1289*x1301))); +evalcond[5]=((0.866025403784439)+((x1291*x1301))+((r00*x1281*x1288))+(((-1.0)*r10*x1283*x1296))+((x1292*x1301))+(((-1.0)*r00*x1293*x1296))+((x1285*x1298))+((r11*x1284*x1296))+((sj5*x1285*x1296))+((r11*x1281*x1283))+((x1284*x1297))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x1302=((1.0)*cj5); +IkReal x1303=(r01*sj5); +IkReal x1304=((0.577350269189626)*r02); +IkReal x1305=(r11*sj5); +IkReal x1306=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*sj0*x1304))+(((-1.0)*cj0*x1305))+((cj0*cj5*r10))+((sj0*x1303))+((cj0*x1306))+(((-1.0)*r00*sj0*x1302)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj0*x1306))+(((-1.0)*r10*sj0*x1302))+(((-1.0)*cj0*x1304))+((sj0*x1305))+(((-1.0)*cj0*r00*x1302))+((cj0*x1303)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*sj0*x1304))+(((-1.0)*cj0*x1305))+((cj0*cj5*r10))+((sj0*x1303))+((cj0*x1306))+(((-1.0)*r00*sj0*x1302))))+IKsqr(((((-1.0)*sj0*x1306))+(((-1.0)*r10*sj0*x1302))+(((-1.0)*cj0*x1304))+((sj0*x1305))+(((-1.0)*cj0*r00*x1302))+((cj0*x1303))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*sj0*x1304))+(((-1.0)*cj0*x1305))+((cj0*cj5*r10))+((sj0*x1303))+((cj0*x1306))+(((-1.0)*r00*sj0*x1302))), ((((-1.0)*sj0*x1306))+(((-1.0)*r10*sj0*x1302))+(((-1.0)*cj0*x1304))+((sj0*x1305))+(((-1.0)*cj0*r00*x1302))+((cj0*x1303)))); +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[2]; +IkReal x1307=((0.866025403784439)*sj5); +IkReal x1308=((0.5)*sj0); +IkReal x1309=((0.5)*cj0); +IkReal x1310=((0.866025403784439)*cj5*r10); +IkReal x1311=((0.866025403784439)*cj5*r00); +evalcond[0]=(((cj0*r11*x1307))+(((-1.0)*r01*sj0*x1307))+(((-1.0)*cj0*x1310))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r12*x1309))+((r02*x1308))+((sj0*x1311))); +evalcond[1]=(((r11*sj0*x1307))+(((-0.866025403784439)*(IKcos(j3))))+(((-1.0)*r02*x1309))+(((-1.0)*sj0*x1310))+(((-1.0)*cj0*x1311))+(((-1.0)*r12*x1308))+((cj0*r01*x1307))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x1312=(r12*sj0); +IkReal x1313=(cj3*sj5); +IkReal x1314=(cj0*r02); +IkReal x1315=(cj3*cj5); +IkReal x1316=(cj0*cj5*r00); +IkReal x1317=(r11*sj0*sj5); +IkReal x1318=(cj0*r01*sj5); +IkReal x1319=(cj5*r10*sj0); +j4eval[0]=(x1314+x1312+(((-1.73205080756888)*x1318))+(((-1.73205080756888)*x1317))+(((1.73205080756888)*x1316))+(((1.73205080756888)*x1319))); +j4eval[1]=((IKabs((((r21*x1315))+((r20*x1313)))))+(((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+((r21*x1313))+(((-1.0)*r20*x1315)))))))); +j4eval[2]=IKsign(((((-0.866025403784439)*x1318))+(((-0.866025403784439)*x1317))+(((0.866025403784439)*x1319))+(((0.866025403784439)*x1316))+(((0.5)*x1312))+(((0.5)*x1314)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x1320=r20*r20; +IkReal x1321=cj5*cj5; +IkReal x1322=r21*r21; +IkReal x1323=r22*r22; +IkReal x1324=(cj5*r20); +IkReal x1325=(r21*sj5); +IkReal x1326=((1.73205080756888)*r22); +IkReal x1327=((3.46410161513775)*r22); +IkReal x1328=(x1321*x1322); +IkReal x1329=(x1320*x1321); +j4eval[0]=((((-1.0)*x1322))+((x1324*x1327))+(((-6.0)*x1324*x1325))+(((-1.0)*x1325*x1327))+(((-3.0)*x1323))+(((-3.0)*x1328))+(((3.0)*x1329))+(((-4.0)*x1320))); +j4eval[1]=((IKabs(((((1.5)*r22))+(((-0.866025403784439)*x1324))+(((0.866025403784439)*x1325)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-3.0)*x1324*x1325))+((x1324*x1326))+(((-0.5)*x1322))+(((-2.0)*x1320))+(((-1.0)*x1325*x1326))+(((1.5)*x1329))+(((-1.5)*x1323))+(((-1.5)*x1328)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x1330=cj5*cj5; +IkReal x1331=(cj0*r01); +IkReal x1332=((0.866025403784439)*r22); +IkReal x1333=(cj5*sj5); +IkReal x1334=(r11*sj0); +IkReal x1335=((0.5)*r21); +IkReal x1336=(cj0*r22); +IkReal x1337=(cj0*r00); +IkReal x1338=((2.0)*r20); +IkReal x1339=(cj5*r20); +IkReal x1340=((3.0)*r20); +IkReal x1341=((4.0)*r20); +IkReal x1342=((1.73205080756888)*cj5); +IkReal x1343=(r21*sj5); +IkReal x1344=((3.0)*sj0); +IkReal x1345=((1.5)*r20); +IkReal x1346=(r12*r22); +IkReal x1347=(cj0*r02); +IkReal x1348=(r10*sj0); +IkReal x1349=((3.0)*r21); +IkReal x1350=((1.5)*x1348); +IkReal x1351=(r20*x1330); +IkReal x1352=((0.866025403784439)*r12*sj0); +IkReal x1353=((1.73205080756888)*r22*sj5); +IkReal x1354=((1.73205080756888)*r12*sj0); +IkReal x1355=((1.5)*r21*x1330); +j4eval[0]=(((x1330*x1334*x1349))+((x1344*x1346))+((x1334*x1353))+((x1333*x1337*x1349))+((x1333*x1334*x1340))+((x1341*x1348))+(((3.0)*r02*x1336))+(((-1.0)*x1339*x1354))+(((-1.0)*x1330*x1340*x1348))+(((1.73205080756888)*x1343*x1347))+((r21*x1331))+((r21*x1334))+((r10*r21*x1333*x1344))+((x1337*x1341))+((x1331*x1353))+(((-1.0)*x1330*x1337*x1340))+(((-1.0)*r22*x1342*x1348))+((x1343*x1354))+(((-1.0)*r00*x1336*x1342))+((x1330*x1331*x1349))+(((-1.73205080756888)*x1339*x1347))+((x1331*x1333*x1340))); +j4eval[1]=((IKabs((((cj3*r20*sj5))+((cj3*cj5*r21)))))+(((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*cj3*x1339))+((cj3*x1343)))))))); +j4eval[2]=IKsign((((x1337*x1338))+((r21*x1333*x1350))+((x1331*x1335))+(((1.5)*sj0*x1346))+(((1.5)*r21*x1333*x1337))+(((-1.0)*cj5*x1332*x1348))+(((-0.866025403784439)*x1339*x1347))+(((-1.0)*cj5*x1332*x1337))+((x1334*x1355))+((sj5*x1332*x1334))+((x1333*x1334*x1345))+(((1.5)*r02*x1336))+(((0.866025403784439)*x1343*x1347))+(((-1.0)*x1339*x1352))+((x1338*x1348))+(((-1.0)*x1330*x1345*x1348))+((x1331*x1355))+(((-1.0)*x1330*x1337*x1345))+((x1343*x1352))+((x1331*x1333*x1345))+((x1334*x1335))+((sj5*x1331*x1332)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x1356=(cj0*r12); +IkReal x1357=((0.866025403784439)*sj5); +IkReal x1358=(r01*sj0); +IkReal x1359=(r02*sj0); +IkReal x1360=(cj0*r11); +IkReal x1361=((1.73205080756888)*sj5); +IkReal x1362=(cj0*cj5*r10); +IkReal x1363=(cj5*r00*sj0); +j4eval[0]=(x1359+(((-1.0)*x1356))+((x1360*x1361))+(((-1.73205080756888)*x1362))+(((1.73205080756888)*x1363))+(((-1.0)*x1358*x1361))); +j4eval[1]=IKsign((((x1357*x1360))+(((-0.866025403784439)*x1362))+(((0.5)*x1359))+(((0.866025403784439)*x1363))+(((-0.5)*x1356))+(((-1.0)*x1357*x1358)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x1364=r20*r20; +IkReal x1365=cj5*cj5; +IkReal x1366=r21*r21; +IkReal x1367=r22*r22; +IkReal x1368=(cj5*r20); +IkReal x1369=(r21*sj5); +IkReal x1370=((1.73205080756888)*r22); +IkReal x1371=((3.46410161513775)*r22); +IkReal x1372=(x1365*x1366); +IkReal x1373=(x1364*x1365); +j4eval[0]=((((-3.0)*x1367))+(((-1.0)*x1366))+(((-6.0)*x1368*x1369))+(((-4.0)*x1364))+(((3.0)*x1373))+(((-3.0)*x1372))+((x1368*x1371))+(((-1.0)*x1369*x1371))); +j4eval[1]=((IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))+(IKabs(((((-0.866025403784439)*x1368))+(((1.5)*r22))+(((0.866025403784439)*x1369)))))); +j4eval[2]=IKsign(((((1.5)*x1373))+(((-3.0)*x1368*x1369))+((x1368*x1370))+(((-1.5)*x1367))+(((-1.5)*x1372))+(((-0.5)*x1366))+(((-2.0)*x1364))+(((-1.0)*x1369*x1370)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x1374=((1.73205080756888)*sj0); +IkReal x1375=((1.73205080756888)*cj0); +IkReal x1376=((((-1.0)*cj5*r10*x1375))+(((-1.0)*r01*sj5*x1374))+(((-1.0)*cj0*r12))+((r02*sj0))+((cj5*r00*x1374))+((r11*sj5*x1375))); +j4eval[0]=x1376; +j4eval[1]=IKsign(x1376); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1377=(r01*sj5); +IkReal x1378=((0.866025403784439)*cj0); +IkReal x1379=((1.73205080756888)*sj0); +IkReal x1380=(cj5*r00); +IkReal x1381=((0.866025403784439)*sj0); +IkReal x1382=(cj5*r10); +IkReal x1383=((1.73205080756888)*cj0); +IkReal x1384=(r11*sj5); +CheckValue x1385=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1382*x1383))+(((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*x1377*x1379))+((x1379*x1380))+((x1383*x1384)))),-1); +if(!x1385.valid){ +continue; +} +CheckValue x1386 = IKatan2WithCheck(IkReal((((x1381*x1384))+(((1.5)*cj0*r02))+((x1377*x1378))+(((-1.0)*x1381*x1382))+(((1.5)*r12*sj0))+(((-1.0)*x1378*x1380)))),IkReal(((((-1.0)*r00*sj5*x1383))+(((-1.0)*cj5*r11*x1379))+(((-1.0)*r10*sj5*x1379))+(((-1.0)*cj5*r01*x1383)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1386.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1385.value)))+(x1386.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1387=IKcos(j4); +IkReal x1388=IKsin(j4); +IkReal x1389=((0.5)*r21); +IkReal x1390=(cj0*r10); +IkReal x1391=((0.5)*sj0); +IkReal x1392=(r10*sj0); +IkReal x1393=((0.5)*r20); +IkReal x1394=((1.0)*sj0); +IkReal x1395=(cj0*r01); +IkReal x1396=(r02*sj0); +IkReal x1397=(cj0*r02); +IkReal x1398=(cj0*r00); +IkReal x1399=(cj0*r11); +IkReal x1400=(sj5*x1388); +IkReal x1401=(cj5*x1388); +IkReal x1402=(sj5*x1387); +IkReal x1403=((0.866025403784439)*x1388); +IkReal x1404=((0.866025403784439)*x1387); +IkReal x1405=(cj5*x1387); +IkReal x1406=(r12*x1404); +evalcond[0]=((((-1.0)*x1389*x1400))+((x1393*x1401))+(((-1.0)*r22*x1403))+((r20*x1402))+((r21*x1405))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*x1389*x1402))+(((-1.0)*r21*x1401))+((x1393*x1405))+(((-1.0)*r22*x1404))+(((-1.0)*r20*x1400))); +evalcond[2]=(((x1392*x1400))+((x1397*x1404))+(((-0.5)*x1398*x1405))+((x1395*x1401))+((sj0*x1406))+((r11*sj0*x1401))+((r11*x1391*x1402))+((x1398*x1400))+(((0.5)*x1395*x1402))+(((-1.0)*r10*x1391*x1405))); +evalcond[3]=(((r00*x1391*x1401))+((r00*sj0*x1402))+(((-0.5)*x1390*x1401))+(((-1.0)*r01*x1391*x1400))+((r01*sj0*x1405))+((cj0*r12*x1403))+(((-1.0)*x1390*x1402))+(((-1.0)*x1396*x1403))+(((0.5)*x1399*x1400))+(((-1.0)*x1399*x1405))); +evalcond[4]=((0.5)+((r00*x1391*x1405))+(((-1.0)*r01*x1394*x1401))+(((-0.5)*x1390*x1405))+((cj0*x1406))+(((-1.0)*r01*x1391*x1402))+((x1399*x1401))+(((-1.0)*r00*x1394*x1400))+(((-1.0)*x1396*x1404))+(((0.5)*x1399*x1402))+((x1390*x1400))); +evalcond[5]=((1.0)+(((-1.0)*x1398*x1402))+((r12*sj0*x1403))+(((-1.0)*x1395*x1405))+((x1397*x1403))+(((-0.5)*x1398*x1401))+(((-1.0)*x1392*x1402))+((r11*x1391*x1400))+(((0.5)*x1395*x1400))+(((-1.0)*r10*x1391*x1401))+(((-1.0)*r11*x1394*x1405))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1407=cj5*cj5; +IkReal x1408=r20*r20; +IkReal x1409=r21*r21; +IkReal x1410=(r21*sj5); +IkReal x1411=(cj5*r20); +IkReal x1412=((1.73205080756888)*r22); +IkReal x1413=((1.5)*x1407); +CheckValue x1414 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x1410))+(((-0.866025403784439)*x1411))+(((1.5)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1414.valid){ +continue; +} +CheckValue x1415=IKPowWithIntegerCheck(IKsign(((((-0.5)*x1409))+(((-2.0)*x1408))+(((-1.5)*(r22*r22)))+((x1411*x1412))+(((-1.0)*x1410*x1412))+(((-1.0)*x1409*x1413))+(((-3.0)*x1410*x1411))+((x1408*x1413)))),-1); +if(!x1415.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1414.value)+(((1.5707963267949)*(x1415.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1416=IKcos(j4); +IkReal x1417=IKsin(j4); +IkReal x1418=((0.5)*r21); +IkReal x1419=(cj0*r10); +IkReal x1420=((0.5)*sj0); +IkReal x1421=(r10*sj0); +IkReal x1422=((0.5)*r20); +IkReal x1423=((1.0)*sj0); +IkReal x1424=(cj0*r01); +IkReal x1425=(r02*sj0); +IkReal x1426=(cj0*r02); +IkReal x1427=(cj0*r00); +IkReal x1428=(cj0*r11); +IkReal x1429=(sj5*x1417); +IkReal x1430=(cj5*x1417); +IkReal x1431=(sj5*x1416); +IkReal x1432=((0.866025403784439)*x1417); +IkReal x1433=((0.866025403784439)*x1416); +IkReal x1434=(cj5*x1416); +IkReal x1435=(r12*x1433); +evalcond[0]=((((-1.0)*r22*x1432))+((r20*x1431))+(((-1.0)*x1418*x1429))+((r21*x1434))+((x1422*x1430))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x1430))+(((-1.0)*r22*x1433))+((x1422*x1434))+(((-1.0)*x1418*x1431))+(((-1.0)*r20*x1429))); +evalcond[2]=((((0.5)*x1424*x1431))+((x1424*x1430))+((sj0*x1435))+(((-1.0)*r10*x1420*x1434))+((x1426*x1433))+((r11*sj0*x1430))+((r11*x1420*x1431))+((x1427*x1429))+((x1421*x1429))+(((-0.5)*x1427*x1434))); +evalcond[3]=(((r00*sj0*x1431))+(((-1.0)*r01*x1420*x1429))+(((-0.5)*x1419*x1430))+(((-1.0)*x1428*x1434))+((cj0*r12*x1432))+(((-1.0)*x1425*x1432))+(((-1.0)*x1419*x1431))+(((0.5)*x1428*x1429))+((r01*sj0*x1434))+((r00*x1420*x1430))); +evalcond[4]=((0.5)+((x1428*x1430))+(((-0.5)*x1419*x1434))+(((0.5)*x1428*x1431))+((cj0*x1435))+(((-1.0)*r00*x1423*x1429))+(((-1.0)*r01*x1423*x1430))+((x1419*x1429))+(((-1.0)*r01*x1420*x1431))+(((-1.0)*x1425*x1433))+((r00*x1420*x1434))); +evalcond[5]=((1.0)+(((-1.0)*r11*x1423*x1434))+((r12*sj0*x1432))+(((-1.0)*x1421*x1431))+((r11*x1420*x1429))+(((-1.0)*r10*x1420*x1430))+((x1426*x1432))+(((-1.0)*x1427*x1431))+(((0.5)*x1424*x1429))+(((-0.5)*x1427*x1430))+(((-1.0)*x1424*x1434))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1436=((0.866025403784439)*sj5); +IkReal x1437=((0.866025403784439)*cj5); +CheckValue x1438 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1438.valid){ +continue; +} +CheckValue x1439=IKPowWithIntegerCheck(IKsign((((r00*sj0*x1437))+(((-1.0)*r01*sj0*x1436))+(((-0.5)*cj0*r12))+(((-1.0)*cj0*r10*x1437))+(((0.5)*r02*sj0))+((cj0*r11*x1436)))),-1); +if(!x1439.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1438.value)+(((1.5707963267949)*(x1439.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1440=IKcos(j4); +IkReal x1441=IKsin(j4); +IkReal x1442=((0.5)*r21); +IkReal x1443=(cj0*r10); +IkReal x1444=((0.5)*sj0); +IkReal x1445=(r10*sj0); +IkReal x1446=((0.5)*r20); +IkReal x1447=((1.0)*sj0); +IkReal x1448=(cj0*r01); +IkReal x1449=(r02*sj0); +IkReal x1450=(cj0*r02); +IkReal x1451=(cj0*r00); +IkReal x1452=(cj0*r11); +IkReal x1453=(sj5*x1441); +IkReal x1454=(cj5*x1441); +IkReal x1455=(sj5*x1440); +IkReal x1456=((0.866025403784439)*x1441); +IkReal x1457=((0.866025403784439)*x1440); +IkReal x1458=(cj5*x1440); +IkReal x1459=(r12*x1457); +evalcond[0]=(((x1446*x1454))+(((-1.0)*r22*x1456))+((r20*x1455))+((r21*x1458))+(((-1.0)*x1442*x1453))); +evalcond[1]=((-0.866025403784439)+((x1446*x1458))+(((-1.0)*r21*x1454))+(((-1.0)*r22*x1457))+(((-1.0)*r20*x1453))+(((-1.0)*x1442*x1455))); +evalcond[2]=(((x1445*x1453))+((x1450*x1457))+((x1448*x1454))+(((-1.0)*r10*x1444*x1458))+((sj0*x1459))+((r11*x1444*x1455))+((r11*sj0*x1454))+(((-0.5)*x1451*x1458))+(((0.5)*x1448*x1455))+((x1451*x1453))); +evalcond[3]=((((-0.5)*x1443*x1454))+((r00*sj0*x1455))+(((-1.0)*r01*x1444*x1453))+(((-1.0)*x1443*x1455))+(((0.5)*x1452*x1453))+((r00*x1444*x1454))+(((-1.0)*x1452*x1458))+((cj0*r12*x1456))+(((-1.0)*x1449*x1456))+((r01*sj0*x1458))); +evalcond[4]=((0.5)+(((-1.0)*r00*x1447*x1453))+(((-0.5)*x1443*x1458))+(((-1.0)*r01*x1444*x1455))+(((0.5)*x1452*x1455))+((r00*x1444*x1458))+((cj0*x1459))+(((-1.0)*r01*x1447*x1454))+(((-1.0)*x1449*x1457))+((x1452*x1454))+((x1443*x1453))); +evalcond[5]=((1.0)+((x1450*x1456))+((r12*sj0*x1456))+(((-1.0)*r10*x1444*x1454))+(((-1.0)*x1448*x1458))+(((-1.0)*x1445*x1455))+(((-1.0)*x1451*x1455))+((r11*x1444*x1453))+(((-1.0)*r11*x1447*x1458))+(((-0.5)*x1451*x1454))+(((0.5)*x1448*x1453))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x1460=((0.866025403784439)*sj5); +IkReal x1461=(r01*sj0); +IkReal x1462=(r02*sj0); +IkReal x1463=(cj0*r11); +IkReal x1464=(cj0*r12); +IkReal x1465=((1.73205080756888)*sj5); +IkReal x1466=(cj0*cj5*r10); +IkReal x1467=(cj5*r00*sj0); +j4eval[0]=((((1.73205080756888)*x1466))+((x1461*x1465))+(((-1.0)*x1463*x1465))+x1464+(((-1.0)*x1462))+(((-1.73205080756888)*x1467))); +j4eval[1]=IKsign(((((0.866025403784439)*x1466))+(((-0.5)*x1462))+(((-0.866025403784439)*x1467))+(((0.5)*x1464))+((x1460*x1461))+(((-1.0)*x1460*x1463)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x1468=r20*r20; +IkReal x1469=cj5*cj5; +IkReal x1470=r21*r21; +IkReal x1471=r22*r22; +IkReal x1472=(cj5*r20); +IkReal x1473=(r21*sj5); +IkReal x1474=((1.73205080756888)*r22); +IkReal x1475=((3.46410161513775)*r22); +IkReal x1476=(x1469*x1470); +IkReal x1477=(x1468*x1469); +j4eval[0]=((((-3.0)*x1471))+(((-3.0)*x1476))+(((-4.0)*x1468))+(((-1.0)*x1470))+((x1472*x1475))+(((3.0)*x1477))+(((-6.0)*x1472*x1473))+(((-1.0)*x1473*x1475))); +j4eval[1]=((IKabs(((((1.5)*r22))+(((0.866025403784439)*x1473))+(((-0.866025403784439)*x1472)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-3.0)*x1472*x1473))+(((-1.5)*x1476))+(((-1.5)*x1471))+(((-2.0)*x1468))+(((1.5)*x1477))+((x1472*x1474))+(((-0.5)*x1470))+(((-1.0)*x1473*x1474)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x1478=((1.73205080756888)*sj0); +IkReal x1479=((1.73205080756888)*cj0); +IkReal x1480=((((-1.0)*cj5*r10*x1479))+((r11*sj5*x1479))+(((-1.0)*cj0*r12))+((r02*sj0))+((cj5*r00*x1478))+(((-1.0)*r01*sj5*x1478))); +j4eval[0]=x1480; +j4eval[1]=IKsign(x1480); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1481=(r01*sj5); +IkReal x1482=((0.866025403784439)*cj0); +IkReal x1483=((1.73205080756888)*sj0); +IkReal x1484=(cj5*r00); +IkReal x1485=((0.866025403784439)*sj0); +IkReal x1486=(cj5*r10); +IkReal x1487=((1.73205080756888)*cj0); +IkReal x1488=(r11*sj5); +CheckValue x1489=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1481*x1483))+((x1487*x1488))+(((-1.0)*x1486*x1487))+(((-1.0)*cj0*r12))+((r02*sj0))+((x1483*x1484)))),-1); +if(!x1489.valid){ +continue; +} +CheckValue x1490 = IKatan2WithCheck(IkReal(((((-1.0)*x1482*x1484))+(((1.5)*cj0*r02))+(((-1.0)*x1485*x1486))+((x1481*x1482))+((x1485*x1488))+(((1.5)*r12*sj0)))),IkReal(((((-1.0)*r10*sj5*x1483))+(((-1.0)*cj5*r11*x1483))+(((-1.0)*r00*sj5*x1487))+(((-1.0)*cj5*r01*x1487)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1490.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1489.value)))+(x1490.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1491=IKcos(j4); +IkReal x1492=IKsin(j4); +IkReal x1493=((0.5)*r21); +IkReal x1494=(cj0*r10); +IkReal x1495=((0.5)*sj0); +IkReal x1496=(r10*sj0); +IkReal x1497=((0.5)*r20); +IkReal x1498=((1.0)*sj0); +IkReal x1499=(cj0*r01); +IkReal x1500=(r02*sj0); +IkReal x1501=(cj0*r02); +IkReal x1502=(cj0*r00); +IkReal x1503=(cj0*r11); +IkReal x1504=(sj5*x1492); +IkReal x1505=(cj5*x1492); +IkReal x1506=(sj5*x1491); +IkReal x1507=((0.866025403784439)*x1492); +IkReal x1508=((0.866025403784439)*x1491); +IkReal x1509=(cj5*x1491); +IkReal x1510=(r12*x1508); +evalcond[0]=((((-1.0)*r22*x1507))+((r20*x1506))+((x1497*x1505))+((r21*x1509))+(((-1.0)*x1493*x1504))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r22*x1508))+(((-1.0)*r21*x1505))+(((-1.0)*r20*x1504))+((x1497*x1509))+(((-1.0)*x1493*x1506))); +evalcond[2]=(((x1499*x1505))+((x1501*x1508))+(((-0.5)*x1502*x1509))+((sj0*x1510))+(((0.5)*x1499*x1506))+((x1496*x1504))+(((-1.0)*r10*x1495*x1509))+((r11*sj0*x1505))+((r11*x1495*x1506))+((x1502*x1504))); +evalcond[3]=(((r01*sj0*x1509))+(((-1.0)*x1503*x1509))+(((-1.0)*r01*x1495*x1504))+(((-0.5)*x1494*x1505))+(((0.5)*x1503*x1504))+((r00*sj0*x1506))+((r00*x1495*x1505))+(((-1.0)*x1500*x1507))+(((-1.0)*x1494*x1506))+((cj0*r12*x1507))); +evalcond[4]=((-0.5)+((cj0*x1510))+((x1494*x1504))+(((-1.0)*r01*x1495*x1506))+(((-0.5)*x1494*x1509))+(((0.5)*x1503*x1506))+(((-1.0)*r00*x1498*x1504))+((r00*x1495*x1509))+(((-1.0)*x1500*x1508))+((x1503*x1505))+(((-1.0)*r01*x1498*x1505))); +evalcond[5]=((-1.0)+(((-1.0)*x1496*x1506))+((x1501*x1507))+(((-0.5)*x1502*x1505))+(((0.5)*x1499*x1504))+((r12*sj0*x1507))+(((-1.0)*r10*x1495*x1505))+((r11*x1495*x1504))+(((-1.0)*r11*x1498*x1509))+(((-1.0)*x1499*x1509))+(((-1.0)*x1502*x1506))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1511=cj5*cj5; +IkReal x1512=r20*r20; +IkReal x1513=r21*r21; +IkReal x1514=(r21*sj5); +IkReal x1515=(cj5*r20); +IkReal x1516=((1.73205080756888)*r22); +IkReal x1517=((1.5)*x1511); +CheckValue x1518 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x1514))+(((1.5)*r22))+(((-0.866025403784439)*x1515)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1518.valid){ +continue; +} +CheckValue x1519=IKPowWithIntegerCheck(IKsign(((((-3.0)*x1514*x1515))+(((-0.5)*x1513))+(((-1.5)*(r22*r22)))+(((-1.0)*x1513*x1517))+(((-1.0)*x1514*x1516))+((x1515*x1516))+(((-2.0)*x1512))+((x1512*x1517)))),-1); +if(!x1519.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1518.value)+(((1.5707963267949)*(x1519.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1520=IKcos(j4); +IkReal x1521=IKsin(j4); +IkReal x1522=((0.5)*r21); +IkReal x1523=(cj0*r10); +IkReal x1524=((0.5)*sj0); +IkReal x1525=(r10*sj0); +IkReal x1526=((0.5)*r20); +IkReal x1527=((1.0)*sj0); +IkReal x1528=(cj0*r01); +IkReal x1529=(r02*sj0); +IkReal x1530=(cj0*r02); +IkReal x1531=(cj0*r00); +IkReal x1532=(cj0*r11); +IkReal x1533=(sj5*x1521); +IkReal x1534=(cj5*x1521); +IkReal x1535=(sj5*x1520); +IkReal x1536=((0.866025403784439)*x1521); +IkReal x1537=((0.866025403784439)*x1520); +IkReal x1538=(cj5*x1520); +IkReal x1539=(r12*x1537); +evalcond[0]=((((-1.0)*r22*x1536))+((r20*x1535))+((r21*x1538))+((x1526*x1534))+(((-1.0)*x1522*x1533))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r22*x1537))+(((-1.0)*r21*x1534))+((x1526*x1538))+(((-1.0)*x1522*x1535))+(((-1.0)*r20*x1533))); +evalcond[2]=(((x1525*x1533))+((r11*x1524*x1535))+(((-0.5)*x1531*x1538))+((x1528*x1534))+((sj0*x1539))+((x1531*x1533))+((x1530*x1537))+((r11*sj0*x1534))+(((0.5)*x1528*x1535))+(((-1.0)*r10*x1524*x1538))); +evalcond[3]=((((-1.0)*x1529*x1536))+(((-1.0)*x1532*x1538))+((r00*sj0*x1535))+(((0.5)*x1532*x1533))+(((-0.5)*x1523*x1534))+(((-1.0)*r01*x1524*x1533))+((cj0*r12*x1536))+((r01*sj0*x1538))+(((-1.0)*x1523*x1535))+((r00*x1524*x1534))); +evalcond[4]=((-0.5)+(((-1.0)*x1529*x1537))+(((-1.0)*r01*x1527*x1534))+((x1523*x1533))+(((0.5)*x1532*x1535))+(((-0.5)*x1523*x1538))+((x1532*x1534))+(((-1.0)*r00*x1527*x1533))+(((-1.0)*r01*x1524*x1535))+((cj0*x1539))+((r00*x1524*x1538))); +evalcond[5]=((-1.0)+(((-1.0)*x1531*x1535))+((r11*x1524*x1533))+(((-1.0)*x1525*x1535))+(((-0.5)*x1531*x1534))+((x1530*x1536))+((r12*sj0*x1536))+(((-1.0)*x1528*x1538))+(((-1.0)*r11*x1527*x1538))+(((0.5)*x1528*x1533))+(((-1.0)*r10*x1524*x1534))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1540=((0.866025403784439)*sj5); +IkReal x1541=((0.866025403784439)*cj5); +CheckValue x1542=IKPowWithIntegerCheck(IKsign(((((-0.5)*r02*sj0))+(((-1.0)*cj0*r11*x1540))+(((-1.0)*r00*sj0*x1541))+(((0.5)*cj0*r12))+((r01*sj0*x1540))+((cj0*r10*x1541)))),-1); +if(!x1542.valid){ +continue; +} +CheckValue x1543 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1543.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1542.value)))+(x1543.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1544=IKcos(j4); +IkReal x1545=IKsin(j4); +IkReal x1546=((0.5)*r21); +IkReal x1547=(cj0*r10); +IkReal x1548=((0.5)*sj0); +IkReal x1549=(r10*sj0); +IkReal x1550=((0.5)*r20); +IkReal x1551=((1.0)*sj0); +IkReal x1552=(cj0*r01); +IkReal x1553=(r02*sj0); +IkReal x1554=(cj0*r02); +IkReal x1555=(cj0*r00); +IkReal x1556=(cj0*r11); +IkReal x1557=(sj5*x1545); +IkReal x1558=(cj5*x1545); +IkReal x1559=(sj5*x1544); +IkReal x1560=((0.866025403784439)*x1545); +IkReal x1561=((0.866025403784439)*x1544); +IkReal x1562=(cj5*x1544); +IkReal x1563=(r12*x1561); +evalcond[0]=((((-1.0)*x1546*x1557))+((x1550*x1558))+(((-1.0)*r22*x1560))+((r21*x1562))+((r20*x1559))); +evalcond[1]=((-0.866025403784439)+((x1550*x1562))+(((-1.0)*x1546*x1559))+(((-1.0)*r21*x1558))+(((-1.0)*r22*x1561))+(((-1.0)*r20*x1557))); +evalcond[2]=(((x1552*x1558))+((x1555*x1557))+((x1554*x1561))+(((-0.5)*x1555*x1562))+((x1549*x1557))+((sj0*x1563))+(((0.5)*x1552*x1559))+((r11*x1548*x1559))+((r11*sj0*x1558))+(((-1.0)*r10*x1548*x1562))); +evalcond[3]=(((cj0*r12*x1560))+(((-1.0)*x1547*x1559))+((r00*sj0*x1559))+((r00*x1548*x1558))+(((-1.0)*x1553*x1560))+(((-1.0)*r01*x1548*x1557))+(((-1.0)*x1556*x1562))+(((0.5)*x1556*x1557))+(((-0.5)*x1547*x1558))+((r01*sj0*x1562))); +evalcond[4]=((-0.5)+((x1547*x1557))+((x1556*x1558))+(((-1.0)*r01*x1551*x1558))+((r00*x1548*x1562))+(((-0.5)*x1547*x1562))+((cj0*x1563))+(((-1.0)*x1553*x1561))+(((-1.0)*r01*x1548*x1559))+(((0.5)*x1556*x1559))+(((-1.0)*r00*x1551*x1557))); +evalcond[5]=((-1.0)+((r12*sj0*x1560))+(((-1.0)*x1555*x1559))+(((-0.5)*x1555*x1558))+(((-1.0)*x1552*x1562))+((x1554*x1560))+(((-1.0)*x1549*x1559))+(((0.5)*x1552*x1557))+((r11*x1548*x1557))+(((-1.0)*r11*x1551*x1562))+(((-1.0)*r10*x1548*x1558))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1564=cj5*cj5; +IkReal x1565=(r20*sj0); +IkReal x1566=(cj0*r01); +IkReal x1567=((0.866025403784439)*r22); +IkReal x1568=((0.5)*r21); +IkReal x1569=(cj0*r00); +IkReal x1570=((0.866025403784439)*r12); +IkReal x1571=(cj5*r20); +IkReal x1572=(r21*sj0); +IkReal x1573=((1.5)*sj5); +IkReal x1574=(cj3*sj5); +IkReal x1575=(cj0*r02); +IkReal x1576=(cj5*r21); +IkReal x1577=(cj5*r10); +IkReal x1578=((1.5)*r22); +IkReal x1579=(r11*sj0); +IkReal x1580=((1.5)*x1564); +CheckValue x1581=IKPowWithIntegerCheck(IKsign((((x1566*x1568))+((sj5*x1567*x1579))+(((-1.0)*cj5*x1567*x1569))+(((-0.866025403784439)*x1571*x1575))+(((-1.0)*r10*x1565*x1580))+(((2.0)*r10*x1565))+((x1575*x1578))+((r12*sj0*x1578))+((cj5*r11*x1565*x1573))+((sj5*x1570*x1572))+((x1568*x1579))+(((-1.0)*r20*x1569*x1580))+((x1569*x1573*x1576))+(((-1.0)*cj5*x1565*x1570))+(((0.866025403784439)*r21*sj5*x1575))+((x1566*x1571*x1573))+(((2.0)*r20*x1569))+(((-1.0)*sj0*x1567*x1577))+((x1572*x1573*x1577))+((r21*x1566*x1580))+((r11*x1572*x1580))+((sj5*x1566*x1567)))),-1); +if(!x1581.valid){ +continue; +} +CheckValue x1582 = IKatan2WithCheck(IkReal((((r20*x1574))+((cj3*x1576)))),IkReal((((cj3*x1567))+((x1568*x1574))+(((-0.5)*cj3*x1571)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1582.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1581.value)))+(x1582.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1583=IKcos(j4); +IkReal x1584=IKsin(j4); +IkReal x1585=((0.5)*cj5); +IkReal x1586=(r01*sj0); +IkReal x1587=((1.0)*sj5); +IkReal x1588=(r11*sj0); +IkReal x1589=(r02*sj0); +IkReal x1590=(r00*sj0); +IkReal x1591=(cj0*r02); +IkReal x1592=(cj0*r01); +IkReal x1593=(r10*sj0); +IkReal x1594=(cj0*r00); +IkReal x1595=(cj0*r11); +IkReal x1596=(cj0*r10); +IkReal x1597=(sj5*x1583); +IkReal x1598=(r20*x1584); +IkReal x1599=(cj5*x1584); +IkReal x1600=((0.866025403784439)*x1584); +IkReal x1601=((0.866025403784439)*x1583); +IkReal x1602=(cj5*x1583); +IkReal x1603=(x1584*x1596); +IkReal x1604=(x1584*x1593); +IkReal x1605=(r12*x1601); +IkReal x1606=((0.5)*sj5*x1584); +evalcond[0]=(((r20*x1597))+(((-1.0)*r21*x1606))+((r21*x1602))+(((-1.0)*r22*x1600))+((x1585*x1598))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x1599))+((r20*x1583*x1585))+(((-1.0)*r22*x1601))+(((-0.5)*r21*x1597))+(((-1.0)*x1587*x1598))); +evalcond[2]=((((-1.0)*x1589*x1600))+((x1590*x1597))+((cj0*r12*x1600))+cj3+((x1595*x1606))+(((-1.0)*x1583*x1587*x1596))+((x1584*x1585*x1590))+((x1586*x1602))+(((-1.0)*x1585*x1603))+(((-1.0)*x1595*x1602))+(((-1.0)*x1586*x1606))); +evalcond[3]=((((0.5)*x1592*x1597))+((x1591*x1601))+((x1592*x1599))+((x1588*x1599))+((sj5*x1604))+((sj5*x1584*x1594))+((sj0*x1605))+(((-1.0)*x1583*x1585*x1594))+(((-1.0)*x1583*x1585*x1593))+(((0.5)*x1588*x1597))+(((-0.5)*cj3))); +evalcond[4]=((((-1.0)*x1589*x1601))+((x1583*x1585*x1590))+((sj5*x1603))+(((0.5)*sj3))+(((-1.0)*x1584*x1587*x1590))+((cj0*x1605))+(((-0.5)*x1586*x1597))+(((-1.0)*x1583*x1585*x1596))+(((0.5)*x1595*x1597))+(((-1.0)*x1586*x1599))+((x1595*x1599))); +evalcond[5]=(((r12*sj0*x1600))+sj3+((x1591*x1600))+(((-1.0)*x1583*x1587*x1593))+(((-1.0)*x1583*x1587*x1594))+(((-1.0)*x1592*x1602))+(((-1.0)*x1585*x1604))+((x1592*x1606))+(((-1.0)*x1584*x1585*x1594))+(((-1.0)*x1588*x1602))+((x1588*x1606))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1607=cj5*cj5; +IkReal x1608=r20*r20; +IkReal x1609=r21*r21; +IkReal x1610=(r21*sj5); +IkReal x1611=(cj5*r20); +IkReal x1612=((1.73205080756888)*r22); +IkReal x1613=((1.5)*x1607); +CheckValue x1614 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((-0.866025403784439)*x1611))+(((0.866025403784439)*x1610)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1614.valid){ +continue; +} +CheckValue x1615=IKPowWithIntegerCheck(IKsign(((((-2.0)*x1608))+(((-1.5)*(r22*r22)))+(((-1.0)*x1610*x1612))+((x1608*x1613))+(((-3.0)*x1610*x1611))+(((-1.0)*x1609*x1613))+(((-0.5)*x1609))+((x1611*x1612)))),-1); +if(!x1615.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1614.value)+(((1.5707963267949)*(x1615.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1616=IKcos(j4); +IkReal x1617=IKsin(j4); +IkReal x1618=((0.5)*cj5); +IkReal x1619=(r01*sj0); +IkReal x1620=((1.0)*sj5); +IkReal x1621=(r11*sj0); +IkReal x1622=(r02*sj0); +IkReal x1623=(r00*sj0); +IkReal x1624=(cj0*r02); +IkReal x1625=(cj0*r01); +IkReal x1626=(r10*sj0); +IkReal x1627=(cj0*r00); +IkReal x1628=(cj0*r11); +IkReal x1629=(cj0*r10); +IkReal x1630=(sj5*x1616); +IkReal x1631=(r20*x1617); +IkReal x1632=(cj5*x1617); +IkReal x1633=((0.866025403784439)*x1617); +IkReal x1634=((0.866025403784439)*x1616); +IkReal x1635=(cj5*x1616); +IkReal x1636=(x1617*x1629); +IkReal x1637=(x1617*x1626); +IkReal x1638=(r12*x1634); +IkReal x1639=((0.5)*sj5*x1617); +evalcond[0]=(((r21*x1635))+(((-1.0)*r22*x1633))+((r20*x1630))+((x1618*x1631))+(((-1.0)*r21*x1639))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*x1620*x1631))+((r20*x1616*x1618))+(((-1.0)*r22*x1634))+(((-0.5)*r21*x1630))+(((-1.0)*r21*x1632))); +evalcond[2]=(((x1617*x1618*x1623))+cj3+(((-1.0)*x1622*x1633))+(((-1.0)*x1616*x1620*x1629))+((x1619*x1635))+((x1628*x1639))+((cj0*r12*x1633))+((x1623*x1630))+(((-1.0)*x1628*x1635))+(((-1.0)*x1619*x1639))+(((-1.0)*x1618*x1636))); +evalcond[3]=((((0.5)*x1621*x1630))+((sj0*x1638))+((x1621*x1632))+((x1624*x1634))+((x1625*x1632))+((sj5*x1637))+(((0.5)*x1625*x1630))+((sj5*x1617*x1627))+(((-0.5)*cj3))+(((-1.0)*x1616*x1618*x1627))+(((-1.0)*x1616*x1618*x1626))); +evalcond[4]=(((x1616*x1618*x1623))+(((-1.0)*x1622*x1634))+((x1628*x1632))+(((0.5)*sj3))+(((0.5)*x1628*x1630))+(((-1.0)*x1619*x1632))+((sj5*x1636))+(((-1.0)*x1617*x1620*x1623))+((cj0*x1638))+(((-0.5)*x1619*x1630))+(((-1.0)*x1616*x1618*x1629))); +evalcond[5]=((((-1.0)*x1617*x1618*x1627))+(((-1.0)*x1621*x1635))+sj3+(((-1.0)*x1616*x1620*x1626))+(((-1.0)*x1616*x1620*x1627))+((x1621*x1639))+((x1624*x1633))+((x1625*x1639))+(((-1.0)*x1625*x1635))+((r12*sj0*x1633))+(((-1.0)*x1618*x1637))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1640=((0.866025403784439)*cj0); +IkReal x1641=(cj3*r21); +IkReal x1642=(cj3*r20); +IkReal x1643=((0.866025403784439)*sj0); +CheckValue x1644 = IKatan2WithCheck(IkReal((((sj5*x1642))+((cj5*x1641)))),IkReal(((((-0.5)*cj5*x1642))+(((0.866025403784439)*cj3*r22))+(((0.5)*sj5*x1641)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1644.valid){ +continue; +} +CheckValue x1645=IKPowWithIntegerCheck(IKsign((((cj5*r10*x1643))+((cj5*r00*x1640))+(((0.5)*r12*sj0))+(((-1.0)*r01*sj5*x1640))+(((0.5)*cj0*r02))+(((-1.0)*r11*sj5*x1643)))),-1); +if(!x1645.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1644.value)+(((1.5707963267949)*(x1645.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1646=IKcos(j4); +IkReal x1647=IKsin(j4); +IkReal x1648=((0.5)*cj5); +IkReal x1649=(r01*sj0); +IkReal x1650=((1.0)*sj5); +IkReal x1651=(r11*sj0); +IkReal x1652=(r02*sj0); +IkReal x1653=(r00*sj0); +IkReal x1654=(cj0*r02); +IkReal x1655=(cj0*r01); +IkReal x1656=(r10*sj0); +IkReal x1657=(cj0*r00); +IkReal x1658=(cj0*r11); +IkReal x1659=(cj0*r10); +IkReal x1660=(sj5*x1646); +IkReal x1661=(r20*x1647); +IkReal x1662=(cj5*x1647); +IkReal x1663=((0.866025403784439)*x1647); +IkReal x1664=((0.866025403784439)*x1646); +IkReal x1665=(cj5*x1646); +IkReal x1666=(x1647*x1659); +IkReal x1667=(x1647*x1656); +IkReal x1668=(r12*x1664); +IkReal x1669=((0.5)*sj5*x1647); +evalcond[0]=(((x1648*x1661))+(((-1.0)*r22*x1663))+((r20*x1660))+(((-1.0)*r21*x1669))+((r21*x1665))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x1662))+(((-1.0)*r22*x1664))+(((-1.0)*x1650*x1661))+(((-0.5)*r21*x1660))+((r20*x1646*x1648))); +evalcond[2]=((((-1.0)*x1646*x1650*x1659))+cj3+(((-1.0)*x1652*x1663))+((cj0*r12*x1663))+((x1649*x1665))+((x1658*x1669))+(((-1.0)*x1658*x1665))+(((-1.0)*x1649*x1669))+(((-1.0)*x1648*x1666))+((x1647*x1648*x1653))+((x1653*x1660))); +evalcond[3]=(((sj0*x1668))+((x1654*x1664))+(((0.5)*x1651*x1660))+((x1655*x1662))+((sj5*x1667))+(((-1.0)*x1646*x1648*x1657))+(((-1.0)*x1646*x1648*x1656))+(((-0.5)*cj3))+((x1651*x1662))+(((0.5)*x1655*x1660))+((sj5*x1647*x1657))); +evalcond[4]=(((x1646*x1648*x1653))+(((-1.0)*x1652*x1664))+(((-1.0)*x1647*x1650*x1653))+(((0.5)*sj3))+((sj5*x1666))+(((-1.0)*x1646*x1648*x1659))+(((-0.5)*x1649*x1660))+((cj0*x1668))+((x1658*x1662))+(((0.5)*x1658*x1660))+(((-1.0)*x1649*x1662))); +evalcond[5]=(sj3+(((-1.0)*x1646*x1650*x1657))+(((-1.0)*x1646*x1650*x1656))+(((-1.0)*x1647*x1648*x1657))+((x1654*x1663))+((x1655*x1669))+((r12*sj0*x1663))+(((-1.0)*x1655*x1665))+((x1651*x1669))+(((-1.0)*x1648*x1667))+(((-1.0)*x1651*x1665))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j1), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j2), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x1670=((0.577350269189626)*r02); +IkReal x1671=(cj5*sj0); +IkReal x1672=(sj0*sj5); +IkReal x1673=((1.0)*r11); +IkReal x1674=(cj0*cj5); +IkReal x1675=(cj0*sj5); +IkReal x1676=((0.577350269189626)*r12); +if( IKabs((((cj0*x1676))+(((-1.0)*x1673*x1675))+(((-1.0)*sj0*x1670))+((r10*x1674))+((r01*x1672))+(((-1.0)*r00*x1671)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((r00*x1674))+((sj0*x1676))+((cj0*x1670))+(((-1.0)*x1672*x1673))+((r10*x1671))+(((-1.0)*r01*x1675)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*x1676))+(((-1.0)*x1673*x1675))+(((-1.0)*sj0*x1670))+((r10*x1674))+((r01*x1672))+(((-1.0)*r00*x1671))))+IKsqr((((r00*x1674))+((sj0*x1676))+((cj0*x1670))+(((-1.0)*x1672*x1673))+((r10*x1671))+(((-1.0)*r01*x1675))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*x1676))+(((-1.0)*x1673*x1675))+(((-1.0)*sj0*x1670))+((r10*x1674))+((r01*x1672))+(((-1.0)*r00*x1671))), (((r00*x1674))+((sj0*x1676))+((cj0*x1670))+(((-1.0)*x1672*x1673))+((r10*x1671))+(((-1.0)*r01*x1675)))); +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[2]; +IkReal x1677=((0.866025403784439)*sj5); +IkReal x1678=((0.5)*sj0); +IkReal x1679=((0.5)*cj0); +IkReal x1680=((0.866025403784439)*cj5*r10); +IkReal x1681=((0.866025403784439)*cj5*r00); +evalcond[0]=(((r02*x1678))+((sj0*x1681))+((cj0*r11*x1677))+(((-1.0)*r01*sj0*x1677))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r12*x1679))+(((-1.0)*cj0*x1680))); +evalcond[1]=((((-1.0)*r02*x1679))+(((-1.0)*sj0*x1680))+((cj0*r01*x1677))+(((0.866025403784439)*(IKcos(j3))))+(((-1.0)*r12*x1678))+(((-1.0)*cj0*x1681))+((r11*sj0*x1677))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x1682=(r12*sj0); +IkReal x1683=(cj3*sj5); +IkReal x1684=(cj0*r02); +IkReal x1685=(cj3*cj5); +IkReal x1686=(cj0*cj5*r00); +IkReal x1687=(r11*sj0*sj5); +IkReal x1688=(cj0*r01*sj5); +IkReal x1689=(cj5*r10*sj0); +j4eval[0]=((((-1.73205080756888)*x1688))+(((-1.73205080756888)*x1687))+x1682+x1684+(((1.73205080756888)*x1689))+(((1.73205080756888)*x1686))); +j4eval[1]=((((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x1685))+((r21*x1683)))))))+(IKabs((((r21*x1685))+((r20*x1683)))))); +j4eval[2]=IKsign(((((0.866025403784439)*x1689))+(((0.866025403784439)*x1686))+(((0.5)*x1684))+(((0.5)*x1682))+(((-0.866025403784439)*x1688))+(((-0.866025403784439)*x1687)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x1690=r21*r21; +IkReal x1691=cj5*cj5; +IkReal x1692=r20*r20; +IkReal x1693=r22*r22; +IkReal x1694=(r21*sj5); +IkReal x1695=((1.73205080756888)*r20); +IkReal x1696=(cj5*r20); +IkReal x1697=((1.15470053837925)*r22); +IkReal x1698=(x1690*x1691); +IkReal x1699=(x1691*x1692); +j4eval[0]=((((2.0)*x1694*x1696))+(((-1.0)*x1696*x1697))+x1693+x1698+(((-1.0)*x1699))+(((0.333333333333333)*x1690))+((x1694*x1697))+(((1.33333333333333)*x1692))); +j4eval[1]=IKsign(((((2.0)*x1692))+(((3.0)*x1694*x1696))+(((0.5)*x1690))+(((1.5)*x1698))+(((1.5)*x1693))+(((-1.0)*cj5*r22*x1695))+(((1.73205080756888)*r22*x1694))+(((-1.5)*x1699)))); +j4eval[2]=((IKabs(((((1.5)*r22))+(((0.866025403784439)*x1694))+(((-0.866025403784439)*x1696)))))+(IKabs(((((1.73205080756888)*cj5*r21))+((sj5*x1695)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x1700=cj5*cj5; +IkReal x1701=(cj0*r01); +IkReal x1702=((0.866025403784439)*r22); +IkReal x1703=(cj5*sj5); +IkReal x1704=((0.5)*r21); +IkReal x1705=(cj0*r22); +IkReal x1706=(cj0*r00); +IkReal x1707=((2.0)*r20); +IkReal x1708=(r20*sj0); +IkReal x1709=((0.866025403784439)*r12); +IkReal x1710=((3.0)*r11); +IkReal x1711=((3.0)*sj0); +IkReal x1712=((1.73205080756888)*cj5); +IkReal x1713=(r21*sj5); +IkReal x1714=(r11*sj0); +IkReal x1715=(r12*r22); +IkReal x1716=(r10*sj0); +IkReal x1717=(cj3*cj5); +IkReal x1718=((3.0)*r21); +IkReal x1719=((1.0)*r21); +IkReal x1720=(cj0*r02); +IkReal x1721=((1.5)*x1716); +IkReal x1722=(r20*x1700); +IkReal x1723=((1.73205080756888)*r22*sj5); +IkReal x1724=(r20*x1720); +IkReal x1725=((1.5)*r21*x1700); +j4eval[0]=((((-1.0)*x1714*x1719))+(((-1.0)*x1701*x1719))+(((-3.0)*r02*x1705))+((r22*x1712*x1716))+(((3.0)*r10*x1700*x1708))+(((-4.0)*r20*x1706))+(((-1.73205080756888)*r12*sj0*x1713))+(((3.0)*x1706*x1722))+(((-4.0)*r10*x1708))+(((-1.0)*x1701*x1723))+(((-1.0)*r21*sj0*x1700*x1710))+(((-1.0)*x1711*x1715))+(((-1.0)*x1703*x1706*x1718))+(((-3.0)*r20*x1701*x1703))+(((-1.0)*x1714*x1723))+(((-1.0)*x1700*x1701*x1718))+(((-1.0)*r10*r21*x1703*x1711))+(((-1.73205080756888)*x1713*x1720))+((x1712*x1724))+((r00*x1705*x1712))+((r12*x1708*x1712))+(((-1.0)*x1703*x1708*x1710))); +j4eval[1]=((((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x1717))+((cj3*x1713)))))))+(IKabs((((cj3*r20*sj5))+((r21*x1717)))))); +j4eval[2]=IKsign(((((-1.5)*r11*x1703*x1708))+((cj5*x1702*x1716))+(((-1.0)*x1707*x1716))+(((-1.0)*sj5*x1701*x1702))+(((-1.0)*sj5*x1702*x1714))+(((-1.0)*r21*x1703*x1721))+(((-1.0)*sj0*x1709*x1713))+(((-1.5)*r20*x1701*x1703))+(((-1.0)*x1701*x1725))+((cj5*x1708*x1709))+(((-1.5)*r02*x1705))+(((-0.866025403784439)*x1713*x1720))+(((1.5)*x1706*x1722))+(((-1.0)*x1714*x1725))+(((-1.0)*x1704*x1714))+(((-1.0)*x1706*x1707))+(((-1.0)*x1701*x1704))+(((-1.5)*sj0*x1715))+(((-1.5)*r21*x1703*x1706))+(((0.866025403784439)*cj5*x1724))+(((1.5)*r10*x1700*x1708))+((cj5*x1702*x1706)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x1726=((0.866025403784439)*sj5); +IkReal x1727=(r01*sj0); +IkReal x1728=(r02*sj0); +IkReal x1729=(cj0*r11); +IkReal x1730=(cj0*r12); +IkReal x1731=((1.73205080756888)*sj5); +IkReal x1732=(cj0*cj5*r10); +IkReal x1733=(cj5*r00*sj0); +j4eval[0]=((((-1.73205080756888)*x1733))+x1730+(((-1.0)*x1729*x1731))+((x1727*x1731))+(((-1.0)*x1728))+(((1.73205080756888)*x1732))); +j4eval[1]=IKsign(((((-1.0)*x1726*x1729))+((x1726*x1727))+(((-0.5)*x1728))+(((0.866025403784439)*x1732))+(((0.5)*x1730))+(((-0.866025403784439)*x1733)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x1734=r21*r21; +IkReal x1735=cj5*cj5; +IkReal x1736=r20*r20; +IkReal x1737=r22*r22; +IkReal x1738=(r21*sj5); +IkReal x1739=((1.73205080756888)*r20); +IkReal x1740=(cj5*r20); +IkReal x1741=((1.15470053837925)*r22); +IkReal x1742=(x1734*x1735); +IkReal x1743=(x1735*x1736); +j4eval[0]=((((0.333333333333333)*x1734))+x1737+x1742+(((-1.0)*x1740*x1741))+(((2.0)*x1738*x1740))+(((-1.0)*x1743))+(((1.33333333333333)*x1736))+((x1738*x1741))); +j4eval[1]=IKsign(((((-1.5)*x1743))+(((1.5)*x1737))+(((1.5)*x1742))+(((0.5)*x1734))+(((2.0)*x1736))+(((1.73205080756888)*r22*x1738))+(((3.0)*x1738*x1740))+(((-1.0)*cj5*r22*x1739)))); +j4eval[2]=((IKabs((((sj5*x1739))+(((1.73205080756888)*cj5*r21)))))+(IKabs(((((1.5)*r22))+(((0.866025403784439)*x1738))+(((-0.866025403784439)*x1740)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x1744=((1.73205080756888)*sj0); +IkReal x1745=((1.73205080756888)*cj0); +IkReal x1746=(((r01*sj5*x1744))+(((-1.0)*cj5*r00*x1744))+(((-1.0)*r02*sj0))+((cj5*r10*x1745))+(((-1.0)*r11*sj5*x1745))+((cj0*r12))); +j4eval[0]=x1746; +j4eval[1]=IKsign(x1746); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1747=((1.73205080756888)*sj0); +IkReal x1748=(r01*sj5); +IkReal x1749=((0.866025403784439)*cj0); +IkReal x1750=(cj5*r00); +IkReal x1751=((0.866025403784439)*sj0); +IkReal x1752=(cj5*r10); +IkReal x1753=((1.73205080756888)*cj0); +IkReal x1754=(r11*sj5); +CheckValue x1755 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+(((-1.0)*x1749*x1750))+(((-1.0)*x1751*x1752))+((x1748*x1749))+(((1.5)*r12*sj0))+((x1751*x1754)))),IkReal(((((-1.0)*r00*sj5*x1753))+(((-1.0)*r10*sj5*x1747))+(((-1.0)*cj5*r01*x1753))+(((-1.0)*cj5*r11*x1747)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1755.valid){ +continue; +} +CheckValue x1756=IKPowWithIntegerCheck(IKsign((((x1752*x1753))+((x1747*x1748))+(((-1.0)*x1747*x1750))+(((-1.0)*r02*sj0))+(((-1.0)*x1753*x1754))+((cj0*r12)))),-1); +if(!x1756.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1755.value)+(((1.5707963267949)*(x1756.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1757=IKcos(j4); +IkReal x1758=IKsin(j4); +IkReal x1759=((0.5)*r21); +IkReal x1760=(cj0*r10); +IkReal x1761=((0.5)*sj0); +IkReal x1762=(r10*sj0); +IkReal x1763=((0.5)*r20); +IkReal x1764=((1.0)*sj0); +IkReal x1765=(cj0*r01); +IkReal x1766=(r02*sj0); +IkReal x1767=(cj0*r02); +IkReal x1768=(cj0*r00); +IkReal x1769=(cj0*r11); +IkReal x1770=(sj5*x1758); +IkReal x1771=(cj5*x1758); +IkReal x1772=(sj5*x1757); +IkReal x1773=((0.866025403784439)*x1758); +IkReal x1774=((0.866025403784439)*x1757); +IkReal x1775=(cj5*x1757); +IkReal x1776=(r12*x1774); +evalcond[0]=(((x1763*x1771))+((r20*x1772))+(((-1.0)*x1759*x1770))+((r21*x1775))+(((-1.0)*r22*x1773))); +evalcond[1]=((0.866025403784439)+((x1763*x1775))+(((-1.0)*r21*x1771))+(((-1.0)*x1759*x1772))+(((-1.0)*r20*x1770))+(((-1.0)*r22*x1774))); +evalcond[2]=(((x1767*x1774))+((r11*sj0*x1771))+(((-0.5)*x1768*x1775))+((r11*x1761*x1772))+((x1765*x1771))+((x1762*x1770))+(((0.5)*x1765*x1772))+((sj0*x1776))+(((-1.0)*r10*x1761*x1775))+((x1768*x1770))); +evalcond[3]=(((r01*sj0*x1775))+((cj0*r12*x1773))+(((-0.5)*x1760*x1771))+(((-1.0)*x1760*x1772))+(((0.5)*x1769*x1770))+((r00*sj0*x1772))+(((-1.0)*x1769*x1775))+((r00*x1761*x1771))+(((-1.0)*r01*x1761*x1770))+(((-1.0)*x1766*x1773))); +evalcond[4]=((0.5)+(((-0.5)*x1760*x1775))+((x1760*x1770))+(((-1.0)*r01*x1764*x1771))+(((0.5)*x1769*x1772))+(((-1.0)*r00*x1764*x1770))+((cj0*x1776))+((r00*x1761*x1775))+(((-1.0)*r01*x1761*x1772))+(((-1.0)*x1766*x1774))+((x1769*x1771))); +evalcond[5]=((-1.0)+((x1767*x1773))+(((-0.5)*x1768*x1771))+((r11*x1761*x1770))+(((-1.0)*x1765*x1775))+(((-1.0)*x1768*x1772))+(((0.5)*x1765*x1770))+(((-1.0)*r11*x1764*x1775))+(((-1.0)*x1762*x1772))+(((-1.0)*r10*x1761*x1771))+((r12*sj0*x1773))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1777=r21*r21; +IkReal x1778=cj5*cj5; +IkReal x1779=r20*r20; +IkReal x1780=(r21*sj5); +IkReal x1781=((1.73205080756888)*cj5); +IkReal x1782=(cj5*r20); +IkReal x1783=((1.5)*x1778); +CheckValue x1784=IKPowWithIntegerCheck(IKsign(((((-1.0)*r20*r22*x1781))+(((3.0)*x1780*x1782))+((x1777*x1783))+(((-1.0)*x1779*x1783))+(((1.5)*(r22*r22)))+(((0.5)*x1777))+(((2.0)*x1779))+(((1.73205080756888)*r22*x1780)))),-1); +if(!x1784.valid){ +continue; +} +CheckValue x1785 = IKatan2WithCheck(IkReal((((r21*x1781))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x1782))+(((1.5)*r22))+(((0.866025403784439)*x1780)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1785.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1784.value)))+(x1785.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1786=IKcos(j4); +IkReal x1787=IKsin(j4); +IkReal x1788=((0.5)*r21); +IkReal x1789=(cj0*r10); +IkReal x1790=((0.5)*sj0); +IkReal x1791=(r10*sj0); +IkReal x1792=((0.5)*r20); +IkReal x1793=((1.0)*sj0); +IkReal x1794=(cj0*r01); +IkReal x1795=(r02*sj0); +IkReal x1796=(cj0*r02); +IkReal x1797=(cj0*r00); +IkReal x1798=(cj0*r11); +IkReal x1799=(sj5*x1787); +IkReal x1800=(cj5*x1787); +IkReal x1801=(sj5*x1786); +IkReal x1802=((0.866025403784439)*x1787); +IkReal x1803=((0.866025403784439)*x1786); +IkReal x1804=(cj5*x1786); +IkReal x1805=(r12*x1803); +evalcond[0]=(((x1792*x1800))+((r21*x1804))+(((-1.0)*x1788*x1799))+((r20*x1801))+(((-1.0)*r22*x1802))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x1800))+(((-1.0)*r20*x1799))+(((-1.0)*x1788*x1801))+((x1792*x1804))+(((-1.0)*r22*x1803))); +evalcond[2]=(((r11*x1790*x1801))+((r11*sj0*x1800))+((sj0*x1805))+(((0.5)*x1794*x1801))+((x1794*x1800))+((x1797*x1799))+((x1796*x1803))+(((-1.0)*r10*x1790*x1804))+(((-0.5)*x1797*x1804))+((x1791*x1799))); +evalcond[3]=((((-1.0)*r01*x1790*x1799))+(((-1.0)*x1798*x1804))+((r00*x1790*x1800))+(((-1.0)*x1795*x1802))+(((0.5)*x1798*x1799))+(((-0.5)*x1789*x1800))+((r00*sj0*x1801))+((cj0*r12*x1802))+(((-1.0)*x1789*x1801))+((r01*sj0*x1804))); +evalcond[4]=((0.5)+((r00*x1790*x1804))+((x1798*x1800))+(((-1.0)*x1795*x1803))+(((0.5)*x1798*x1801))+((cj0*x1805))+(((-0.5)*x1789*x1804))+((x1789*x1799))+(((-1.0)*r01*x1790*x1801))+(((-1.0)*r01*x1793*x1800))+(((-1.0)*r00*x1793*x1799))); +evalcond[5]=((-1.0)+(((-1.0)*x1794*x1804))+(((-1.0)*x1791*x1801))+(((-1.0)*x1797*x1801))+((r12*sj0*x1802))+((x1796*x1802))+(((-1.0)*r10*x1790*x1800))+(((-0.5)*x1797*x1800))+(((0.5)*x1794*x1799))+((r11*x1790*x1799))+(((-1.0)*r11*x1793*x1804))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1806=((0.866025403784439)*sj5); +IkReal x1807=((0.866025403784439)*cj5); +CheckValue x1808 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1808.valid){ +continue; +} +CheckValue x1809=IKPowWithIntegerCheck(IKsign(((((-1.0)*r00*sj0*x1807))+(((-0.5)*r02*sj0))+((cj0*r10*x1807))+(((-1.0)*cj0*r11*x1806))+(((0.5)*cj0*r12))+((r01*sj0*x1806)))),-1); +if(!x1809.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1808.value)+(((1.5707963267949)*(x1809.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1810=IKcos(j4); +IkReal x1811=IKsin(j4); +IkReal x1812=((0.5)*r21); +IkReal x1813=(cj0*r10); +IkReal x1814=((0.5)*sj0); +IkReal x1815=(r10*sj0); +IkReal x1816=((0.5)*r20); +IkReal x1817=((1.0)*sj0); +IkReal x1818=(cj0*r01); +IkReal x1819=(r02*sj0); +IkReal x1820=(cj0*r02); +IkReal x1821=(cj0*r00); +IkReal x1822=(cj0*r11); +IkReal x1823=(sj5*x1811); +IkReal x1824=(cj5*x1811); +IkReal x1825=(sj5*x1810); +IkReal x1826=((0.866025403784439)*x1811); +IkReal x1827=((0.866025403784439)*x1810); +IkReal x1828=(cj5*x1810); +IkReal x1829=(r12*x1827); +evalcond[0]=((((-1.0)*x1812*x1823))+(((-1.0)*r22*x1826))+((x1816*x1824))+((r21*x1828))+((r20*x1825))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x1812*x1825))+(((-1.0)*r20*x1823))+(((-1.0)*r22*x1827))+(((-1.0)*r21*x1824))+((x1816*x1828))); +evalcond[2]=(((r11*sj0*x1824))+((x1818*x1824))+(((-0.5)*x1821*x1828))+(((-1.0)*r10*x1814*x1828))+((x1821*x1823))+((sj0*x1829))+((r11*x1814*x1825))+((x1820*x1827))+(((0.5)*x1818*x1825))+((x1815*x1823))); +evalcond[3]=(((r00*x1814*x1824))+((r00*sj0*x1825))+((cj0*r12*x1826))+(((-1.0)*x1813*x1825))+(((-0.5)*x1813*x1824))+((r01*sj0*x1828))+(((-1.0)*x1819*x1826))+(((-1.0)*r01*x1814*x1823))+(((-1.0)*x1822*x1828))+(((0.5)*x1822*x1823))); +evalcond[4]=((0.5)+((r00*x1814*x1828))+((cj0*x1829))+(((-0.5)*x1813*x1828))+(((-1.0)*r01*x1817*x1824))+(((-1.0)*x1819*x1827))+((x1822*x1824))+(((-1.0)*r01*x1814*x1825))+(((0.5)*x1822*x1825))+(((-1.0)*r00*x1817*x1823))+((x1813*x1823))); +evalcond[5]=((-1.0)+(((-1.0)*x1818*x1828))+(((-0.5)*x1821*x1824))+(((-1.0)*x1821*x1825))+(((-1.0)*r10*x1814*x1824))+((r11*x1814*x1823))+((x1820*x1826))+(((-1.0)*x1815*x1825))+(((0.5)*x1818*x1823))+(((-1.0)*r11*x1817*x1828))+((r12*sj0*x1826))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x1830=(cj0*r12); +IkReal x1831=((0.866025403784439)*sj5); +IkReal x1832=(r01*sj0); +IkReal x1833=(r02*sj0); +IkReal x1834=(cj0*r11); +IkReal x1835=((1.73205080756888)*sj5); +IkReal x1836=(cj0*cj5*r10); +IkReal x1837=(cj5*r00*sj0); +j4eval[0]=((((1.73205080756888)*x1837))+(((-1.0)*x1832*x1835))+(((-1.0)*x1830))+x1833+((x1834*x1835))+(((-1.73205080756888)*x1836))); +j4eval[1]=IKsign(((((-0.866025403784439)*x1836))+(((0.5)*x1833))+(((0.866025403784439)*x1837))+((x1831*x1834))+(((-1.0)*x1831*x1832))+(((-0.5)*x1830)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x1838=r21*r21; +IkReal x1839=cj5*cj5; +IkReal x1840=r20*r20; +IkReal x1841=r22*r22; +IkReal x1842=(r21*sj5); +IkReal x1843=((1.73205080756888)*r20); +IkReal x1844=(cj5*r20); +IkReal x1845=((1.15470053837925)*r22); +IkReal x1846=(x1838*x1839); +IkReal x1847=(x1839*x1840); +j4eval[0]=((((1.33333333333333)*x1840))+x1841+x1846+(((0.333333333333333)*x1838))+(((-1.0)*x1847))+((x1842*x1845))+(((-1.0)*x1844*x1845))+(((2.0)*x1842*x1844))); +j4eval[1]=IKsign(((((-1.5)*x1847))+(((0.5)*x1838))+(((-1.0)*cj5*r22*x1843))+(((1.5)*x1841))+(((1.5)*x1846))+(((3.0)*x1842*x1844))+(((1.73205080756888)*r22*x1842))+(((2.0)*x1840)))); +j4eval[2]=((IKabs((((sj5*x1843))+(((1.73205080756888)*cj5*r21)))))+(IKabs(((((1.5)*r22))+(((-0.866025403784439)*x1844))+(((0.866025403784439)*x1842)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=0; +sj1=0; +cj1=1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x1848=((1.73205080756888)*sj0); +IkReal x1849=((1.73205080756888)*cj0); +IkReal x1850=((((-1.0)*r02*sj0))+((cj5*r10*x1849))+(((-1.0)*r11*sj5*x1849))+((r01*sj5*x1848))+((cj0*r12))+(((-1.0)*cj5*r00*x1848))); +j4eval[0]=x1850; +j4eval[1]=IKsign(x1850); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1851=((1.73205080756888)*sj0); +IkReal x1852=(r01*sj5); +IkReal x1853=((0.866025403784439)*cj0); +IkReal x1854=(cj5*r00); +IkReal x1855=((0.866025403784439)*sj0); +IkReal x1856=(cj5*r10); +IkReal x1857=((1.73205080756888)*cj0); +IkReal x1858=(r11*sj5); +CheckValue x1859=IKPowWithIntegerCheck(IKsign((((x1851*x1852))+(((-1.0)*x1857*x1858))+((x1856*x1857))+(((-1.0)*x1851*x1854))+(((-1.0)*r02*sj0))+((cj0*r12)))),-1); +if(!x1859.valid){ +continue; +} +CheckValue x1860 = IKatan2WithCheck(IkReal((((x1852*x1853))+(((1.5)*cj0*r02))+((x1855*x1858))+(((1.5)*r12*sj0))+(((-1.0)*x1855*x1856))+(((-1.0)*x1853*x1854)))),IkReal(((((-1.0)*r10*sj5*x1851))+(((-1.0)*cj5*r01*x1857))+(((-1.0)*cj5*r11*x1851))+(((-1.0)*r00*sj5*x1857)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1860.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1859.value)))+(x1860.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1861=IKcos(j4); +IkReal x1862=IKsin(j4); +IkReal x1863=((0.5)*r21); +IkReal x1864=(cj0*r10); +IkReal x1865=((0.5)*sj0); +IkReal x1866=(r10*sj0); +IkReal x1867=((0.5)*r20); +IkReal x1868=((1.0)*sj0); +IkReal x1869=(cj0*r01); +IkReal x1870=(r02*sj0); +IkReal x1871=(cj0*r02); +IkReal x1872=(cj0*r00); +IkReal x1873=(cj0*r11); +IkReal x1874=(sj5*x1862); +IkReal x1875=(cj5*x1862); +IkReal x1876=(sj5*x1861); +IkReal x1877=((0.866025403784439)*x1862); +IkReal x1878=((0.866025403784439)*x1861); +IkReal x1879=(cj5*x1861); +IkReal x1880=(r12*x1878); +evalcond[0]=((((-1.0)*r22*x1877))+((r20*x1876))+(((-1.0)*x1863*x1874))+((r21*x1879))+((x1867*x1875))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r22*x1878))+(((-1.0)*r20*x1874))+(((-1.0)*x1863*x1876))+(((-1.0)*r21*x1875))+((x1867*x1879))); +evalcond[2]=(((x1866*x1874))+((x1871*x1878))+(((-1.0)*r10*x1865*x1879))+((x1872*x1874))+((x1869*x1875))+(((0.5)*x1869*x1876))+(((-0.5)*x1872*x1879))+((sj0*x1880))+((r11*sj0*x1875))+((r11*x1865*x1876))); +evalcond[3]=((((-0.5)*x1864*x1875))+((r00*x1865*x1875))+((r00*sj0*x1876))+(((-1.0)*r01*x1865*x1874))+((cj0*r12*x1877))+(((0.5)*x1873*x1874))+(((-1.0)*x1870*x1877))+(((-1.0)*x1873*x1879))+(((-1.0)*x1864*x1876))+((r01*sj0*x1879))); +evalcond[4]=((-0.5)+(((-0.5)*x1864*x1879))+((r00*x1865*x1879))+(((-1.0)*r01*x1865*x1876))+(((0.5)*x1873*x1876))+(((-1.0)*x1870*x1878))+((x1873*x1875))+(((-1.0)*r00*x1868*x1874))+(((-1.0)*r01*x1868*x1875))+((cj0*x1880))+((x1864*x1874))); +evalcond[5]=((1.0)+(((-1.0)*x1869*x1879))+(((-1.0)*r11*x1868*x1879))+((x1871*x1877))+(((-1.0)*x1866*x1876))+(((-1.0)*r10*x1865*x1875))+(((-1.0)*x1872*x1876))+((r12*sj0*x1877))+(((0.5)*x1869*x1874))+(((-0.5)*x1872*x1875))+((r11*x1865*x1874))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1881=r21*r21; +IkReal x1882=cj5*cj5; +IkReal x1883=r20*r20; +IkReal x1884=(r21*sj5); +IkReal x1885=((1.73205080756888)*cj5); +IkReal x1886=(cj5*r20); +IkReal x1887=((1.5)*x1882); +CheckValue x1888=IKPowWithIntegerCheck(IKsign((((x1881*x1887))+(((-1.0)*x1883*x1887))+(((3.0)*x1884*x1886))+(((1.73205080756888)*r22*x1884))+(((1.5)*(r22*r22)))+(((0.5)*x1881))+(((2.0)*x1883))+(((-1.0)*r20*r22*x1885)))),-1); +if(!x1888.valid){ +continue; +} +CheckValue x1889 = IKatan2WithCheck(IkReal((((r21*x1885))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x1884))+(((1.5)*r22))+(((-0.866025403784439)*x1886)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1889.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1888.value)))+(x1889.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1890=IKcos(j4); +IkReal x1891=IKsin(j4); +IkReal x1892=((0.5)*r21); +IkReal x1893=(cj0*r10); +IkReal x1894=((0.5)*sj0); +IkReal x1895=(r10*sj0); +IkReal x1896=((0.5)*r20); +IkReal x1897=((1.0)*sj0); +IkReal x1898=(cj0*r01); +IkReal x1899=(r02*sj0); +IkReal x1900=(cj0*r02); +IkReal x1901=(cj0*r00); +IkReal x1902=(cj0*r11); +IkReal x1903=(sj5*x1891); +IkReal x1904=(cj5*x1891); +IkReal x1905=(sj5*x1890); +IkReal x1906=((0.866025403784439)*x1891); +IkReal x1907=((0.866025403784439)*x1890); +IkReal x1908=(cj5*x1890); +IkReal x1909=(r12*x1907); +evalcond[0]=((((-1.0)*r22*x1906))+(((-1.0)*x1892*x1903))+((r21*x1908))+((r20*x1905))+((x1896*x1904))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x1904))+(((-1.0)*r22*x1907))+(((-1.0)*x1892*x1905))+(((-1.0)*r20*x1903))+((x1896*x1908))); +evalcond[2]=((((-1.0)*r10*x1894*x1908))+((x1901*x1903))+((r11*x1894*x1905))+((x1898*x1904))+((x1900*x1907))+((sj0*x1909))+((x1895*x1903))+(((0.5)*x1898*x1905))+(((-0.5)*x1901*x1908))+((r11*sj0*x1904))); +evalcond[3]=((((-1.0)*x1893*x1905))+(((-1.0)*x1899*x1906))+(((0.5)*x1902*x1903))+(((-1.0)*r01*x1894*x1903))+((r00*x1894*x1904))+(((-1.0)*x1902*x1908))+((cj0*r12*x1906))+((r01*sj0*x1908))+(((-0.5)*x1893*x1904))+((r00*sj0*x1905))); +evalcond[4]=((-0.5)+(((-1.0)*x1899*x1907))+(((0.5)*x1902*x1905))+(((-1.0)*r01*x1894*x1905))+((r00*x1894*x1908))+(((-1.0)*r00*x1897*x1903))+(((-1.0)*r01*x1897*x1904))+((cj0*x1909))+(((-0.5)*x1893*x1908))+((x1902*x1904))+((x1893*x1903))); +evalcond[5]=((1.0)+(((-1.0)*r10*x1894*x1904))+((r12*sj0*x1906))+(((-1.0)*x1895*x1905))+((r11*x1894*x1903))+(((-1.0)*r11*x1897*x1908))+((x1900*x1906))+(((-1.0)*x1901*x1905))+(((-1.0)*x1898*x1908))+(((0.5)*x1898*x1903))+(((-0.5)*x1901*x1904))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1910=((0.866025403784439)*sj5); +IkReal x1911=((0.866025403784439)*cj5); +CheckValue x1912 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1912.valid){ +continue; +} +CheckValue x1913=IKPowWithIntegerCheck(IKsign((((r00*sj0*x1911))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((-1.0)*cj0*r10*x1911))+(((-1.0)*r01*sj0*x1910))+((cj0*r11*x1910)))),-1); +if(!x1913.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1912.value)+(((1.5707963267949)*(x1913.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1914=IKcos(j4); +IkReal x1915=IKsin(j4); +IkReal x1916=((0.5)*r21); +IkReal x1917=(cj0*r10); +IkReal x1918=((0.5)*sj0); +IkReal x1919=(r10*sj0); +IkReal x1920=((0.5)*r20); +IkReal x1921=((1.0)*sj0); +IkReal x1922=(cj0*r01); +IkReal x1923=(r02*sj0); +IkReal x1924=(cj0*r02); +IkReal x1925=(cj0*r00); +IkReal x1926=(cj0*r11); +IkReal x1927=(sj5*x1915); +IkReal x1928=(cj5*x1915); +IkReal x1929=(sj5*x1914); +IkReal x1930=((0.866025403784439)*x1915); +IkReal x1931=((0.866025403784439)*x1914); +IkReal x1932=(cj5*x1914); +IkReal x1933=(r12*x1931); +evalcond[0]=((((-1.0)*x1916*x1927))+(((-1.0)*r22*x1930))+((r20*x1929))+((r21*x1932))+((x1920*x1928))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x1916*x1929))+(((-1.0)*r22*x1931))+(((-1.0)*r21*x1928))+((x1920*x1932))+(((-1.0)*r20*x1927))); +evalcond[2]=((((-1.0)*r10*x1918*x1932))+(((0.5)*x1922*x1929))+((x1924*x1931))+((sj0*x1933))+((x1919*x1927))+(((-0.5)*x1925*x1932))+((r11*sj0*x1928))+((x1925*x1927))+((r11*x1918*x1929))+((x1922*x1928))); +evalcond[3]=((((0.5)*x1926*x1927))+(((-1.0)*x1923*x1930))+(((-1.0)*r01*x1918*x1927))+((r01*sj0*x1932))+((cj0*r12*x1930))+(((-1.0)*x1917*x1929))+(((-1.0)*x1926*x1932))+((r00*sj0*x1929))+(((-0.5)*x1917*x1928))+((r00*x1918*x1928))); +evalcond[4]=((-0.5)+(((-0.5)*x1917*x1932))+(((0.5)*x1926*x1929))+(((-1.0)*x1923*x1931))+(((-1.0)*r01*x1918*x1929))+((x1917*x1927))+(((-1.0)*r01*x1921*x1928))+((cj0*x1933))+(((-1.0)*r00*x1921*x1927))+((x1926*x1928))+((r00*x1918*x1932))); +evalcond[5]=((1.0)+(((-1.0)*r11*x1921*x1932))+(((-1.0)*r10*x1918*x1928))+(((-1.0)*x1925*x1929))+(((0.5)*x1922*x1927))+(((-1.0)*x1922*x1932))+((x1924*x1930))+(((-1.0)*x1919*x1929))+(((-0.5)*x1925*x1928))+((r11*x1918*x1927))+((r12*sj0*x1930))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1934=cj5*cj5; +IkReal x1935=(r20*sj0); +IkReal x1936=(cj0*r01); +IkReal x1937=((0.866025403784439)*r22); +IkReal x1938=((0.5)*r21); +IkReal x1939=(cj0*r00); +IkReal x1940=((0.866025403784439)*r12); +IkReal x1941=(cj5*r20); +IkReal x1942=(r21*sj0); +IkReal x1943=((1.5)*sj5); +IkReal x1944=(cj3*sj5); +IkReal x1945=(cj0*r02); +IkReal x1946=(cj5*r21); +IkReal x1947=(cj5*r10); +IkReal x1948=((1.5)*r22); +IkReal x1949=(r11*sj0); +IkReal x1950=((1.5)*x1934); +CheckValue x1951 = IKatan2WithCheck(IkReal((((cj3*x1946))+((r20*x1944)))),IkReal((((x1938*x1944))+(((-0.5)*cj3*x1941))+((cj3*x1937)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1951.valid){ +continue; +} +CheckValue x1952=IKPowWithIntegerCheck(IKsign(((((-1.0)*r21*x1936*x1950))+(((0.866025403784439)*x1941*x1945))+(((-1.0)*x1945*x1948))+(((-2.0)*r20*x1939))+(((-1.0)*r12*sj0*x1948))+((r10*x1935*x1950))+(((-2.0)*r10*x1935))+(((-1.0)*x1936*x1941*x1943))+((cj5*x1937*x1939))+(((-1.0)*sj5*x1936*x1937))+(((-1.0)*cj5*r11*x1935*x1943))+(((-1.0)*sj5*x1940*x1942))+(((-1.0)*x1938*x1949))+(((-1.0)*r11*x1942*x1950))+(((-0.866025403784439)*r21*sj5*x1945))+((r20*x1939*x1950))+((cj5*x1935*x1940))+(((-1.0)*x1939*x1943*x1946))+(((-1.0)*x1942*x1943*x1947))+(((-1.0)*x1936*x1938))+((sj0*x1937*x1947))+(((-1.0)*sj5*x1937*x1949)))),-1); +if(!x1952.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1951.value)+(((1.5707963267949)*(x1952.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1953=IKcos(j4); +IkReal x1954=IKsin(j4); +IkReal x1955=((0.5)*cj5); +IkReal x1956=(r01*sj0); +IkReal x1957=((1.0)*sj5); +IkReal x1958=(r11*sj0); +IkReal x1959=(r02*sj0); +IkReal x1960=(r00*sj0); +IkReal x1961=(cj0*r02); +IkReal x1962=(cj0*r01); +IkReal x1963=(r10*sj0); +IkReal x1964=(cj0*r00); +IkReal x1965=(cj0*r11); +IkReal x1966=(cj0*r10); +IkReal x1967=(sj5*x1953); +IkReal x1968=(r20*x1954); +IkReal x1969=(cj5*x1954); +IkReal x1970=((0.866025403784439)*x1954); +IkReal x1971=((0.866025403784439)*x1953); +IkReal x1972=(cj5*x1953); +IkReal x1973=(x1954*x1966); +IkReal x1974=(x1954*x1963); +IkReal x1975=(r12*x1971); +IkReal x1976=((0.5)*sj5*x1954); +evalcond[0]=((((-1.0)*r22*x1970))+((r20*x1967))+((r21*x1972))+((x1955*x1968))+(((-1.0)*r21*x1976))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x1957*x1968))+((r20*x1953*x1955))+(((-1.0)*r22*x1971))+(((-0.5)*r21*x1967))+(((-1.0)*r21*x1969))); +evalcond[2]=((((-1.0)*x1959*x1970))+((x1956*x1972))+cj3+(((-1.0)*x1956*x1976))+((x1954*x1955*x1960))+(((-1.0)*x1953*x1957*x1966))+((x1960*x1967))+((x1965*x1976))+(((-1.0)*x1955*x1973))+(((-1.0)*x1965*x1972))+((cj0*r12*x1970))); +evalcond[3]=((((0.5)*x1962*x1967))+((x1958*x1969))+((sj5*x1974))+((sj0*x1975))+(((0.5)*cj3))+((x1961*x1971))+((x1962*x1969))+(((-1.0)*x1953*x1955*x1963))+(((-1.0)*x1953*x1955*x1964))+(((0.5)*x1958*x1967))+((sj5*x1954*x1964))); +evalcond[4]=((((-1.0)*x1959*x1971))+((sj5*x1973))+(((0.5)*sj3))+(((-1.0)*x1956*x1969))+(((0.5)*x1965*x1967))+(((-1.0)*x1954*x1957*x1960))+((x1965*x1969))+(((-0.5)*x1956*x1967))+(((-1.0)*x1953*x1955*x1966))+((x1953*x1955*x1960))+((cj0*x1975))); +evalcond[5]=((((-1.0)*x1962*x1972))+((r12*sj0*x1970))+(((-1.0)*sj3))+((x1958*x1976))+(((-1.0)*x1953*x1957*x1963))+(((-1.0)*x1953*x1957*x1964))+((x1961*x1970))+((x1962*x1976))+(((-1.0)*x1958*x1972))+(((-1.0)*x1955*x1974))+(((-1.0)*x1954*x1955*x1964))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x1977=r21*r21; +IkReal x1978=cj5*cj5; +IkReal x1979=r20*r20; +IkReal x1980=(r21*sj5); +IkReal x1981=((1.73205080756888)*cj5); +IkReal x1982=(cj5*r20); +IkReal x1983=((1.5)*x1978); +CheckValue x1984 = IKatan2WithCheck(IkReal((((r21*x1981))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((0.866025403784439)*x1980))+(((-0.866025403784439)*x1982)))),IKFAST_ATAN2_MAGTHRESH); +if(!x1984.valid){ +continue; +} +CheckValue x1985=IKPowWithIntegerCheck(IKsign(((((2.0)*x1979))+(((3.0)*x1980*x1982))+(((1.73205080756888)*r22*x1980))+(((-1.0)*x1979*x1983))+(((0.5)*x1977))+(((-1.0)*r20*r22*x1981))+(((1.5)*(r22*r22)))+((x1977*x1983)))),-1); +if(!x1985.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x1984.value)+(((1.5707963267949)*(x1985.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x1986=IKcos(j4); +IkReal x1987=IKsin(j4); +IkReal x1988=((0.5)*cj5); +IkReal x1989=(r01*sj0); +IkReal x1990=((1.0)*sj5); +IkReal x1991=(r11*sj0); +IkReal x1992=(r02*sj0); +IkReal x1993=(r00*sj0); +IkReal x1994=(cj0*r02); +IkReal x1995=(cj0*r01); +IkReal x1996=(r10*sj0); +IkReal x1997=(cj0*r00); +IkReal x1998=(cj0*r11); +IkReal x1999=(cj0*r10); +IkReal x2000=(sj5*x1986); +IkReal x2001=(r20*x1987); +IkReal x2002=(cj5*x1987); +IkReal x2003=((0.866025403784439)*x1987); +IkReal x2004=((0.866025403784439)*x1986); +IkReal x2005=(cj5*x1986); +IkReal x2006=(x1987*x1999); +IkReal x2007=(x1987*x1996); +IkReal x2008=(r12*x2004); +IkReal x2009=((0.5)*sj5*x1987); +evalcond[0]=(((r21*x2005))+(((-1.0)*r22*x2003))+((r20*x2000))+(((-1.0)*r21*x2009))+((x1988*x2001))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x1990*x2001))+(((-1.0)*r21*x2002))+(((-0.5)*r21*x2000))+(((-1.0)*r22*x2004))+((r20*x1986*x1988))); +evalcond[2]=((((-1.0)*x1986*x1990*x1999))+cj3+((x1993*x2000))+(((-1.0)*x1992*x2003))+(((-1.0)*x1988*x2006))+(((-1.0)*x1998*x2005))+((x1989*x2005))+(((-1.0)*x1989*x2009))+((cj0*r12*x2003))+((x1987*x1988*x1993))+((x1998*x2009))); +evalcond[3]=((((-1.0)*x1986*x1988*x1996))+(((-1.0)*x1986*x1988*x1997))+(((0.5)*x1991*x2000))+((sj5*x2007))+(((0.5)*cj3))+((sj0*x2008))+((x1994*x2004))+((x1995*x2002))+(((0.5)*x1995*x2000))+((x1991*x2002))+((sj5*x1987*x1997))); +evalcond[4]=((((-1.0)*x1986*x1988*x1999))+(((0.5)*x1998*x2000))+(((-1.0)*x1989*x2002))+(((-1.0)*x1987*x1990*x1993))+((sj5*x2006))+(((0.5)*sj3))+(((-1.0)*x1992*x2004))+(((-0.5)*x1989*x2000))+((x1986*x1988*x1993))+((cj0*x2008))+((x1998*x2002))); +evalcond[5]=((((-1.0)*x1986*x1990*x1997))+(((-1.0)*x1986*x1990*x1996))+(((-1.0)*x1987*x1988*x1997))+(((-1.0)*sj3))+((r12*sj0*x2003))+(((-1.0)*x1988*x2007))+(((-1.0)*x1995*x2005))+((x1994*x2003))+((x1995*x2009))+((x1991*x2009))+(((-1.0)*x1991*x2005))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2010=((0.866025403784439)*cj0); +IkReal x2011=(cj3*r21); +IkReal x2012=(cj3*r20); +IkReal x2013=((0.866025403784439)*sj0); +CheckValue x2014=IKPowWithIntegerCheck(IKsign(((((-1.0)*r11*sj5*x2013))+(((0.5)*r12*sj0))+((cj5*r10*x2013))+(((-1.0)*r01*sj5*x2010))+((cj5*r00*x2010))+(((0.5)*cj0*r02)))),-1); +if(!x2014.valid){ +continue; +} +CheckValue x2015 = IKatan2WithCheck(IkReal((((sj5*x2012))+((cj5*x2011)))),IkReal(((((-0.5)*cj5*x2012))+(((0.5)*sj5*x2011))+(((0.866025403784439)*cj3*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2015.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2014.value)))+(x2015.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2016=IKcos(j4); +IkReal x2017=IKsin(j4); +IkReal x2018=((0.5)*cj5); +IkReal x2019=(r01*sj0); +IkReal x2020=((1.0)*sj5); +IkReal x2021=(r11*sj0); +IkReal x2022=(r02*sj0); +IkReal x2023=(r00*sj0); +IkReal x2024=(cj0*r02); +IkReal x2025=(cj0*r01); +IkReal x2026=(r10*sj0); +IkReal x2027=(cj0*r00); +IkReal x2028=(cj0*r11); +IkReal x2029=(cj0*r10); +IkReal x2030=(sj5*x2016); +IkReal x2031=(r20*x2017); +IkReal x2032=(cj5*x2017); +IkReal x2033=((0.866025403784439)*x2017); +IkReal x2034=((0.866025403784439)*x2016); +IkReal x2035=(cj5*x2016); +IkReal x2036=(x2017*x2029); +IkReal x2037=(x2017*x2026); +IkReal x2038=(r12*x2034); +IkReal x2039=((0.5)*sj5*x2017); +evalcond[0]=(((x2018*x2031))+((r21*x2035))+(((-1.0)*r22*x2033))+((r20*x2030))+(((-1.0)*r21*x2039))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x2020*x2031))+(((-1.0)*r22*x2034))+((r20*x2016*x2018))+(((-0.5)*r21*x2030))+(((-1.0)*r21*x2032))); +evalcond[2]=(cj3+((x2019*x2035))+((x2017*x2018*x2023))+(((-1.0)*x2022*x2033))+(((-1.0)*x2018*x2036))+((x2028*x2039))+((x2023*x2030))+(((-1.0)*x2028*x2035))+(((-1.0)*x2019*x2039))+(((-1.0)*x2016*x2020*x2029))+((cj0*r12*x2033))); +evalcond[3]=(((x2025*x2032))+((x2024*x2034))+((sj5*x2037))+(((0.5)*cj3))+((sj5*x2017*x2027))+(((-1.0)*x2016*x2018*x2026))+(((-1.0)*x2016*x2018*x2027))+(((0.5)*x2021*x2030))+((sj0*x2038))+(((0.5)*x2025*x2030))+((x2021*x2032))); +evalcond[4]=((((-1.0)*x2019*x2032))+(((-1.0)*x2022*x2034))+(((0.5)*x2028*x2030))+(((0.5)*sj3))+((sj5*x2036))+((x2028*x2032))+(((-1.0)*x2016*x2018*x2029))+((cj0*x2038))+((x2016*x2018*x2023))+(((-0.5)*x2019*x2030))+(((-1.0)*x2017*x2020*x2023))); +evalcond[5]=((((-1.0)*x2017*x2018*x2027))+((x2025*x2039))+(((-1.0)*x2021*x2035))+(((-1.0)*sj3))+((x2024*x2033))+((r12*sj0*x2033))+(((-1.0)*x2018*x2037))+((x2021*x2039))+(((-1.0)*x2025*x2035))+(((-1.0)*x2016*x2020*x2027))+(((-1.0)*x2016*x2020*x2026))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j1), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j2), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x2040=((0.577350269189626)*r02); +IkReal x2041=(cj5*sj0); +IkReal x2042=(sj0*sj5); +IkReal x2043=((1.0)*r11); +IkReal x2044=(cj0*cj5); +IkReal x2045=(cj0*sj5); +IkReal x2046=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*sj0*x2040))+((cj0*x2046))+(((-1.0)*r00*x2041))+(((-1.0)*x2043*x2045))+((r01*x2042))+((r10*x2044)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj0*x2040))+(((-1.0)*r01*x2045))+((sj0*x2046))+((r10*x2041))+((r00*x2044))+(((-1.0)*x2042*x2043)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*sj0*x2040))+((cj0*x2046))+(((-1.0)*r00*x2041))+(((-1.0)*x2043*x2045))+((r01*x2042))+((r10*x2044))))+IKsqr((((cj0*x2040))+(((-1.0)*r01*x2045))+((sj0*x2046))+((r10*x2041))+((r00*x2044))+(((-1.0)*x2042*x2043))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*sj0*x2040))+((cj0*x2046))+(((-1.0)*r00*x2041))+(((-1.0)*x2043*x2045))+((r01*x2042))+((r10*x2044))), (((cj0*x2040))+(((-1.0)*r01*x2045))+((sj0*x2046))+((r10*x2041))+((r00*x2044))+(((-1.0)*x2042*x2043)))); +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[2]; +IkReal x2047=((0.866025403784439)*sj5); +IkReal x2048=((0.5)*sj0); +IkReal x2049=((0.5)*cj0); +IkReal x2050=((0.866025403784439)*cj5*r10); +IkReal x2051=((0.866025403784439)*cj5*r00); +evalcond[0]=(((r02*x2048))+((sj0*x2051))+(((-1.0)*r01*sj0*x2047))+(((0.866025403784439)*(IKsin(j3))))+((cj0*r11*x2047))+(((-1.0)*cj0*x2050))+(((-1.0)*r12*x2049))); +evalcond[1]=(((r11*sj0*x2047))+(((-1.0)*r02*x2049))+(((0.866025403784439)*(IKcos(j3))))+(((-1.0)*cj0*x2051))+(((-1.0)*sj0*x2050))+((cj0*r01*x2047))+(((-1.0)*r12*x2048))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x2052=(r12*sj0); +IkReal x2053=(cj3*sj5); +IkReal x2054=(cj0*r02); +IkReal x2055=(cj3*cj5); +IkReal x2056=(cj0*cj5*r00); +IkReal x2057=(r11*sj0*sj5); +IkReal x2058=(cj0*r01*sj5); +IkReal x2059=(cj5*r10*sj0); +j4eval[0]=((((1.73205080756888)*x2059))+(((1.73205080756888)*x2056))+x2052+x2054+(((-1.73205080756888)*x2058))+(((-1.73205080756888)*x2057))); +j4eval[1]=((((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x2055))+((r21*x2053)))))))+(IKabs((((r20*x2053))+((r21*x2055)))))); +j4eval[2]=IKsign(((((0.5)*x2054))+(((0.5)*x2052))+(((0.866025403784439)*x2059))+(((0.866025403784439)*x2056))+(((-0.866025403784439)*x2058))+(((-0.866025403784439)*x2057)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x2060=r21*r21; +IkReal x2061=cj5*cj5; +IkReal x2062=r20*r20; +IkReal x2063=r22*r22; +IkReal x2064=(r21*sj5); +IkReal x2065=((1.73205080756888)*r20); +IkReal x2066=(cj5*r20); +IkReal x2067=((1.15470053837925)*r22); +IkReal x2068=(x2060*x2061); +IkReal x2069=(x2061*x2062); +j4eval[0]=((((0.333333333333333)*x2060))+(((2.0)*x2064*x2066))+x2068+x2063+(((1.33333333333333)*x2062))+(((-1.0)*x2069))+(((-1.0)*x2066*x2067))+((x2064*x2067))); +j4eval[1]=IKsign(((((-1.0)*cj5*r22*x2065))+(((2.0)*x2062))+(((1.5)*x2063))+(((1.5)*x2068))+(((-1.5)*x2069))+(((3.0)*x2064*x2066))+(((1.73205080756888)*r22*x2064))+(((0.5)*x2060)))); +j4eval[2]=((IKabs(((((1.73205080756888)*cj5*r21))+((sj5*x2065)))))+(IKabs(((((0.866025403784439)*x2064))+(((1.5)*r22))+(((-0.866025403784439)*x2066)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x2070=cj5*cj5; +IkReal x2071=(cj0*r01); +IkReal x2072=((0.866025403784439)*r22); +IkReal x2073=(cj5*sj5); +IkReal x2074=((0.5)*r21); +IkReal x2075=(cj0*r22); +IkReal x2076=(cj0*r00); +IkReal x2077=((2.0)*r20); +IkReal x2078=(r20*sj0); +IkReal x2079=((0.866025403784439)*r12); +IkReal x2080=((3.0)*r11); +IkReal x2081=((3.0)*sj0); +IkReal x2082=((1.73205080756888)*cj5); +IkReal x2083=(r21*sj5); +IkReal x2084=(r11*sj0); +IkReal x2085=(r12*r22); +IkReal x2086=(r10*sj0); +IkReal x2087=(cj3*cj5); +IkReal x2088=((3.0)*r21); +IkReal x2089=((1.0)*r21); +IkReal x2090=(cj0*r02); +IkReal x2091=((1.5)*x2086); +IkReal x2092=(r20*x2070); +IkReal x2093=((1.73205080756888)*r22*sj5); +IkReal x2094=(r20*x2090); +IkReal x2095=((1.5)*r21*x2070); +j4eval[0]=(((r00*x2075*x2082))+(((-1.0)*x2071*x2093))+((r22*x2082*x2086))+(((-3.0)*r02*x2075))+(((-1.0)*r21*sj0*x2070*x2080))+(((-1.0)*x2084*x2089))+(((-1.0)*x2073*x2076*x2088))+(((-1.0)*x2081*x2085))+((x2082*x2094))+(((-3.0)*r20*x2071*x2073))+(((-1.0)*x2071*x2089))+(((-4.0)*r10*x2078))+(((-1.0)*x2084*x2093))+((r12*x2078*x2082))+(((-1.0)*x2070*x2071*x2088))+(((-4.0)*r20*x2076))+(((-1.73205080756888)*r12*sj0*x2083))+(((-1.73205080756888)*x2083*x2090))+(((-1.0)*r10*r21*x2073*x2081))+(((-1.0)*x2073*x2078*x2080))+(((3.0)*x2076*x2092))+(((3.0)*r10*x2070*x2078))); +j4eval[1]=((IKabs((((cj3*r20*sj5))+((r21*x2087)))))+(((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x2087))+((cj3*x2083)))))))); +j4eval[2]=IKsign(((((1.5)*r10*x2070*x2078))+(((-1.5)*r02*x2075))+(((-1.0)*x2071*x2095))+((cj5*x2078*x2079))+(((-1.5)*r21*x2073*x2076))+(((-1.0)*sj0*x2079*x2083))+(((1.5)*x2076*x2092))+(((-1.0)*x2071*x2074))+(((-1.0)*x2074*x2084))+(((-1.0)*r21*x2073*x2091))+(((-1.0)*x2076*x2077))+(((-1.0)*x2077*x2086))+(((-0.866025403784439)*x2083*x2090))+((cj5*x2072*x2086))+(((-1.5)*r20*x2071*x2073))+((cj5*x2072*x2076))+(((-1.0)*x2084*x2095))+(((0.866025403784439)*cj5*x2094))+(((-1.0)*sj5*x2071*x2072))+(((-1.5)*r11*x2073*x2078))+(((-1.5)*sj0*x2085))+(((-1.0)*sj5*x2072*x2084)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2096=((0.866025403784439)*sj5); +IkReal x2097=(r01*sj0); +IkReal x2098=(r02*sj0); +IkReal x2099=(cj0*r11); +IkReal x2100=(cj0*r12); +IkReal x2101=((1.73205080756888)*sj5); +IkReal x2102=(cj0*cj5*r10); +IkReal x2103=(cj5*r00*sj0); +j4eval[0]=(x2100+(((-1.0)*x2099*x2101))+((x2097*x2101))+(((-1.73205080756888)*x2103))+(((-1.0)*x2098))+(((1.73205080756888)*x2102))); +j4eval[1]=IKsign(((((-1.0)*x2096*x2099))+((x2096*x2097))+(((0.866025403784439)*x2102))+(((-0.866025403784439)*x2103))+(((0.5)*x2100))+(((-0.5)*x2098)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2104=r21*r21; +IkReal x2105=cj5*cj5; +IkReal x2106=r20*r20; +IkReal x2107=r22*r22; +IkReal x2108=(r21*sj5); +IkReal x2109=((1.73205080756888)*r20); +IkReal x2110=(cj5*r20); +IkReal x2111=((1.15470053837925)*r22); +IkReal x2112=(x2104*x2105); +IkReal x2113=(x2105*x2106); +j4eval[0]=((((0.333333333333333)*x2104))+x2112+x2107+(((2.0)*x2108*x2110))+(((-1.0)*x2110*x2111))+(((1.33333333333333)*x2106))+(((-1.0)*x2113))+((x2108*x2111))); +j4eval[1]=IKsign(((((3.0)*x2108*x2110))+(((-1.5)*x2113))+(((1.5)*x2107))+(((1.73205080756888)*r22*x2108))+(((0.5)*x2104))+(((1.5)*x2112))+(((2.0)*x2106))+(((-1.0)*cj5*r22*x2109)))); +j4eval[2]=((IKabs((((sj5*x2109))+(((1.73205080756888)*cj5*r21)))))+(IKabs(((((1.5)*r22))+(((-0.866025403784439)*x2110))+(((0.866025403784439)*x2108)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2114=((1.73205080756888)*sj0); +IkReal x2115=((1.73205080756888)*cj0); +IkReal x2116=(((r01*sj5*x2114))+((cj5*r10*x2115))+(((-1.0)*r11*sj5*x2115))+(((-1.0)*cj5*r00*x2114))+(((-1.0)*r02*sj0))+((cj0*r12))); +j4eval[0]=x2116; +j4eval[1]=IKsign(x2116); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2117=((1.73205080756888)*sj0); +IkReal x2118=(r01*sj5); +IkReal x2119=((0.866025403784439)*cj0); +IkReal x2120=(cj5*r00); +IkReal x2121=((0.866025403784439)*sj0); +IkReal x2122=(cj5*r10); +IkReal x2123=((1.73205080756888)*cj0); +IkReal x2124=(r11*sj5); +CheckValue x2125 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+(((-1.0)*x2119*x2120))+(((-1.0)*x2121*x2122))+((x2121*x2124))+((x2118*x2119))+(((1.5)*r12*sj0)))),IkReal(((((-1.0)*r10*sj5*x2117))+(((-1.0)*cj5*r11*x2117))+(((-1.0)*r00*sj5*x2123))+(((-1.0)*cj5*r01*x2123)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2125.valid){ +continue; +} +CheckValue x2126=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2117*x2120))+((x2117*x2118))+((x2122*x2123))+(((-1.0)*r02*sj0))+((cj0*r12))+(((-1.0)*x2123*x2124)))),-1); +if(!x2126.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2125.value)+(((1.5707963267949)*(x2126.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2127=IKcos(j4); +IkReal x2128=IKsin(j4); +IkReal x2129=((0.5)*r21); +IkReal x2130=(cj0*r10); +IkReal x2131=((0.5)*sj0); +IkReal x2132=(r10*sj0); +IkReal x2133=((0.5)*r20); +IkReal x2134=((1.0)*sj0); +IkReal x2135=(cj0*r01); +IkReal x2136=(r02*sj0); +IkReal x2137=(cj0*r02); +IkReal x2138=(cj0*r00); +IkReal x2139=(cj0*r11); +IkReal x2140=(sj5*x2128); +IkReal x2141=(cj5*x2128); +IkReal x2142=(sj5*x2127); +IkReal x2143=((0.866025403784439)*x2128); +IkReal x2144=((0.866025403784439)*x2127); +IkReal x2145=(cj5*x2127); +IkReal x2146=(r12*x2144); +evalcond[0]=((((-1.0)*x2129*x2140))+((x2133*x2141))+((r20*x2142))+(((-1.0)*r22*x2143))+((r21*x2145))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x2129*x2142))+((x2133*x2145))+(((-1.0)*r20*x2140))+(((-1.0)*r21*x2141))+(((-1.0)*r22*x2144))); +evalcond[2]=(((x2138*x2140))+((x2132*x2140))+(((-1.0)*r10*x2131*x2145))+((sj0*x2146))+((r11*sj0*x2141))+(((0.5)*x2135*x2142))+((x2137*x2144))+((r11*x2131*x2142))+((x2135*x2141))+(((-0.5)*x2138*x2145))); +evalcond[3]=((((-1.0)*x2136*x2143))+((r00*x2131*x2141))+(((-1.0)*x2130*x2142))+((r01*sj0*x2145))+(((0.5)*x2139*x2140))+((cj0*r12*x2143))+(((-1.0)*r01*x2131*x2140))+(((-0.5)*x2130*x2141))+(((-1.0)*x2139*x2145))+((r00*sj0*x2142))); +evalcond[4]=((0.5)+(((-1.0)*x2136*x2144))+((r00*x2131*x2145))+((x2130*x2140))+(((-1.0)*r01*x2134*x2141))+((cj0*x2146))+(((0.5)*x2139*x2142))+(((-1.0)*r01*x2131*x2142))+(((-0.5)*x2130*x2145))+(((-1.0)*r00*x2134*x2140))+((x2139*x2141))); +evalcond[5]=((-1.0)+(((-1.0)*x2132*x2142))+(((-1.0)*r10*x2131*x2141))+((r12*sj0*x2143))+(((0.5)*x2135*x2140))+((x2137*x2143))+(((-1.0)*x2135*x2145))+(((-1.0)*x2138*x2142))+(((-1.0)*r11*x2134*x2145))+((r11*x2131*x2140))+(((-0.5)*x2138*x2141))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2147=r21*r21; +IkReal x2148=cj5*cj5; +IkReal x2149=r20*r20; +IkReal x2150=(r21*sj5); +IkReal x2151=((1.73205080756888)*cj5); +IkReal x2152=(cj5*r20); +IkReal x2153=((1.5)*x2148); +CheckValue x2154 = IKatan2WithCheck(IkReal(((((1.73205080756888)*r20*sj5))+((r21*x2151)))),IkReal(((((1.5)*r22))+(((0.866025403784439)*x2150))+(((-0.866025403784439)*x2152)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2154.valid){ +continue; +} +CheckValue x2155=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2149*x2153))+((x2147*x2153))+(((2.0)*x2149))+(((-1.0)*r20*r22*x2151))+(((0.5)*x2147))+(((3.0)*x2150*x2152))+(((1.5)*(r22*r22)))+(((1.73205080756888)*r22*x2150)))),-1); +if(!x2155.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2154.value)+(((1.5707963267949)*(x2155.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2156=IKcos(j4); +IkReal x2157=IKsin(j4); +IkReal x2158=((0.5)*r21); +IkReal x2159=(cj0*r10); +IkReal x2160=((0.5)*sj0); +IkReal x2161=(r10*sj0); +IkReal x2162=((0.5)*r20); +IkReal x2163=((1.0)*sj0); +IkReal x2164=(cj0*r01); +IkReal x2165=(r02*sj0); +IkReal x2166=(cj0*r02); +IkReal x2167=(cj0*r00); +IkReal x2168=(cj0*r11); +IkReal x2169=(sj5*x2157); +IkReal x2170=(cj5*x2157); +IkReal x2171=(sj5*x2156); +IkReal x2172=((0.866025403784439)*x2157); +IkReal x2173=((0.866025403784439)*x2156); +IkReal x2174=(cj5*x2156); +IkReal x2175=(r12*x2173); +evalcond[0]=(((r21*x2174))+(((-1.0)*x2158*x2169))+((r20*x2171))+((x2162*x2170))+(((-1.0)*r22*x2172))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x2170))+((x2162*x2174))+(((-1.0)*x2158*x2171))+(((-1.0)*r22*x2173))+(((-1.0)*r20*x2169))); +evalcond[2]=((((-1.0)*r10*x2160*x2174))+(((-0.5)*x2167*x2174))+((x2164*x2170))+(((0.5)*x2164*x2171))+((r11*x2160*x2171))+((r11*sj0*x2170))+((x2166*x2173))+((x2161*x2169))+((x2167*x2169))+((sj0*x2175))); +evalcond[3]=((((-1.0)*x2168*x2174))+((r01*sj0*x2174))+((r00*sj0*x2171))+((r00*x2160*x2170))+(((-1.0)*r01*x2160*x2169))+((cj0*r12*x2172))+(((-1.0)*x2159*x2171))+(((0.5)*x2168*x2169))+(((-1.0)*x2165*x2172))+(((-0.5)*x2159*x2170))); +evalcond[4]=((0.5)+((cj0*x2175))+(((-1.0)*r01*x2160*x2171))+(((-1.0)*r01*x2163*x2170))+((r00*x2160*x2174))+(((-1.0)*x2165*x2173))+((x2168*x2170))+(((-0.5)*x2159*x2174))+((x2159*x2169))+(((-1.0)*r00*x2163*x2169))+(((0.5)*x2168*x2171))); +evalcond[5]=((-1.0)+(((-1.0)*r10*x2160*x2170))+(((-0.5)*x2167*x2170))+(((0.5)*x2164*x2169))+(((-1.0)*r11*x2163*x2174))+((x2166*x2172))+((r12*sj0*x2172))+(((-1.0)*x2167*x2171))+(((-1.0)*x2161*x2171))+(((-1.0)*x2164*x2174))+((r11*x2160*x2169))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2176=((0.866025403784439)*sj5); +IkReal x2177=((0.866025403784439)*cj5); +CheckValue x2178 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2178.valid){ +continue; +} +CheckValue x2179=IKPowWithIntegerCheck(IKsign(((((-1.0)*r00*sj0*x2177))+(((-0.5)*r02*sj0))+((r01*sj0*x2176))+(((-1.0)*cj0*r11*x2176))+((cj0*r10*x2177))+(((0.5)*cj0*r12)))),-1); +if(!x2179.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2178.value)+(((1.5707963267949)*(x2179.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2180=IKcos(j4); +IkReal x2181=IKsin(j4); +IkReal x2182=((0.5)*r21); +IkReal x2183=(cj0*r10); +IkReal x2184=((0.5)*sj0); +IkReal x2185=(r10*sj0); +IkReal x2186=((0.5)*r20); +IkReal x2187=((1.0)*sj0); +IkReal x2188=(cj0*r01); +IkReal x2189=(r02*sj0); +IkReal x2190=(cj0*r02); +IkReal x2191=(cj0*r00); +IkReal x2192=(cj0*r11); +IkReal x2193=(sj5*x2181); +IkReal x2194=(cj5*x2181); +IkReal x2195=(sj5*x2180); +IkReal x2196=((0.866025403784439)*x2181); +IkReal x2197=((0.866025403784439)*x2180); +IkReal x2198=(cj5*x2180); +IkReal x2199=(r12*x2197); +evalcond[0]=(((x2186*x2194))+((r20*x2195))+((r21*x2198))+(((-1.0)*x2182*x2193))+(((-1.0)*r22*x2196))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r20*x2193))+((x2186*x2198))+(((-1.0)*x2182*x2195))+(((-1.0)*r22*x2197))+(((-1.0)*r21*x2194))); +evalcond[2]=(((r11*x2184*x2195))+((x2191*x2193))+(((0.5)*x2188*x2195))+((sj0*x2199))+((x2190*x2197))+(((-1.0)*r10*x2184*x2198))+((x2188*x2194))+((r11*sj0*x2194))+((x2185*x2193))+(((-0.5)*x2191*x2198))); +evalcond[3]=((((-1.0)*x2189*x2196))+((r00*sj0*x2195))+(((-0.5)*x2183*x2194))+((r00*x2184*x2194))+(((-1.0)*x2192*x2198))+((r01*sj0*x2198))+(((0.5)*x2192*x2193))+((cj0*r12*x2196))+(((-1.0)*x2183*x2195))+(((-1.0)*r01*x2184*x2193))); +evalcond[4]=((0.5)+(((-1.0)*r00*x2187*x2193))+((x2192*x2194))+(((-1.0)*x2189*x2197))+(((-0.5)*x2183*x2198))+((x2183*x2193))+((r00*x2184*x2198))+((cj0*x2199))+(((0.5)*x2192*x2195))+(((-1.0)*r01*x2184*x2195))+(((-1.0)*r01*x2187*x2194))); +evalcond[5]=((-1.0)+((r11*x2184*x2193))+((r12*sj0*x2196))+(((-1.0)*x2188*x2198))+(((-1.0)*r11*x2187*x2198))+(((0.5)*x2188*x2193))+((x2190*x2196))+(((-1.0)*x2191*x2195))+(((-1.0)*r10*x2184*x2194))+(((-1.0)*x2185*x2195))+(((-0.5)*x2191*x2194))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2200=(cj0*r12); +IkReal x2201=((0.866025403784439)*sj5); +IkReal x2202=(r01*sj0); +IkReal x2203=(r02*sj0); +IkReal x2204=(cj0*r11); +IkReal x2205=((1.73205080756888)*sj5); +IkReal x2206=(cj0*cj5*r10); +IkReal x2207=(cj5*r00*sj0); +j4eval[0]=((((-1.0)*x2200))+x2203+(((-1.0)*x2202*x2205))+((x2204*x2205))+(((1.73205080756888)*x2207))+(((-1.73205080756888)*x2206))); +j4eval[1]=IKsign(((((-0.5)*x2200))+((x2201*x2204))+(((0.5)*x2203))+(((-1.0)*x2201*x2202))+(((0.866025403784439)*x2207))+(((-0.866025403784439)*x2206)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2208=r21*r21; +IkReal x2209=cj5*cj5; +IkReal x2210=r20*r20; +IkReal x2211=r22*r22; +IkReal x2212=(r21*sj5); +IkReal x2213=((1.73205080756888)*r20); +IkReal x2214=(cj5*r20); +IkReal x2215=((1.15470053837925)*r22); +IkReal x2216=(x2208*x2209); +IkReal x2217=(x2209*x2210); +j4eval[0]=((((-1.0)*x2214*x2215))+(((-1.0)*x2217))+x2211+x2216+(((0.333333333333333)*x2208))+((x2212*x2215))+(((1.33333333333333)*x2210))+(((2.0)*x2212*x2214))); +j4eval[1]=IKsign(((((-1.0)*cj5*r22*x2213))+(((1.5)*x2211))+(((1.5)*x2216))+(((-1.5)*x2217))+(((0.5)*x2208))+(((2.0)*x2210))+(((3.0)*x2212*x2214))+(((1.73205080756888)*r22*x2212)))); +j4eval[2]=((IKabs(((((1.73205080756888)*cj5*r21))+((sj5*x2213)))))+(IKabs(((((1.5)*r22))+(((0.866025403784439)*x2212))+(((-0.866025403784439)*x2214)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2218=((1.73205080756888)*sj0); +IkReal x2219=((1.73205080756888)*cj0); +IkReal x2220=(((cj5*r10*x2219))+(((-1.0)*cj5*r00*x2218))+(((-1.0)*r02*sj0))+((r01*sj5*x2218))+(((-1.0)*r11*sj5*x2219))+((cj0*r12))); +j4eval[0]=x2220; +j4eval[1]=IKsign(x2220); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2221=((1.73205080756888)*sj0); +IkReal x2222=(r01*sj5); +IkReal x2223=((0.866025403784439)*cj0); +IkReal x2224=(cj5*r00); +IkReal x2225=((0.866025403784439)*sj0); +IkReal x2226=(cj5*r10); +IkReal x2227=((1.73205080756888)*cj0); +IkReal x2228=(r11*sj5); +CheckValue x2229=IKPowWithIntegerCheck(IKsign((((x2226*x2227))+(((-1.0)*x2227*x2228))+(((-1.0)*x2221*x2224))+(((-1.0)*r02*sj0))+((x2221*x2222))+((cj0*r12)))),-1); +if(!x2229.valid){ +continue; +} +CheckValue x2230 = IKatan2WithCheck(IkReal(((((-1.0)*x2225*x2226))+(((1.5)*cj0*r02))+((x2225*x2228))+((x2222*x2223))+(((-1.0)*x2223*x2224))+(((1.5)*r12*sj0)))),IkReal(((((-1.0)*r10*sj5*x2221))+(((-1.0)*cj5*r01*x2227))+(((-1.0)*cj5*r11*x2221))+(((-1.0)*r00*sj5*x2227)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2230.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2229.value)))+(x2230.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2231=IKcos(j4); +IkReal x2232=IKsin(j4); +IkReal x2233=((0.5)*r21); +IkReal x2234=(cj0*r10); +IkReal x2235=((0.5)*sj0); +IkReal x2236=(r10*sj0); +IkReal x2237=((0.5)*r20); +IkReal x2238=((1.0)*sj0); +IkReal x2239=(cj0*r01); +IkReal x2240=(r02*sj0); +IkReal x2241=(cj0*r02); +IkReal x2242=(cj0*r00); +IkReal x2243=(cj0*r11); +IkReal x2244=(sj5*x2232); +IkReal x2245=(cj5*x2232); +IkReal x2246=(sj5*x2231); +IkReal x2247=((0.866025403784439)*x2232); +IkReal x2248=((0.866025403784439)*x2231); +IkReal x2249=(cj5*x2231); +IkReal x2250=(r12*x2248); +evalcond[0]=((((-1.0)*r22*x2247))+(((-1.0)*x2233*x2244))+((r20*x2246))+((r21*x2249))+((x2237*x2245))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x2245))+(((-1.0)*r22*x2248))+(((-1.0)*x2233*x2246))+(((-1.0)*r20*x2244))+((x2237*x2249))); +evalcond[2]=((((-0.5)*x2242*x2249))+((x2241*x2248))+((x2236*x2244))+(((0.5)*x2239*x2246))+((sj0*x2250))+((r11*x2235*x2246))+((x2239*x2245))+(((-1.0)*r10*x2235*x2249))+((x2242*x2244))+((r11*sj0*x2245))); +evalcond[3]=(((r00*x2235*x2245))+(((-1.0)*x2234*x2246))+(((-1.0)*r01*x2235*x2244))+(((0.5)*x2243*x2244))+(((-1.0)*x2240*x2247))+(((-0.5)*x2234*x2245))+(((-1.0)*x2243*x2249))+((cj0*r12*x2247))+((r01*sj0*x2249))+((r00*sj0*x2246))); +evalcond[4]=((-0.5)+((r00*x2235*x2249))+((x2243*x2245))+((cj0*x2250))+(((-1.0)*r01*x2235*x2246))+((x2234*x2244))+(((-1.0)*r00*x2238*x2244))+(((0.5)*x2243*x2246))+(((-1.0)*x2240*x2248))+(((-0.5)*x2234*x2249))+(((-1.0)*r01*x2238*x2245))); +evalcond[5]=((1.0)+(((-0.5)*x2242*x2245))+((x2241*x2247))+(((0.5)*x2239*x2244))+(((-1.0)*x2239*x2249))+(((-1.0)*x2236*x2246))+(((-1.0)*x2242*x2246))+(((-1.0)*r11*x2238*x2249))+((r12*sj0*x2247))+((r11*x2235*x2244))+(((-1.0)*r10*x2235*x2245))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2251=r21*r21; +IkReal x2252=cj5*cj5; +IkReal x2253=r20*r20; +IkReal x2254=(r21*sj5); +IkReal x2255=((1.73205080756888)*cj5); +IkReal x2256=(cj5*r20); +IkReal x2257=((1.5)*x2252); +CheckValue x2258=IKPowWithIntegerCheck(IKsign(((((3.0)*x2254*x2256))+(((-1.0)*x2253*x2257))+(((0.5)*x2251))+(((1.5)*(r22*r22)))+((x2251*x2257))+(((1.73205080756888)*r22*x2254))+(((-1.0)*r20*r22*x2255))+(((2.0)*x2253)))),-1); +if(!x2258.valid){ +continue; +} +CheckValue x2259 = IKatan2WithCheck(IkReal(((((1.73205080756888)*r20*sj5))+((r21*x2255)))),IkReal(((((1.5)*r22))+(((0.866025403784439)*x2254))+(((-0.866025403784439)*x2256)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2259.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2258.value)))+(x2259.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2260=IKcos(j4); +IkReal x2261=IKsin(j4); +IkReal x2262=((0.5)*r21); +IkReal x2263=(cj0*r10); +IkReal x2264=((0.5)*sj0); +IkReal x2265=(r10*sj0); +IkReal x2266=((0.5)*r20); +IkReal x2267=((1.0)*sj0); +IkReal x2268=(cj0*r01); +IkReal x2269=(r02*sj0); +IkReal x2270=(cj0*r02); +IkReal x2271=(cj0*r00); +IkReal x2272=(cj0*r11); +IkReal x2273=(sj5*x2261); +IkReal x2274=(cj5*x2261); +IkReal x2275=(sj5*x2260); +IkReal x2276=((0.866025403784439)*x2261); +IkReal x2277=((0.866025403784439)*x2260); +IkReal x2278=(cj5*x2260); +IkReal x2279=(r12*x2277); +evalcond[0]=(((r21*x2278))+(((-1.0)*r22*x2276))+((r20*x2275))+((x2266*x2274))+(((-1.0)*x2262*x2273))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r20*x2273))+(((-1.0)*r22*x2277))+((x2266*x2278))+(((-1.0)*r21*x2274))+(((-1.0)*x2262*x2275))); +evalcond[2]=(((sj0*x2279))+((r11*x2264*x2275))+((x2270*x2277))+((x2268*x2274))+(((-1.0)*r10*x2264*x2278))+(((-0.5)*x2271*x2278))+((x2265*x2273))+((r11*sj0*x2274))+(((0.5)*x2268*x2275))+((x2271*x2273))); +evalcond[3]=(((cj0*r12*x2276))+(((0.5)*x2272*x2273))+(((-0.5)*x2263*x2274))+(((-1.0)*r01*x2264*x2273))+((r00*sj0*x2275))+(((-1.0)*x2269*x2276))+(((-1.0)*x2263*x2275))+((r01*sj0*x2278))+((r00*x2264*x2274))+(((-1.0)*x2272*x2278))); +evalcond[4]=((-0.5)+((x2263*x2273))+(((0.5)*x2272*x2275))+(((-0.5)*x2263*x2278))+(((-1.0)*r00*x2267*x2273))+(((-1.0)*r01*x2264*x2275))+(((-1.0)*r01*x2267*x2274))+((cj0*x2279))+((x2272*x2274))+(((-1.0)*x2269*x2277))+((r00*x2264*x2278))); +evalcond[5]=((1.0)+((r12*sj0*x2276))+((r11*x2264*x2273))+((x2270*x2276))+(((-1.0)*r11*x2267*x2278))+(((-1.0)*x2268*x2278))+(((-1.0)*x2271*x2275))+(((-1.0)*r10*x2264*x2274))+(((-1.0)*x2265*x2275))+(((-0.5)*x2271*x2274))+(((0.5)*x2268*x2273))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2280=((0.866025403784439)*sj5); +IkReal x2281=((0.866025403784439)*cj5); +CheckValue x2282=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj0*r10*x2281))+((cj0*r11*x2280))+(((-0.5)*cj0*r12))+(((-1.0)*r01*sj0*x2280))+(((0.5)*r02*sj0))+((r00*sj0*x2281)))),-1); +if(!x2282.valid){ +continue; +} +CheckValue x2283 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2283.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2282.value)))+(x2283.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2284=IKcos(j4); +IkReal x2285=IKsin(j4); +IkReal x2286=((0.5)*r21); +IkReal x2287=(cj0*r10); +IkReal x2288=((0.5)*sj0); +IkReal x2289=(r10*sj0); +IkReal x2290=((0.5)*r20); +IkReal x2291=((1.0)*sj0); +IkReal x2292=(cj0*r01); +IkReal x2293=(r02*sj0); +IkReal x2294=(cj0*r02); +IkReal x2295=(cj0*r00); +IkReal x2296=(cj0*r11); +IkReal x2297=(sj5*x2285); +IkReal x2298=(cj5*x2285); +IkReal x2299=(sj5*x2284); +IkReal x2300=((0.866025403784439)*x2285); +IkReal x2301=((0.866025403784439)*x2284); +IkReal x2302=(cj5*x2284); +IkReal x2303=(r12*x2301); +evalcond[0]=((((-1.0)*r22*x2300))+(((-1.0)*x2286*x2297))+((r20*x2299))+((r21*x2302))+((x2290*x2298))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r22*x2301))+(((-1.0)*r20*x2297))+((x2290*x2302))+(((-1.0)*x2286*x2299))+(((-1.0)*r21*x2298))); +evalcond[2]=(((sj0*x2303))+(((-1.0)*r10*x2288*x2302))+((r11*sj0*x2298))+((x2295*x2297))+(((-0.5)*x2295*x2302))+((x2289*x2297))+((x2292*x2298))+(((0.5)*x2292*x2299))+((x2294*x2301))+((r11*x2288*x2299))); +evalcond[3]=(((r00*x2288*x2298))+(((-1.0)*x2293*x2300))+(((-1.0)*x2296*x2302))+(((0.5)*x2296*x2297))+(((-1.0)*r01*x2288*x2297))+((r00*sj0*x2299))+(((-0.5)*x2287*x2298))+(((-1.0)*x2287*x2299))+((cj0*r12*x2300))+((r01*sj0*x2302))); +evalcond[4]=((-0.5)+((x2296*x2298))+(((-1.0)*x2293*x2301))+(((0.5)*x2296*x2299))+(((-1.0)*r00*x2291*x2297))+(((-1.0)*r01*x2288*x2299))+((cj0*x2303))+(((-0.5)*x2287*x2302))+((x2287*x2297))+((r00*x2288*x2302))+(((-1.0)*r01*x2291*x2298))); +evalcond[5]=((1.0)+(((-1.0)*r10*x2288*x2298))+(((-1.0)*x2292*x2302))+(((-1.0)*x2289*x2299))+(((-0.5)*x2295*x2298))+(((-1.0)*x2295*x2299))+(((-1.0)*r11*x2291*x2302))+((r12*sj0*x2300))+(((0.5)*x2292*x2297))+((x2294*x2300))+((r11*x2288*x2297))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2304=cj5*cj5; +IkReal x2305=(r20*sj0); +IkReal x2306=(cj0*r01); +IkReal x2307=((0.866025403784439)*r22); +IkReal x2308=((0.5)*r21); +IkReal x2309=(cj0*r00); +IkReal x2310=((0.866025403784439)*r12); +IkReal x2311=(cj5*r20); +IkReal x2312=(r21*sj0); +IkReal x2313=((1.5)*sj5); +IkReal x2314=(cj3*sj5); +IkReal x2315=(cj0*r02); +IkReal x2316=(cj5*r21); +IkReal x2317=(cj5*r10); +IkReal x2318=((1.5)*r22); +IkReal x2319=(r11*sj0); +IkReal x2320=((1.5)*x2304); +CheckValue x2321 = IKatan2WithCheck(IkReal((((cj3*x2316))+((r20*x2314)))),IkReal((((x2308*x2314))+(((-0.5)*cj3*x2311))+((cj3*x2307)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2321.valid){ +continue; +} +CheckValue x2322=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2306*x2308))+(((-1.0)*r11*x2312*x2320))+(((-1.0)*x2309*x2313*x2316))+(((-1.0)*sj5*x2306*x2307))+(((-1.0)*r21*x2306*x2320))+(((-2.0)*r20*x2309))+(((-1.0)*cj5*r11*x2305*x2313))+((r20*x2309*x2320))+(((-1.0)*x2315*x2318))+((r10*x2305*x2320))+(((-0.866025403784439)*r21*sj5*x2315))+(((-2.0)*r10*x2305))+(((-1.0)*x2308*x2319))+(((0.866025403784439)*x2311*x2315))+((cj5*x2305*x2310))+(((-1.0)*x2312*x2313*x2317))+(((-1.0)*sj5*x2307*x2319))+(((-1.0)*x2306*x2311*x2313))+((cj5*x2307*x2309))+(((-1.0)*r12*sj0*x2318))+((sj0*x2307*x2317))+(((-1.0)*sj5*x2310*x2312)))),-1); +if(!x2322.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2321.value)+(((1.5707963267949)*(x2322.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2323=IKcos(j4); +IkReal x2324=IKsin(j4); +IkReal x2325=((0.5)*cj5); +IkReal x2326=(r01*sj0); +IkReal x2327=((1.0)*sj5); +IkReal x2328=(r11*sj0); +IkReal x2329=(r02*sj0); +IkReal x2330=(r00*sj0); +IkReal x2331=(cj0*r02); +IkReal x2332=(cj0*r01); +IkReal x2333=(r10*sj0); +IkReal x2334=(cj0*r00); +IkReal x2335=(cj0*r11); +IkReal x2336=(cj0*r10); +IkReal x2337=(sj5*x2323); +IkReal x2338=(r20*x2324); +IkReal x2339=(cj5*x2324); +IkReal x2340=((0.866025403784439)*x2324); +IkReal x2341=((0.866025403784439)*x2323); +IkReal x2342=(cj5*x2323); +IkReal x2343=(x2324*x2336); +IkReal x2344=(x2324*x2333); +IkReal x2345=(r12*x2341); +IkReal x2346=((0.5)*sj5*x2324); +evalcond[0]=((((-1.0)*r21*x2346))+(((-1.0)*r22*x2340))+((x2325*x2338))+((r21*x2342))+((r20*x2337))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x2339))+((r20*x2323*x2325))+(((-1.0)*r22*x2341))+(((-1.0)*x2327*x2338))+(((-0.5)*r21*x2337))); +evalcond[2]=(((x2335*x2346))+(((-1.0)*x2325*x2343))+(((-1.0)*x2326*x2346))+(((-1.0)*x2335*x2342))+cj3+(((-1.0)*x2323*x2327*x2336))+((x2326*x2342))+((cj0*r12*x2340))+((x2324*x2325*x2330))+((x2330*x2337))+(((-1.0)*x2329*x2340))); +evalcond[3]=(((sj0*x2345))+((x2331*x2341))+((sj5*x2324*x2334))+(((0.5)*x2328*x2337))+((x2332*x2339))+(((0.5)*x2332*x2337))+(((0.5)*cj3))+((sj5*x2344))+(((-1.0)*x2323*x2325*x2334))+(((-1.0)*x2323*x2325*x2333))+((x2328*x2339))); +evalcond[4]=((((-1.0)*x2324*x2327*x2330))+(((0.5)*sj3))+((x2335*x2339))+(((-1.0)*x2326*x2339))+((x2323*x2325*x2330))+((sj5*x2343))+(((0.5)*x2335*x2337))+(((-1.0)*x2323*x2325*x2336))+(((-0.5)*x2326*x2337))+((cj0*x2345))+(((-1.0)*x2329*x2341))); +evalcond[5]=(((x2331*x2340))+(((-1.0)*x2325*x2344))+(((-1.0)*sj3))+(((-1.0)*x2332*x2342))+(((-1.0)*x2323*x2327*x2334))+(((-1.0)*x2323*x2327*x2333))+(((-1.0)*x2324*x2325*x2334))+((x2332*x2346))+(((-1.0)*x2328*x2342))+((r12*sj0*x2340))+((x2328*x2346))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2347=r21*r21; +IkReal x2348=cj5*cj5; +IkReal x2349=r20*r20; +IkReal x2350=(r21*sj5); +IkReal x2351=((1.73205080756888)*cj5); +IkReal x2352=(cj5*r20); +IkReal x2353=((1.5)*x2348); +CheckValue x2354=IKPowWithIntegerCheck(IKsign(((((0.5)*x2347))+(((3.0)*x2350*x2352))+(((2.0)*x2349))+(((-1.0)*r20*r22*x2351))+(((1.5)*(r22*r22)))+((x2347*x2353))+(((1.73205080756888)*r22*x2350))+(((-1.0)*x2349*x2353)))),-1); +if(!x2354.valid){ +continue; +} +CheckValue x2355 = IKatan2WithCheck(IkReal(((((1.73205080756888)*r20*sj5))+((r21*x2351)))),IkReal(((((1.5)*r22))+(((-0.866025403784439)*x2352))+(((0.866025403784439)*x2350)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2355.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2354.value)))+(x2355.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2356=IKcos(j4); +IkReal x2357=IKsin(j4); +IkReal x2358=((0.5)*cj5); +IkReal x2359=(r01*sj0); +IkReal x2360=((1.0)*sj5); +IkReal x2361=(r11*sj0); +IkReal x2362=(r02*sj0); +IkReal x2363=(r00*sj0); +IkReal x2364=(cj0*r02); +IkReal x2365=(cj0*r01); +IkReal x2366=(r10*sj0); +IkReal x2367=(cj0*r00); +IkReal x2368=(cj0*r11); +IkReal x2369=(cj0*r10); +IkReal x2370=(sj5*x2356); +IkReal x2371=(r20*x2357); +IkReal x2372=(cj5*x2357); +IkReal x2373=((0.866025403784439)*x2357); +IkReal x2374=((0.866025403784439)*x2356); +IkReal x2375=(cj5*x2356); +IkReal x2376=(x2357*x2369); +IkReal x2377=(x2357*x2366); +IkReal x2378=(r12*x2374); +IkReal x2379=((0.5)*sj5*x2357); +evalcond[0]=(((r20*x2370))+(((-1.0)*r21*x2379))+((x2358*x2371))+(((-1.0)*r22*x2373))+((r21*x2375))); +evalcond[1]=((0.866025403784439)+(((-0.5)*r21*x2370))+(((-1.0)*r21*x2372))+(((-1.0)*x2360*x2371))+((r20*x2356*x2358))+(((-1.0)*r22*x2374))); +evalcond[2]=(((x2368*x2379))+cj3+(((-1.0)*x2368*x2375))+((x2359*x2375))+(((-1.0)*x2358*x2376))+(((-1.0)*x2359*x2379))+((cj0*r12*x2373))+(((-1.0)*x2362*x2373))+((x2357*x2358*x2363))+((x2363*x2370))+(((-1.0)*x2356*x2360*x2369))); +evalcond[3]=(((x2365*x2372))+((x2364*x2374))+((sj0*x2378))+(((0.5)*x2361*x2370))+((sj5*x2377))+(((0.5)*cj3))+((x2361*x2372))+(((0.5)*x2365*x2370))+(((-1.0)*x2356*x2358*x2367))+(((-1.0)*x2356*x2358*x2366))+((sj5*x2357*x2367))); +evalcond[4]=(((x2368*x2372))+(((-1.0)*x2359*x2372))+((cj0*x2378))+(((0.5)*sj3))+(((-0.5)*x2359*x2370))+((sj5*x2376))+(((-1.0)*x2357*x2360*x2363))+(((0.5)*x2368*x2370))+(((-1.0)*x2362*x2374))+((x2356*x2358*x2363))+(((-1.0)*x2356*x2358*x2369))); +evalcond[5]=((((-1.0)*sj3))+((x2365*x2379))+((x2364*x2373))+(((-1.0)*x2361*x2375))+(((-1.0)*x2358*x2377))+(((-1.0)*x2365*x2375))+((x2361*x2379))+(((-1.0)*x2357*x2358*x2367))+((r12*sj0*x2373))+(((-1.0)*x2356*x2360*x2367))+(((-1.0)*x2356*x2360*x2366))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2380=((0.866025403784439)*cj0); +IkReal x2381=(cj3*r21); +IkReal x2382=(cj3*r20); +IkReal x2383=((0.866025403784439)*sj0); +CheckValue x2384=IKPowWithIntegerCheck(IKsign(((((-1.0)*r11*sj5*x2383))+(((0.5)*r12*sj0))+(((-1.0)*r01*sj5*x2380))+(((0.5)*cj0*r02))+((cj5*r00*x2380))+((cj5*r10*x2383)))),-1); +if(!x2384.valid){ +continue; +} +CheckValue x2385 = IKatan2WithCheck(IkReal((((sj5*x2382))+((cj5*x2381)))),IkReal(((((-0.5)*cj5*x2382))+(((0.5)*sj5*x2381))+(((0.866025403784439)*cj3*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2385.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2384.value)))+(x2385.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2386=IKcos(j4); +IkReal x2387=IKsin(j4); +IkReal x2388=((0.5)*cj5); +IkReal x2389=(r01*sj0); +IkReal x2390=((1.0)*sj5); +IkReal x2391=(r11*sj0); +IkReal x2392=(r02*sj0); +IkReal x2393=(r00*sj0); +IkReal x2394=(cj0*r02); +IkReal x2395=(cj0*r01); +IkReal x2396=(r10*sj0); +IkReal x2397=(cj0*r00); +IkReal x2398=(cj0*r11); +IkReal x2399=(cj0*r10); +IkReal x2400=(sj5*x2386); +IkReal x2401=(r20*x2387); +IkReal x2402=(cj5*x2387); +IkReal x2403=((0.866025403784439)*x2387); +IkReal x2404=((0.866025403784439)*x2386); +IkReal x2405=(cj5*x2386); +IkReal x2406=(x2387*x2399); +IkReal x2407=(x2387*x2396); +IkReal x2408=(r12*x2404); +IkReal x2409=((0.5)*sj5*x2387); +evalcond[0]=(((r20*x2400))+((r21*x2405))+((x2388*x2401))+(((-1.0)*r21*x2409))+(((-1.0)*r22*x2403))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x2402))+((r20*x2386*x2388))+(((-1.0)*x2390*x2401))+(((-0.5)*r21*x2400))+(((-1.0)*r22*x2404))); +evalcond[2]=(cj3+(((-1.0)*x2389*x2409))+((x2393*x2400))+((x2398*x2409))+(((-1.0)*x2392*x2403))+((cj0*r12*x2403))+(((-1.0)*x2398*x2405))+(((-1.0)*x2388*x2406))+(((-1.0)*x2386*x2390*x2399))+((x2389*x2405))+((x2387*x2388*x2393))); +evalcond[3]=(((sj5*x2387*x2397))+(((0.5)*x2391*x2400))+((sj5*x2407))+(((0.5)*cj3))+((x2394*x2404))+(((-1.0)*x2386*x2388*x2396))+(((-1.0)*x2386*x2388*x2397))+((sj0*x2408))+(((0.5)*x2395*x2400))+((x2391*x2402))+((x2395*x2402))); +evalcond[4]=((((-1.0)*x2389*x2402))+((cj0*x2408))+(((0.5)*sj3))+(((0.5)*x2398*x2400))+(((-1.0)*x2387*x2390*x2393))+((x2398*x2402))+(((-1.0)*x2392*x2404))+((sj5*x2406))+(((-1.0)*x2386*x2388*x2399))+((x2386*x2388*x2393))+(((-0.5)*x2389*x2400))); +evalcond[5]=((((-1.0)*x2391*x2405))+(((-1.0)*sj3))+(((-1.0)*x2387*x2388*x2397))+((x2394*x2403))+((r12*sj0*x2403))+(((-1.0)*x2388*x2407))+(((-1.0)*x2395*x2405))+(((-1.0)*x2386*x2390*x2397))+(((-1.0)*x2386*x2390*x2396))+((x2391*x2409))+((x2395*x2409))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(j2, 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x2410=((1.0)*cj5); +IkReal x2411=(r01*sj5); +IkReal x2412=((0.577350269189626)*r02); +IkReal x2413=(r11*sj5); +IkReal x2414=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*cj0*x2413))+((cj0*cj5*r10))+((sj0*x2411))+(((-1.0)*sj0*x2412))+(((-1.0)*r00*sj0*x2410))+((cj0*x2414)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj0*x2412))+((sj0*x2413))+(((-1.0)*cj0*r00*x2410))+(((-1.0)*sj0*x2414))+((cj0*x2411))+(((-1.0)*r10*sj0*x2410)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj0*x2413))+((cj0*cj5*r10))+((sj0*x2411))+(((-1.0)*sj0*x2412))+(((-1.0)*r00*sj0*x2410))+((cj0*x2414))))+IKsqr(((((-1.0)*cj0*x2412))+((sj0*x2413))+(((-1.0)*cj0*r00*x2410))+(((-1.0)*sj0*x2414))+((cj0*x2411))+(((-1.0)*r10*sj0*x2410))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*cj0*x2413))+((cj0*cj5*r10))+((sj0*x2411))+(((-1.0)*sj0*x2412))+(((-1.0)*r00*sj0*x2410))+((cj0*x2414))), ((((-1.0)*cj0*x2412))+((sj0*x2413))+(((-1.0)*cj0*r00*x2410))+(((-1.0)*sj0*x2414))+((cj0*x2411))+(((-1.0)*r10*sj0*x2410)))); +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[2]; +IkReal x2415=((0.866025403784439)*sj5); +IkReal x2416=((0.5)*sj0); +IkReal x2417=((0.5)*cj0); +IkReal x2418=((0.866025403784439)*cj5*r10); +IkReal x2419=((0.866025403784439)*cj5*r00); +evalcond[0]=((((-1.0)*cj0*x2418))+(((-1.0)*r12*x2417))+((sj0*x2419))+(((-1.0)*r01*sj0*x2415))+(((0.866025403784439)*(IKsin(j3))))+((r02*x2416))+((cj0*r11*x2415))); +evalcond[1]=((((-1.0)*cj0*x2419))+(((-0.866025403784439)*(IKcos(j3))))+((cj0*r01*x2415))+((r11*sj0*x2415))+(((-1.0)*r12*x2416))+(((-1.0)*sj0*x2418))+(((-1.0)*r02*x2417))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x2420=(r12*sj0); +IkReal x2421=(cj3*sj5); +IkReal x2422=(cj0*r02); +IkReal x2423=(cj3*cj5); +IkReal x2424=(cj0*cj5*r00); +IkReal x2425=(r11*sj0*sj5); +IkReal x2426=(cj0*r01*sj5); +IkReal x2427=(cj5*r10*sj0); +j4eval[0]=(x2420+x2422+(((-1.73205080756888)*x2426))+(((-1.73205080756888)*x2425))+(((1.73205080756888)*x2427))+(((1.73205080756888)*x2424))); +j4eval[1]=((IKabs((((r21*x2423))+((r20*x2421)))))+(((0.5)*(IKabs((((r21*x2421))+(((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x2423)))))))); +j4eval[2]=IKsign(((((0.866025403784439)*x2427))+(((0.866025403784439)*x2424))+(((-0.866025403784439)*x2426))+(((-0.866025403784439)*x2425))+(((0.5)*x2422))+(((0.5)*x2420)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x2428=r20*r20; +IkReal x2429=cj5*cj5; +IkReal x2430=r21*r21; +IkReal x2431=r22*r22; +IkReal x2432=(cj5*r20); +IkReal x2433=(r21*sj5); +IkReal x2434=((1.73205080756888)*r22); +IkReal x2435=((3.46410161513775)*r22); +IkReal x2436=(x2429*x2430); +IkReal x2437=(x2428*x2429); +j4eval[0]=((((-1.0)*x2433*x2435))+(((-1.0)*x2430))+((x2432*x2435))+(((-4.0)*x2428))+(((3.0)*x2437))+(((-3.0)*x2431))+(((-3.0)*x2436))+(((-6.0)*x2432*x2433))); +j4eval[1]=((IKabs(((((1.5)*r22))+(((-0.866025403784439)*x2432))+(((0.866025403784439)*x2433)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-1.0)*x2433*x2434))+(((1.5)*x2437))+((x2432*x2434))+(((-2.0)*x2428))+(((-1.5)*x2436))+(((-1.5)*x2431))+(((-3.0)*x2432*x2433))+(((-0.5)*x2430)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x2438=cj5*cj5; +IkReal x2439=(cj0*r01); +IkReal x2440=((0.866025403784439)*r22); +IkReal x2441=(cj5*sj5); +IkReal x2442=(r11*sj0); +IkReal x2443=((0.5)*r21); +IkReal x2444=(cj0*r22); +IkReal x2445=(cj0*r00); +IkReal x2446=((2.0)*r20); +IkReal x2447=(cj5*r20); +IkReal x2448=((3.0)*r20); +IkReal x2449=((4.0)*r20); +IkReal x2450=((1.73205080756888)*cj5); +IkReal x2451=(r21*sj5); +IkReal x2452=((3.0)*sj0); +IkReal x2453=((1.5)*r20); +IkReal x2454=(r12*r22); +IkReal x2455=(cj0*r02); +IkReal x2456=(r10*sj0); +IkReal x2457=((3.0)*r21); +IkReal x2458=((1.5)*x2456); +IkReal x2459=(r20*x2438); +IkReal x2460=((0.866025403784439)*r12*sj0); +IkReal x2461=((1.73205080756888)*r22*sj5); +IkReal x2462=((1.73205080756888)*r12*sj0); +IkReal x2463=((1.5)*r21*x2438); +j4eval[0]=(((x2438*x2442*x2457))+((x2442*x2461))+((r10*r21*x2441*x2452))+(((1.73205080756888)*x2451*x2455))+((x2441*x2442*x2448))+((x2452*x2454))+((x2439*x2441*x2448))+((x2449*x2456))+(((3.0)*r02*x2444))+(((-1.73205080756888)*x2447*x2455))+(((-1.0)*x2438*x2445*x2448))+((r21*x2439))+((x2441*x2445*x2457))+((x2445*x2449))+((x2451*x2462))+(((-1.0)*r22*x2450*x2456))+((x2439*x2461))+((x2438*x2439*x2457))+(((-1.0)*x2438*x2448*x2456))+(((-1.0)*r00*x2444*x2450))+((r21*x2442))+(((-1.0)*x2447*x2462))); +j4eval[1]=((IKabs((((cj3*r20*sj5))+((cj3*cj5*r21)))))+(((0.5)*(IKabs((((cj3*x2451))+(((1.73205080756888)*cj3*r22))+(((-1.0)*cj3*x2447)))))))); +j4eval[2]=IKsign((((x2442*x2463))+(((1.5)*r21*x2441*x2445))+(((-1.0)*x2438*x2453*x2456))+((x2441*x2442*x2453))+((r21*x2441*x2458))+((x2442*x2443))+((sj5*x2439*x2440))+((sj5*x2440*x2442))+(((-0.866025403784439)*x2447*x2455))+((x2439*x2441*x2453))+(((-1.0)*x2438*x2445*x2453))+(((1.5)*sj0*x2454))+(((-1.0)*cj5*x2440*x2445))+((x2445*x2446))+((x2451*x2460))+((x2439*x2443))+(((-1.0)*cj5*x2440*x2456))+(((1.5)*r02*x2444))+((x2439*x2463))+(((0.866025403784439)*x2451*x2455))+((x2446*x2456))+(((-1.0)*x2447*x2460)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2464=(cj0*r12); +IkReal x2465=((0.866025403784439)*sj5); +IkReal x2466=(r01*sj0); +IkReal x2467=(r02*sj0); +IkReal x2468=(cj0*r11); +IkReal x2469=((1.73205080756888)*sj5); +IkReal x2470=(cj0*cj5*r10); +IkReal x2471=(cj5*r00*sj0); +j4eval[0]=(x2467+(((-1.0)*x2466*x2469))+(((-1.0)*x2464))+(((-1.73205080756888)*x2470))+(((1.73205080756888)*x2471))+((x2468*x2469))); +j4eval[1]=IKsign(((((-0.5)*x2464))+(((0.5)*x2467))+(((-1.0)*x2465*x2466))+(((0.866025403784439)*x2471))+((x2465*x2468))+(((-0.866025403784439)*x2470)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2472=r20*r20; +IkReal x2473=cj5*cj5; +IkReal x2474=r21*r21; +IkReal x2475=r22*r22; +IkReal x2476=(cj5*r20); +IkReal x2477=(r21*sj5); +IkReal x2478=((1.73205080756888)*r22); +IkReal x2479=((3.46410161513775)*r22); +IkReal x2480=(x2473*x2474); +IkReal x2481=(x2472*x2473); +j4eval[0]=((((3.0)*x2481))+(((-4.0)*x2472))+(((-1.0)*x2477*x2479))+(((-6.0)*x2476*x2477))+(((-3.0)*x2480))+(((-1.0)*x2474))+(((-3.0)*x2475))+((x2476*x2479))); +j4eval[1]=((IKabs(((((0.866025403784439)*x2477))+(((1.5)*r22))+(((-0.866025403784439)*x2476)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-1.0)*x2477*x2478))+(((-1.5)*x2475))+(((-0.5)*x2474))+(((-2.0)*x2472))+((x2476*x2478))+(((-1.5)*x2480))+(((1.5)*x2481))+(((-3.0)*x2476*x2477)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2482=((1.73205080756888)*sj0); +IkReal x2483=((1.73205080756888)*cj0); +IkReal x2484=((((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*cj5*r10*x2483))+((r11*sj5*x2483))+((cj5*r00*x2482))+(((-1.0)*r01*sj5*x2482))); +j4eval[0]=x2484; +j4eval[1]=IKsign(x2484); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2485=(r01*sj5); +IkReal x2486=((0.866025403784439)*cj0); +IkReal x2487=((1.73205080756888)*sj0); +IkReal x2488=(cj5*r00); +IkReal x2489=((0.866025403784439)*sj0); +IkReal x2490=(cj5*r10); +IkReal x2491=((1.73205080756888)*cj0); +IkReal x2492=(r11*sj5); +CheckValue x2493=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2490*x2491))+((x2487*x2488))+(((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*x2485*x2487))+((x2491*x2492)))),-1); +if(!x2493.valid){ +continue; +} +CheckValue x2494 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+((x2485*x2486))+(((-1.0)*x2486*x2488))+(((-1.0)*x2489*x2490))+(((1.5)*r12*sj0))+((x2489*x2492)))),IkReal(((((-1.0)*r00*sj5*x2491))+(((-1.0)*cj5*r11*x2487))+(((-1.0)*r10*sj5*x2487))+(((-1.0)*cj5*r01*x2491)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2494.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2493.value)))+(x2494.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2495=IKcos(j4); +IkReal x2496=IKsin(j4); +IkReal x2497=((0.5)*r21); +IkReal x2498=(cj0*r10); +IkReal x2499=((0.5)*sj0); +IkReal x2500=(r10*sj0); +IkReal x2501=((0.5)*r20); +IkReal x2502=((1.0)*sj0); +IkReal x2503=(cj0*r01); +IkReal x2504=(r02*sj0); +IkReal x2505=(cj0*r02); +IkReal x2506=(cj0*r00); +IkReal x2507=(cj0*r11); +IkReal x2508=(sj5*x2496); +IkReal x2509=(cj5*x2496); +IkReal x2510=(sj5*x2495); +IkReal x2511=((0.866025403784439)*x2496); +IkReal x2512=((0.866025403784439)*x2495); +IkReal x2513=(cj5*x2495); +IkReal x2514=(r12*x2512); +evalcond[0]=(((x2501*x2509))+(((-1.0)*x2497*x2508))+(((-1.0)*r22*x2511))+((r21*x2513))+((r20*x2510))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x2509))+(((-1.0)*x2497*x2510))+((x2501*x2513))+(((-1.0)*r22*x2512))+(((-1.0)*r20*x2508))); +evalcond[2]=(((r11*x2499*x2510))+((x2500*x2508))+((x2505*x2512))+((sj0*x2514))+((x2506*x2508))+(((0.5)*x2503*x2510))+(((-1.0)*r10*x2499*x2513))+((r11*sj0*x2509))+(((-0.5)*x2506*x2513))+((x2503*x2509))); +evalcond[3]=((((-0.5)*x2498*x2509))+(((0.5)*x2507*x2508))+(((-1.0)*x2498*x2510))+(((-1.0)*r01*x2499*x2508))+((r01*sj0*x2513))+((r00*sj0*x2510))+((cj0*r12*x2511))+(((-1.0)*x2504*x2511))+(((-1.0)*x2507*x2513))+((r00*x2499*x2509))); +evalcond[4]=((0.5)+((cj0*x2514))+(((-1.0)*r00*x2502*x2508))+(((0.5)*x2507*x2510))+(((-1.0)*x2504*x2512))+((x2507*x2509))+(((-0.5)*x2498*x2513))+(((-1.0)*r01*x2502*x2509))+((r00*x2499*x2513))+((x2498*x2508))+(((-1.0)*r01*x2499*x2510))); +evalcond[5]=((1.0)+(((-1.0)*x2506*x2510))+((x2505*x2511))+((r11*x2499*x2508))+(((-1.0)*x2500*x2510))+(((-1.0)*r10*x2499*x2509))+(((0.5)*x2503*x2508))+((r12*sj0*x2511))+(((-1.0)*x2503*x2513))+(((-0.5)*x2506*x2509))+(((-1.0)*r11*x2502*x2513))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2515=cj5*cj5; +IkReal x2516=r20*r20; +IkReal x2517=r21*r21; +IkReal x2518=(r21*sj5); +IkReal x2519=(cj5*r20); +IkReal x2520=((1.73205080756888)*r22); +IkReal x2521=((1.5)*x2515); +CheckValue x2522=IKPowWithIntegerCheck(IKsign((((x2516*x2521))+((x2519*x2520))+(((-1.5)*(r22*r22)))+(((-0.5)*x2517))+(((-3.0)*x2518*x2519))+(((-2.0)*x2516))+(((-1.0)*x2517*x2521))+(((-1.0)*x2518*x2520)))),-1); +if(!x2522.valid){ +continue; +} +CheckValue x2523 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x2519))+(((1.5)*r22))+(((0.866025403784439)*x2518)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2523.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2522.value)))+(x2523.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2524=IKcos(j4); +IkReal x2525=IKsin(j4); +IkReal x2526=((0.5)*r21); +IkReal x2527=(cj0*r10); +IkReal x2528=((0.5)*sj0); +IkReal x2529=(r10*sj0); +IkReal x2530=((0.5)*r20); +IkReal x2531=((1.0)*sj0); +IkReal x2532=(cj0*r01); +IkReal x2533=(r02*sj0); +IkReal x2534=(cj0*r02); +IkReal x2535=(cj0*r00); +IkReal x2536=(cj0*r11); +IkReal x2537=(sj5*x2525); +IkReal x2538=(cj5*x2525); +IkReal x2539=(sj5*x2524); +IkReal x2540=((0.866025403784439)*x2525); +IkReal x2541=((0.866025403784439)*x2524); +IkReal x2542=(cj5*x2524); +IkReal x2543=(r12*x2541); +evalcond[0]=(((x2530*x2538))+((r21*x2542))+((r20*x2539))+(((-1.0)*x2526*x2537))+(((-1.0)*r22*x2540))); +evalcond[1]=((-0.866025403784439)+((x2530*x2542))+(((-1.0)*x2526*x2539))+(((-1.0)*r21*x2538))+(((-1.0)*r22*x2541))+(((-1.0)*r20*x2537))); +evalcond[2]=(((x2534*x2541))+(((-0.5)*x2535*x2542))+(((-1.0)*r10*x2528*x2542))+((r11*x2528*x2539))+((x2532*x2538))+(((0.5)*x2532*x2539))+((sj0*x2543))+((x2529*x2537))+((r11*sj0*x2538))+((x2535*x2537))); +evalcond[3]=(((r01*sj0*x2542))+((cj0*r12*x2540))+(((0.5)*x2536*x2537))+(((-1.0)*x2536*x2542))+(((-1.0)*x2533*x2540))+(((-0.5)*x2527*x2538))+((r00*x2528*x2538))+(((-1.0)*r01*x2528*x2537))+((r00*sj0*x2539))+(((-1.0)*x2527*x2539))); +evalcond[4]=((0.5)+((r00*x2528*x2542))+(((0.5)*x2536*x2539))+((x2536*x2538))+(((-1.0)*r01*x2531*x2538))+(((-1.0)*x2533*x2541))+((x2527*x2537))+(((-1.0)*r01*x2528*x2539))+(((-0.5)*x2527*x2542))+((cj0*x2543))+(((-1.0)*r00*x2531*x2537))); +evalcond[5]=((1.0)+((r12*sj0*x2540))+(((-0.5)*x2535*x2538))+((x2534*x2540))+((r11*x2528*x2537))+(((-1.0)*x2529*x2539))+(((-1.0)*x2535*x2539))+(((-1.0)*r10*x2528*x2538))+(((-1.0)*x2532*x2542))+(((0.5)*x2532*x2537))+(((-1.0)*r11*x2531*x2542))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2544=((0.866025403784439)*sj5); +IkReal x2545=((0.866025403784439)*cj5); +CheckValue x2546 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2546.valid){ +continue; +} +CheckValue x2547=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj0*r10*x2545))+((r00*sj0*x2545))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((-1.0)*r01*sj0*x2544))+((cj0*r11*x2544)))),-1); +if(!x2547.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2546.value)+(((1.5707963267949)*(x2547.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2548=IKcos(j4); +IkReal x2549=IKsin(j4); +IkReal x2550=((0.5)*r21); +IkReal x2551=(cj0*r10); +IkReal x2552=((0.5)*sj0); +IkReal x2553=(r10*sj0); +IkReal x2554=((0.5)*r20); +IkReal x2555=((1.0)*sj0); +IkReal x2556=(cj0*r01); +IkReal x2557=(r02*sj0); +IkReal x2558=(cj0*r02); +IkReal x2559=(cj0*r00); +IkReal x2560=(cj0*r11); +IkReal x2561=(sj5*x2549); +IkReal x2562=(cj5*x2549); +IkReal x2563=(sj5*x2548); +IkReal x2564=((0.866025403784439)*x2549); +IkReal x2565=((0.866025403784439)*x2548); +IkReal x2566=(cj5*x2548); +IkReal x2567=(r12*x2565); +evalcond[0]=((((-1.0)*r22*x2564))+((x2554*x2562))+(((-1.0)*x2550*x2561))+((r20*x2563))+((r21*x2566))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x2562))+(((-1.0)*r22*x2565))+((x2554*x2566))+(((-1.0)*x2550*x2563))+(((-1.0)*r20*x2561))); +evalcond[2]=(((r11*x2552*x2563))+((x2556*x2562))+((x2559*x2561))+((x2558*x2565))+(((-1.0)*r10*x2552*x2566))+(((-0.5)*x2559*x2566))+((r11*sj0*x2562))+(((0.5)*x2556*x2563))+((sj0*x2567))+((x2553*x2561))); +evalcond[3]=((((-1.0)*r01*x2552*x2561))+(((0.5)*x2560*x2561))+(((-1.0)*x2557*x2564))+((r00*x2552*x2562))+(((-1.0)*x2551*x2563))+((cj0*r12*x2564))+(((-0.5)*x2551*x2562))+((r00*sj0*x2563))+(((-1.0)*x2560*x2566))+((r01*sj0*x2566))); +evalcond[4]=((0.5)+((x2560*x2562))+(((-1.0)*r01*x2552*x2563))+(((0.5)*x2560*x2563))+(((-1.0)*r01*x2555*x2562))+(((-1.0)*x2557*x2565))+((cj0*x2567))+((r00*x2552*x2566))+(((-1.0)*r00*x2555*x2561))+(((-0.5)*x2551*x2566))+((x2551*x2561))); +evalcond[5]=((1.0)+(((-1.0)*x2556*x2566))+((r11*x2552*x2561))+(((-1.0)*r11*x2555*x2566))+((x2558*x2564))+(((-1.0)*r10*x2552*x2562))+(((-0.5)*x2559*x2562))+(((-1.0)*x2559*x2563))+(((0.5)*x2556*x2561))+((r12*sj0*x2564))+(((-1.0)*x2553*x2563))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2568=((0.866025403784439)*sj5); +IkReal x2569=(r01*sj0); +IkReal x2570=(r02*sj0); +IkReal x2571=(cj0*r11); +IkReal x2572=(cj0*r12); +IkReal x2573=((1.73205080756888)*sj5); +IkReal x2574=(cj0*cj5*r10); +IkReal x2575=(cj5*r00*sj0); +j4eval[0]=(x2572+(((-1.0)*x2571*x2573))+(((-1.73205080756888)*x2575))+(((1.73205080756888)*x2574))+(((-1.0)*x2570))+((x2569*x2573))); +j4eval[1]=IKsign(((((0.866025403784439)*x2574))+(((0.5)*x2572))+(((-1.0)*x2568*x2571))+(((-0.5)*x2570))+(((-0.866025403784439)*x2575))+((x2568*x2569)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2576=r20*r20; +IkReal x2577=cj5*cj5; +IkReal x2578=r21*r21; +IkReal x2579=r22*r22; +IkReal x2580=(cj5*r20); +IkReal x2581=(r21*sj5); +IkReal x2582=((1.73205080756888)*r22); +IkReal x2583=((3.46410161513775)*r22); +IkReal x2584=(x2577*x2578); +IkReal x2585=(x2576*x2577); +j4eval[0]=((((3.0)*x2585))+(((-3.0)*x2584))+(((-3.0)*x2579))+(((-4.0)*x2576))+(((-6.0)*x2580*x2581))+((x2580*x2583))+(((-1.0)*x2578))+(((-1.0)*x2581*x2583))); +j4eval[1]=((IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))+(IKabs(((((1.5)*r22))+(((0.866025403784439)*x2581))+(((-0.866025403784439)*x2580)))))); +j4eval[2]=IKsign(((((-1.5)*x2579))+((x2580*x2582))+(((1.5)*x2585))+(((-0.5)*x2578))+(((-2.0)*x2576))+(((-1.5)*x2584))+(((-3.0)*x2580*x2581))+(((-1.0)*x2581*x2582)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=1.5707963267949; +sj1=1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2586=((1.73205080756888)*sj0); +IkReal x2587=((1.73205080756888)*cj0); +IkReal x2588=((((-1.0)*cj0*r12))+((r02*sj0))+((r11*sj5*x2587))+((cj5*r00*x2586))+(((-1.0)*r01*sj5*x2586))+(((-1.0)*cj5*r10*x2587))); +j4eval[0]=x2588; +j4eval[1]=IKsign(x2588); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2589=(r01*sj5); +IkReal x2590=((0.866025403784439)*cj0); +IkReal x2591=((1.73205080756888)*sj0); +IkReal x2592=(cj5*r00); +IkReal x2593=((0.866025403784439)*sj0); +IkReal x2594=(cj5*r10); +IkReal x2595=((1.73205080756888)*cj0); +IkReal x2596=(r11*sj5); +CheckValue x2597 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+(((-1.0)*x2590*x2592))+(((-1.0)*x2593*x2594))+((x2593*x2596))+(((1.5)*r12*sj0))+((x2589*x2590)))),IkReal(((((-1.0)*r10*sj5*x2591))+(((-1.0)*cj5*r11*x2591))+(((-1.0)*cj5*r01*x2595))+(((-1.0)*r00*sj5*x2595)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2597.valid){ +continue; +} +CheckValue x2598=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2594*x2595))+((x2591*x2592))+(((-1.0)*cj0*r12))+(((-1.0)*x2589*x2591))+((r02*sj0))+((x2595*x2596)))),-1); +if(!x2598.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2597.value)+(((1.5707963267949)*(x2598.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2599=IKcos(j4); +IkReal x2600=IKsin(j4); +IkReal x2601=((0.5)*r21); +IkReal x2602=(cj0*r10); +IkReal x2603=((0.5)*sj0); +IkReal x2604=(r10*sj0); +IkReal x2605=((0.5)*r20); +IkReal x2606=((1.0)*sj0); +IkReal x2607=(cj0*r01); +IkReal x2608=(r02*sj0); +IkReal x2609=(cj0*r02); +IkReal x2610=(cj0*r00); +IkReal x2611=(cj0*r11); +IkReal x2612=(sj5*x2600); +IkReal x2613=(cj5*x2600); +IkReal x2614=(sj5*x2599); +IkReal x2615=((0.866025403784439)*x2600); +IkReal x2616=((0.866025403784439)*x2599); +IkReal x2617=(cj5*x2599); +IkReal x2618=(r12*x2616); +evalcond[0]=(((r20*x2614))+(((-1.0)*r22*x2615))+((x2605*x2613))+(((-1.0)*x2601*x2612))+((r21*x2617))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r20*x2612))+(((-1.0)*r22*x2616))+((x2605*x2617))+(((-1.0)*r21*x2613))+(((-1.0)*x2601*x2614))); +evalcond[2]=((((-1.0)*r10*x2603*x2617))+((x2610*x2612))+((r11*sj0*x2613))+((r11*x2603*x2614))+(((0.5)*x2607*x2614))+((x2609*x2616))+((x2604*x2612))+(((-0.5)*x2610*x2617))+((sj0*x2618))+((x2607*x2613))); +evalcond[3]=((((-1.0)*r01*x2603*x2612))+(((-1.0)*x2608*x2615))+((cj0*r12*x2615))+((r01*sj0*x2617))+(((0.5)*x2611*x2612))+((r00*x2603*x2613))+(((-1.0)*x2611*x2617))+((r00*sj0*x2614))+(((-1.0)*x2602*x2614))+(((-0.5)*x2602*x2613))); +evalcond[4]=((-0.5)+(((-1.0)*r01*x2603*x2614))+(((-1.0)*x2608*x2616))+((x2602*x2612))+(((-1.0)*r00*x2606*x2612))+(((0.5)*x2611*x2614))+((r00*x2603*x2617))+(((-1.0)*r01*x2606*x2613))+((x2611*x2613))+(((-0.5)*x2602*x2617))+((cj0*x2618))); +evalcond[5]=((-1.0)+((r12*sj0*x2615))+(((-1.0)*r10*x2603*x2613))+(((-1.0)*r11*x2606*x2617))+((r11*x2603*x2612))+(((0.5)*x2607*x2612))+(((-1.0)*x2607*x2617))+((x2609*x2615))+(((-0.5)*x2610*x2613))+(((-1.0)*x2604*x2614))+(((-1.0)*x2610*x2614))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2619=cj5*cj5; +IkReal x2620=r20*r20; +IkReal x2621=r21*r21; +IkReal x2622=(r21*sj5); +IkReal x2623=(cj5*r20); +IkReal x2624=((1.73205080756888)*r22); +IkReal x2625=((1.5)*x2619); +CheckValue x2626 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((0.866025403784439)*x2622))+(((-0.866025403784439)*x2623)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2626.valid){ +continue; +} +CheckValue x2627=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2622*x2624))+(((-1.5)*(r22*r22)))+(((-3.0)*x2622*x2623))+(((-0.5)*x2621))+((x2620*x2625))+((x2623*x2624))+(((-2.0)*x2620))+(((-1.0)*x2621*x2625)))),-1); +if(!x2627.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2626.value)+(((1.5707963267949)*(x2627.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2628=IKcos(j4); +IkReal x2629=IKsin(j4); +IkReal x2630=((0.5)*r21); +IkReal x2631=(cj0*r10); +IkReal x2632=((0.5)*sj0); +IkReal x2633=(r10*sj0); +IkReal x2634=((0.5)*r20); +IkReal x2635=((1.0)*sj0); +IkReal x2636=(cj0*r01); +IkReal x2637=(r02*sj0); +IkReal x2638=(cj0*r02); +IkReal x2639=(cj0*r00); +IkReal x2640=(cj0*r11); +IkReal x2641=(sj5*x2629); +IkReal x2642=(cj5*x2629); +IkReal x2643=(sj5*x2628); +IkReal x2644=((0.866025403784439)*x2629); +IkReal x2645=((0.866025403784439)*x2628); +IkReal x2646=(cj5*x2628); +IkReal x2647=(r12*x2645); +evalcond[0]=((((-1.0)*r22*x2644))+((r21*x2646))+((x2634*x2642))+(((-1.0)*x2630*x2641))+((r20*x2643))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r22*x2645))+(((-1.0)*r21*x2642))+((x2634*x2646))+(((-1.0)*x2630*x2643))+(((-1.0)*r20*x2641))); +evalcond[2]=(((sj0*x2647))+((x2639*x2641))+((x2638*x2645))+(((0.5)*x2636*x2643))+(((-0.5)*x2639*x2646))+((r11*x2632*x2643))+((x2636*x2642))+((x2633*x2641))+((r11*sj0*x2642))+(((-1.0)*r10*x2632*x2646))); +evalcond[3]=((((0.5)*x2640*x2641))+((r01*sj0*x2646))+(((-1.0)*x2640*x2646))+(((-0.5)*x2631*x2642))+(((-1.0)*x2637*x2644))+((r00*sj0*x2643))+(((-1.0)*r01*x2632*x2641))+(((-1.0)*x2631*x2643))+((r00*x2632*x2642))+((cj0*r12*x2644))); +evalcond[4]=((-0.5)+(((0.5)*x2640*x2643))+(((-1.0)*r01*x2635*x2642))+(((-0.5)*x2631*x2646))+(((-1.0)*x2637*x2645))+((cj0*x2647))+(((-1.0)*r01*x2632*x2643))+((x2631*x2641))+(((-1.0)*r00*x2635*x2641))+((r00*x2632*x2646))+((x2640*x2642))); +evalcond[5]=((-1.0)+(((-1.0)*r11*x2635*x2646))+((r12*sj0*x2644))+((x2638*x2644))+(((-1.0)*x2633*x2643))+(((0.5)*x2636*x2641))+(((-0.5)*x2639*x2642))+(((-1.0)*x2636*x2646))+((r11*x2632*x2641))+(((-1.0)*x2639*x2643))+(((-1.0)*r10*x2632*x2642))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2648=((0.866025403784439)*sj5); +IkReal x2649=((0.866025403784439)*cj5); +CheckValue x2650=IKPowWithIntegerCheck(IKsign((((r01*sj0*x2648))+(((-0.5)*r02*sj0))+((cj0*r10*x2649))+(((-1.0)*r00*sj0*x2649))+(((0.5)*cj0*r12))+(((-1.0)*cj0*r11*x2648)))),-1); +if(!x2650.valid){ +continue; +} +CheckValue x2651 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2651.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2650.value)))+(x2651.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2652=IKcos(j4); +IkReal x2653=IKsin(j4); +IkReal x2654=((0.5)*r21); +IkReal x2655=(cj0*r10); +IkReal x2656=((0.5)*sj0); +IkReal x2657=(r10*sj0); +IkReal x2658=((0.5)*r20); +IkReal x2659=((1.0)*sj0); +IkReal x2660=(cj0*r01); +IkReal x2661=(r02*sj0); +IkReal x2662=(cj0*r02); +IkReal x2663=(cj0*r00); +IkReal x2664=(cj0*r11); +IkReal x2665=(sj5*x2653); +IkReal x2666=(cj5*x2653); +IkReal x2667=(sj5*x2652); +IkReal x2668=((0.866025403784439)*x2653); +IkReal x2669=((0.866025403784439)*x2652); +IkReal x2670=(cj5*x2652); +IkReal x2671=(r12*x2669); +evalcond[0]=(((x2658*x2666))+((r21*x2670))+(((-1.0)*x2654*x2665))+(((-1.0)*r22*x2668))+((r20*x2667))); +evalcond[1]=((-0.866025403784439)+((x2658*x2670))+(((-1.0)*r21*x2666))+(((-1.0)*r20*x2665))+(((-1.0)*x2654*x2667))+(((-1.0)*r22*x2669))); +evalcond[2]=(((x2657*x2665))+((sj0*x2671))+((x2662*x2669))+((x2663*x2665))+((r11*x2656*x2667))+(((-1.0)*r10*x2656*x2670))+(((-0.5)*x2663*x2670))+((x2660*x2666))+((r11*sj0*x2666))+(((0.5)*x2660*x2667))); +evalcond[3]=(((cj0*r12*x2668))+(((-0.5)*x2655*x2666))+((r01*sj0*x2670))+(((-1.0)*x2655*x2667))+((r00*x2656*x2666))+((r00*sj0*x2667))+(((0.5)*x2664*x2665))+(((-1.0)*r01*x2656*x2665))+(((-1.0)*x2664*x2670))+(((-1.0)*x2661*x2668))); +evalcond[4]=((-0.5)+(((-1.0)*r01*x2659*x2666))+(((-0.5)*x2655*x2670))+((r00*x2656*x2670))+((x2655*x2665))+((x2664*x2666))+(((0.5)*x2664*x2667))+((cj0*x2671))+(((-1.0)*r00*x2659*x2665))+(((-1.0)*r01*x2656*x2667))+(((-1.0)*x2661*x2669))); +evalcond[5]=((-1.0)+((x2662*x2668))+((r12*sj0*x2668))+(((-1.0)*x2657*x2667))+((r11*x2656*x2665))+(((-1.0)*r10*x2656*x2666))+(((-0.5)*x2663*x2666))+(((-1.0)*x2663*x2667))+(((-1.0)*x2660*x2670))+(((-1.0)*r11*x2659*x2670))+(((0.5)*x2660*x2665))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2672=cj5*cj5; +IkReal x2673=(r20*sj0); +IkReal x2674=(cj0*r01); +IkReal x2675=((0.866025403784439)*r22); +IkReal x2676=((0.5)*r21); +IkReal x2677=(cj0*r00); +IkReal x2678=((0.866025403784439)*r12); +IkReal x2679=(cj5*r20); +IkReal x2680=(r21*sj0); +IkReal x2681=((1.5)*sj5); +IkReal x2682=(cj3*sj5); +IkReal x2683=(cj0*r02); +IkReal x2684=(cj5*r21); +IkReal x2685=(cj5*r10); +IkReal x2686=((1.5)*r22); +IkReal x2687=(r11*sj0); +IkReal x2688=((1.5)*x2672); +CheckValue x2689=IKPowWithIntegerCheck(IKsign(((((-1.0)*r20*x2677*x2688))+(((-1.0)*cj5*x2675*x2677))+((x2680*x2681*x2685))+(((-0.866025403784439)*x2679*x2683))+(((0.866025403784439)*r21*sj5*x2683))+((sj5*x2674*x2675))+((x2683*x2686))+(((-1.0)*cj5*x2673*x2678))+((x2677*x2681*x2684))+(((2.0)*r10*x2673))+(((-1.0)*r10*x2673*x2688))+((r11*x2680*x2688))+((sj5*x2678*x2680))+((cj5*r11*x2673*x2681))+((x2676*x2687))+((x2674*x2679*x2681))+(((2.0)*r20*x2677))+(((-1.0)*sj0*x2675*x2685))+((r21*x2674*x2688))+((sj5*x2675*x2687))+((r12*sj0*x2686))+((x2674*x2676)))),-1); +if(!x2689.valid){ +continue; +} +CheckValue x2690 = IKatan2WithCheck(IkReal((((cj3*x2684))+((r20*x2682)))),IkReal(((((-0.5)*cj3*x2679))+((cj3*x2675))+((x2676*x2682)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2690.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2689.value)))+(x2690.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2691=IKcos(j4); +IkReal x2692=IKsin(j4); +IkReal x2693=((0.5)*cj5); +IkReal x2694=(r01*sj0); +IkReal x2695=((1.0)*sj5); +IkReal x2696=(r11*sj0); +IkReal x2697=(r02*sj0); +IkReal x2698=(r00*sj0); +IkReal x2699=(cj0*r02); +IkReal x2700=(cj0*r01); +IkReal x2701=(r10*sj0); +IkReal x2702=(cj0*r00); +IkReal x2703=(cj0*r11); +IkReal x2704=(cj0*r10); +IkReal x2705=(sj5*x2691); +IkReal x2706=(r20*x2692); +IkReal x2707=(cj5*x2692); +IkReal x2708=((0.866025403784439)*x2692); +IkReal x2709=((0.866025403784439)*x2691); +IkReal x2710=(cj5*x2691); +IkReal x2711=(x2692*x2704); +IkReal x2712=(x2692*x2701); +IkReal x2713=(r12*x2709); +IkReal x2714=((0.5)*sj5*x2692); +evalcond[0]=((((-1.0)*r22*x2708))+((x2693*x2706))+(((-1.0)*r21*x2714))+((r20*x2705))+((r21*x2710))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r22*x2709))+((r20*x2691*x2693))+(((-1.0)*r21*x2707))+(((-0.5)*r21*x2705))+(((-1.0)*x2695*x2706))); +evalcond[2]=(((cj0*r12*x2708))+(((-1.0)*x2703*x2710))+cj3+(((-1.0)*x2697*x2708))+((x2703*x2714))+((x2698*x2705))+(((-1.0)*x2691*x2695*x2704))+((x2692*x2693*x2698))+(((-1.0)*x2693*x2711))+(((-1.0)*x2694*x2714))+((x2694*x2710))); +evalcond[3]=(((x2699*x2709))+((sj0*x2713))+(((-1.0)*x2691*x2693*x2702))+(((-1.0)*x2691*x2693*x2701))+(((0.5)*x2700*x2705))+((x2700*x2707))+((sj5*x2712))+((sj5*x2692*x2702))+(((-0.5)*cj3))+(((0.5)*x2696*x2705))+((x2696*x2707))); +evalcond[4]=((((-1.0)*x2692*x2695*x2698))+(((-1.0)*x2697*x2709))+((x2691*x2693*x2698))+(((0.5)*sj3))+(((0.5)*x2703*x2705))+(((-1.0)*x2691*x2693*x2704))+((cj0*x2713))+((sj5*x2711))+(((-0.5)*x2694*x2705))+((x2703*x2707))+(((-1.0)*x2694*x2707))); +evalcond[5]=(sj3+((x2699*x2708))+(((-1.0)*x2691*x2695*x2702))+(((-1.0)*x2691*x2695*x2701))+(((-1.0)*x2696*x2710))+(((-1.0)*x2700*x2710))+((x2700*x2714))+(((-1.0)*x2692*x2693*x2702))+((r12*sj0*x2708))+(((-1.0)*x2693*x2712))+((x2696*x2714))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2715=cj5*cj5; +IkReal x2716=r20*r20; +IkReal x2717=r21*r21; +IkReal x2718=(r21*sj5); +IkReal x2719=(cj5*r20); +IkReal x2720=((1.73205080756888)*r22); +IkReal x2721=((1.5)*x2715); +CheckValue x2722 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x2719))+(((1.5)*r22))+(((0.866025403784439)*x2718)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2722.valid){ +continue; +} +CheckValue x2723=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2718*x2720))+(((-1.5)*(r22*r22)))+(((-3.0)*x2718*x2719))+((x2719*x2720))+(((-1.0)*x2717*x2721))+((x2716*x2721))+(((-0.5)*x2717))+(((-2.0)*x2716)))),-1); +if(!x2723.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2722.value)+(((1.5707963267949)*(x2723.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2724=IKcos(j4); +IkReal x2725=IKsin(j4); +IkReal x2726=((0.5)*cj5); +IkReal x2727=(r01*sj0); +IkReal x2728=((1.0)*sj5); +IkReal x2729=(r11*sj0); +IkReal x2730=(r02*sj0); +IkReal x2731=(r00*sj0); +IkReal x2732=(cj0*r02); +IkReal x2733=(cj0*r01); +IkReal x2734=(r10*sj0); +IkReal x2735=(cj0*r00); +IkReal x2736=(cj0*r11); +IkReal x2737=(cj0*r10); +IkReal x2738=(sj5*x2724); +IkReal x2739=(r20*x2725); +IkReal x2740=(cj5*x2725); +IkReal x2741=((0.866025403784439)*x2725); +IkReal x2742=((0.866025403784439)*x2724); +IkReal x2743=(cj5*x2724); +IkReal x2744=(x2725*x2737); +IkReal x2745=(x2725*x2734); +IkReal x2746=(r12*x2742); +IkReal x2747=((0.5)*sj5*x2725); +evalcond[0]=(((r21*x2743))+((x2726*x2739))+((r20*x2738))+(((-1.0)*r21*x2747))+(((-1.0)*r22*x2741))); +evalcond[1]=((-0.866025403784439)+(((-0.5)*r21*x2738))+(((-1.0)*r21*x2740))+(((-1.0)*x2728*x2739))+((r20*x2724*x2726))+(((-1.0)*r22*x2742))); +evalcond[2]=(cj3+((cj0*r12*x2741))+((x2731*x2738))+(((-1.0)*x2726*x2744))+((x2727*x2743))+(((-1.0)*x2727*x2747))+(((-1.0)*x2724*x2728*x2737))+(((-1.0)*x2736*x2743))+(((-1.0)*x2730*x2741))+((x2736*x2747))+((x2725*x2726*x2731))); +evalcond[3]=((((0.5)*x2729*x2738))+((x2729*x2740))+((sj5*x2745))+((x2733*x2740))+(((0.5)*x2733*x2738))+((sj0*x2746))+(((-0.5)*cj3))+(((-1.0)*x2724*x2726*x2735))+(((-1.0)*x2724*x2726*x2734))+((sj5*x2725*x2735))+((x2732*x2742))); +evalcond[4]=((((0.5)*x2736*x2738))+((sj5*x2744))+((cj0*x2746))+(((0.5)*sj3))+(((-0.5)*x2727*x2738))+(((-1.0)*x2725*x2728*x2731))+(((-1.0)*x2727*x2740))+((x2724*x2726*x2731))+(((-1.0)*x2730*x2742))+((x2736*x2740))+(((-1.0)*x2724*x2726*x2737))); +evalcond[5]=(((x2729*x2747))+sj3+(((-1.0)*x2725*x2726*x2735))+(((-1.0)*x2733*x2743))+(((-1.0)*x2726*x2745))+(((-1.0)*x2729*x2743))+((x2733*x2747))+(((-1.0)*x2724*x2728*x2735))+(((-1.0)*x2724*x2728*x2734))+((r12*sj0*x2741))+((x2732*x2741))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2748=((0.866025403784439)*cj0); +IkReal x2749=(cj3*r21); +IkReal x2750=(cj3*r20); +IkReal x2751=((0.866025403784439)*sj0); +CheckValue x2752 = IKatan2WithCheck(IkReal((((sj5*x2750))+((cj5*x2749)))),IkReal(((((0.5)*sj5*x2749))+(((-0.5)*cj5*x2750))+(((0.866025403784439)*cj3*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2752.valid){ +continue; +} +CheckValue x2753=IKPowWithIntegerCheck(IKsign(((((0.5)*r12*sj0))+((cj5*r10*x2751))+(((0.5)*cj0*r02))+((cj5*r00*x2748))+(((-1.0)*r11*sj5*x2751))+(((-1.0)*r01*sj5*x2748)))),-1); +if(!x2753.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2752.value)+(((1.5707963267949)*(x2753.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2754=IKcos(j4); +IkReal x2755=IKsin(j4); +IkReal x2756=((0.5)*cj5); +IkReal x2757=(r01*sj0); +IkReal x2758=((1.0)*sj5); +IkReal x2759=(r11*sj0); +IkReal x2760=(r02*sj0); +IkReal x2761=(r00*sj0); +IkReal x2762=(cj0*r02); +IkReal x2763=(cj0*r01); +IkReal x2764=(r10*sj0); +IkReal x2765=(cj0*r00); +IkReal x2766=(cj0*r11); +IkReal x2767=(cj0*r10); +IkReal x2768=(sj5*x2754); +IkReal x2769=(r20*x2755); +IkReal x2770=(cj5*x2755); +IkReal x2771=((0.866025403784439)*x2755); +IkReal x2772=((0.866025403784439)*x2754); +IkReal x2773=(cj5*x2754); +IkReal x2774=(x2755*x2767); +IkReal x2775=(x2755*x2764); +IkReal x2776=(r12*x2772); +IkReal x2777=((0.5)*sj5*x2755); +evalcond[0]=(((x2756*x2769))+((r20*x2768))+(((-1.0)*r21*x2777))+((r21*x2773))+(((-1.0)*r22*x2771))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*x2758*x2769))+(((-0.5)*r21*x2768))+(((-1.0)*r21*x2770))+(((-1.0)*r22*x2772))+((r20*x2754*x2756))); +evalcond[2]=(((cj0*r12*x2771))+((x2757*x2773))+((x2766*x2777))+(((-1.0)*x2754*x2758*x2767))+cj3+((x2761*x2768))+(((-1.0)*x2766*x2773))+(((-1.0)*x2756*x2774))+(((-1.0)*x2760*x2771))+(((-1.0)*x2757*x2777))+((x2755*x2756*x2761))); +evalcond[3]=((((-1.0)*x2754*x2756*x2765))+(((-1.0)*x2754*x2756*x2764))+((x2763*x2770))+((x2762*x2772))+(((0.5)*x2763*x2768))+(((0.5)*x2759*x2768))+((sj5*x2755*x2765))+((sj5*x2775))+(((-0.5)*cj3))+((x2759*x2770))+((sj0*x2776))); +evalcond[4]=((((-1.0)*x2754*x2756*x2767))+(((-1.0)*x2757*x2770))+((x2766*x2770))+(((-0.5)*x2757*x2768))+(((0.5)*sj3))+((x2754*x2756*x2761))+((cj0*x2776))+(((0.5)*x2766*x2768))+(((-1.0)*x2760*x2772))+((sj5*x2774))+(((-1.0)*x2755*x2758*x2761))); +evalcond[5]=(((x2763*x2777))+sj3+(((-1.0)*x2754*x2758*x2765))+(((-1.0)*x2754*x2758*x2764))+((x2762*x2771))+(((-1.0)*x2755*x2756*x2765))+(((-1.0)*x2759*x2773))+(((-1.0)*x2756*x2775))+((r12*sj0*x2771))+(((-1.0)*x2763*x2773))+((x2759*x2777))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((1.5707963267949)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(j1, 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x2778=((0.577350269189626)*r02); +IkReal x2779=(cj5*sj0); +IkReal x2780=(sj0*sj5); +IkReal x2781=((1.0)*r11); +IkReal x2782=(cj0*cj5); +IkReal x2783=(cj0*sj5); +IkReal x2784=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*x2781*x2783))+(((-1.0)*r00*x2779))+((r01*x2780))+((cj0*x2784))+((r10*x2782))+(((-1.0)*sj0*x2778)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*x2780*x2781))+((cj0*x2778))+((r10*x2779))+((sj0*x2784))+((r00*x2782))+(((-1.0)*r01*x2783)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x2781*x2783))+(((-1.0)*r00*x2779))+((r01*x2780))+((cj0*x2784))+((r10*x2782))+(((-1.0)*sj0*x2778))))+IKsqr(((((-1.0)*x2780*x2781))+((cj0*x2778))+((r10*x2779))+((sj0*x2784))+((r00*x2782))+(((-1.0)*r01*x2783))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*x2781*x2783))+(((-1.0)*r00*x2779))+((r01*x2780))+((cj0*x2784))+((r10*x2782))+(((-1.0)*sj0*x2778))), ((((-1.0)*x2780*x2781))+((cj0*x2778))+((r10*x2779))+((sj0*x2784))+((r00*x2782))+(((-1.0)*r01*x2783)))); +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[2]; +IkReal x2785=((0.866025403784439)*sj5); +IkReal x2786=((0.5)*sj0); +IkReal x2787=((0.5)*cj0); +IkReal x2788=((0.866025403784439)*cj5*r10); +IkReal x2789=((0.866025403784439)*cj5*r00); +evalcond[0]=((((-1.0)*r12*x2787))+(((-1.0)*r01*sj0*x2785))+((r02*x2786))+((cj0*r11*x2785))+((sj0*x2789))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*cj0*x2788))); +evalcond[1]=((((-1.0)*r12*x2786))+((cj0*r01*x2785))+((r11*sj0*x2785))+(((-1.0)*sj0*x2788))+(((0.866025403784439)*(IKcos(j3))))+(((-1.0)*cj0*x2789))+(((-1.0)*r02*x2787))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x2790=(r12*sj0); +IkReal x2791=(cj3*sj5); +IkReal x2792=(cj0*r02); +IkReal x2793=(cj3*cj5); +IkReal x2794=(cj0*cj5*r00); +IkReal x2795=(r11*sj0*sj5); +IkReal x2796=(cj0*r01*sj5); +IkReal x2797=(cj5*r10*sj0); +j4eval[0]=(x2792+x2790+(((1.73205080756888)*x2794))+(((1.73205080756888)*x2797))+(((-1.73205080756888)*x2795))+(((-1.73205080756888)*x2796))); +j4eval[1]=((((0.5)*(IKabs((((r21*x2791))+(((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x2793)))))))+(IKabs((((r21*x2793))+((r20*x2791)))))); +j4eval[2]=IKsign(((((0.866025403784439)*x2794))+(((0.866025403784439)*x2797))+(((0.5)*x2792))+(((0.5)*x2790))+(((-0.866025403784439)*x2795))+(((-0.866025403784439)*x2796)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x2798=r21*r21; +IkReal x2799=cj5*cj5; +IkReal x2800=r20*r20; +IkReal x2801=r22*r22; +IkReal x2802=(r21*sj5); +IkReal x2803=((1.73205080756888)*r20); +IkReal x2804=(cj5*r20); +IkReal x2805=((1.15470053837925)*r22); +IkReal x2806=(x2798*x2799); +IkReal x2807=(x2799*x2800); +j4eval[0]=((((-1.0)*x2807))+(((1.33333333333333)*x2800))+(((2.0)*x2802*x2804))+((x2802*x2805))+(((-1.0)*x2804*x2805))+(((0.333333333333333)*x2798))+x2801+x2806); +j4eval[1]=IKsign(((((2.0)*x2800))+(((1.5)*x2806))+(((1.5)*x2801))+(((1.73205080756888)*r22*x2802))+(((-1.5)*x2807))+(((0.5)*x2798))+(((3.0)*x2802*x2804))+(((-1.0)*cj5*r22*x2803)))); +j4eval[2]=((IKabs(((((1.73205080756888)*cj5*r21))+((sj5*x2803)))))+(IKabs(((((1.5)*r22))+(((-0.866025403784439)*x2804))+(((0.866025403784439)*x2802)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +IkReal x2808=cj5*cj5; +IkReal x2809=(cj0*r01); +IkReal x2810=((0.866025403784439)*r22); +IkReal x2811=(cj5*sj5); +IkReal x2812=((0.5)*r21); +IkReal x2813=(cj0*r22); +IkReal x2814=(cj0*r00); +IkReal x2815=((2.0)*r20); +IkReal x2816=(r20*sj0); +IkReal x2817=((0.866025403784439)*r12); +IkReal x2818=((3.0)*r11); +IkReal x2819=((3.0)*sj0); +IkReal x2820=((1.73205080756888)*cj5); +IkReal x2821=(r21*sj5); +IkReal x2822=(r11*sj0); +IkReal x2823=(r12*r22); +IkReal x2824=(r10*sj0); +IkReal x2825=(cj3*cj5); +IkReal x2826=((3.0)*r21); +IkReal x2827=((1.0)*r21); +IkReal x2828=(cj0*r02); +IkReal x2829=((1.5)*x2824); +IkReal x2830=(r20*x2808); +IkReal x2831=((1.73205080756888)*r22*sj5); +IkReal x2832=(r20*x2828); +IkReal x2833=((1.5)*r21*x2808); +j4eval[0]=((((-1.0)*r10*r21*x2811*x2819))+(((-1.0)*x2809*x2827))+(((3.0)*r10*x2808*x2816))+(((-3.0)*r02*x2813))+(((-1.0)*x2809*x2831))+(((-1.0)*x2811*x2816*x2818))+(((3.0)*x2814*x2830))+(((-1.0)*x2811*x2814*x2826))+(((-1.0)*x2822*x2827))+(((-3.0)*r20*x2809*x2811))+((x2820*x2832))+(((-1.0)*x2808*x2809*x2826))+(((-1.0)*x2822*x2831))+((r00*x2813*x2820))+(((-1.0)*x2819*x2823))+(((-1.73205080756888)*r12*sj0*x2821))+(((-1.73205080756888)*x2821*x2828))+((r12*x2816*x2820))+(((-4.0)*r10*x2816))+(((-1.0)*r21*sj0*x2808*x2818))+(((-4.0)*r20*x2814))+((r22*x2820*x2824))); +j4eval[1]=((IKabs((((cj3*r20*sj5))+((r21*x2825)))))+(((0.5)*(IKabs((((cj3*x2821))+(((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x2825)))))))); +j4eval[2]=IKsign(((((-1.0)*x2812*x2822))+(((-1.5)*r20*x2809*x2811))+(((-1.0)*x2809*x2812))+((cj5*x2816*x2817))+(((-1.0)*x2809*x2833))+(((-0.866025403784439)*x2821*x2828))+(((-1.0)*sj5*x2810*x2822))+(((-1.0)*sj0*x2817*x2821))+(((-1.5)*r02*x2813))+((cj5*x2810*x2814))+((cj5*x2810*x2824))+(((-1.0)*x2822*x2833))+(((-1.0)*sj5*x2809*x2810))+(((-1.5)*r11*x2811*x2816))+(((-1.5)*r21*x2811*x2814))+(((1.5)*x2814*x2830))+(((-1.0)*r21*x2811*x2829))+(((1.5)*r10*x2808*x2816))+(((-1.0)*x2814*x2815))+(((-1.5)*sj0*x2823))+(((0.866025403784439)*cj5*x2832))+(((-1.0)*x2815*x2824)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2834=((0.866025403784439)*sj5); +IkReal x2835=(r01*sj0); +IkReal x2836=(r02*sj0); +IkReal x2837=(cj0*r11); +IkReal x2838=(cj0*r12); +IkReal x2839=((1.73205080756888)*sj5); +IkReal x2840=(cj0*cj5*r10); +IkReal x2841=(cj5*r00*sj0); +j4eval[0]=((((-1.73205080756888)*x2841))+(((-1.0)*x2836))+(((1.73205080756888)*x2840))+(((-1.0)*x2837*x2839))+((x2835*x2839))+x2838); +j4eval[1]=IKsign(((((-0.5)*x2836))+((x2834*x2835))+(((0.866025403784439)*x2840))+(((-0.866025403784439)*x2841))+(((-1.0)*x2834*x2837))+(((0.5)*x2838)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2842=r21*r21; +IkReal x2843=cj5*cj5; +IkReal x2844=r20*r20; +IkReal x2845=r22*r22; +IkReal x2846=(r21*sj5); +IkReal x2847=((1.73205080756888)*r20); +IkReal x2848=(cj5*r20); +IkReal x2849=((1.15470053837925)*r22); +IkReal x2850=(x2842*x2843); +IkReal x2851=(x2843*x2844); +j4eval[0]=((((0.333333333333333)*x2842))+((x2846*x2849))+(((2.0)*x2846*x2848))+(((-1.0)*x2848*x2849))+(((-1.0)*x2851))+x2845+x2850+(((1.33333333333333)*x2844))); +j4eval[1]=IKsign(((((1.73205080756888)*r22*x2846))+(((-1.5)*x2851))+(((-1.0)*cj5*r22*x2847))+(((2.0)*x2844))+(((3.0)*x2846*x2848))+(((1.5)*x2845))+(((1.5)*x2850))+(((0.5)*x2842)))); +j4eval[2]=((IKabs(((((0.866025403784439)*x2846))+(((-0.866025403784439)*x2848))+(((1.5)*r22)))))+(IKabs((((sj5*x2847))+(((1.73205080756888)*cj5*r21)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x2852=((1.73205080756888)*sj0); +IkReal x2853=((1.73205080756888)*cj0); +IkReal x2854=((((-1.0)*r11*sj5*x2853))+(((-1.0)*cj5*r00*x2852))+((r01*sj5*x2852))+(((-1.0)*r02*sj0))+((cj5*r10*x2853))+((cj0*r12))); +j4eval[0]=x2854; +j4eval[1]=IKsign(x2854); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2855=((1.73205080756888)*sj0); +IkReal x2856=(r01*sj5); +IkReal x2857=((0.866025403784439)*cj0); +IkReal x2858=(cj5*r00); +IkReal x2859=((0.866025403784439)*sj0); +IkReal x2860=(cj5*r10); +IkReal x2861=((1.73205080756888)*cj0); +IkReal x2862=(r11*sj5); +CheckValue x2863 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+(((-1.0)*x2859*x2860))+(((-1.0)*x2857*x2858))+(((1.5)*r12*sj0))+((x2859*x2862))+((x2856*x2857)))),IkReal(((((-1.0)*r10*sj5*x2855))+(((-1.0)*cj5*r11*x2855))+(((-1.0)*cj5*r01*x2861))+(((-1.0)*r00*sj5*x2861)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2863.valid){ +continue; +} +CheckValue x2864=IKPowWithIntegerCheck(IKsign((((x2855*x2856))+(((-1.0)*x2861*x2862))+(((-1.0)*r02*sj0))+(((-1.0)*x2855*x2858))+((x2860*x2861))+((cj0*r12)))),-1); +if(!x2864.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2863.value)+(((1.5707963267949)*(x2864.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2865=IKcos(j4); +IkReal x2866=IKsin(j4); +IkReal x2867=((0.5)*r21); +IkReal x2868=(cj0*r10); +IkReal x2869=((0.5)*sj0); +IkReal x2870=(r10*sj0); +IkReal x2871=((0.5)*r20); +IkReal x2872=((1.0)*sj0); +IkReal x2873=(cj0*r01); +IkReal x2874=(r02*sj0); +IkReal x2875=(cj0*r02); +IkReal x2876=(cj0*r00); +IkReal x2877=(cj0*r11); +IkReal x2878=(sj5*x2866); +IkReal x2879=(cj5*x2866); +IkReal x2880=(sj5*x2865); +IkReal x2881=((0.866025403784439)*x2866); +IkReal x2882=((0.866025403784439)*x2865); +IkReal x2883=(cj5*x2865); +IkReal x2884=(r12*x2882); +evalcond[0]=(((r20*x2880))+(((-1.0)*r22*x2881))+((r21*x2883))+(((-1.0)*x2867*x2878))+((x2871*x2879))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r22*x2882))+(((-1.0)*r20*x2878))+(((-1.0)*x2867*x2880))+(((-1.0)*r21*x2879))+((x2871*x2883))); +evalcond[2]=((((-0.5)*x2876*x2883))+((x2876*x2878))+((x2870*x2878))+(((0.5)*x2873*x2880))+((sj0*x2884))+((r11*x2869*x2880))+((x2873*x2879))+(((-1.0)*r10*x2869*x2883))+((x2875*x2882))+((r11*sj0*x2879))); +evalcond[3]=((((-1.0)*r01*x2869*x2878))+(((-1.0)*x2868*x2880))+(((-0.5)*x2868*x2879))+(((0.5)*x2877*x2878))+((r01*sj0*x2883))+(((-1.0)*x2874*x2881))+((r00*sj0*x2880))+(((-1.0)*x2877*x2883))+((r00*x2869*x2879))+((cj0*r12*x2881))); +evalcond[4]=((0.5)+(((-1.0)*r01*x2872*x2879))+(((0.5)*x2877*x2880))+((x2868*x2878))+((x2877*x2879))+(((-1.0)*r00*x2872*x2878))+(((-1.0)*x2874*x2882))+((cj0*x2884))+((r00*x2869*x2883))+(((-1.0)*r01*x2869*x2880))+(((-0.5)*x2868*x2883))); +evalcond[5]=((-1.0)+(((-1.0)*r11*x2872*x2883))+(((0.5)*x2873*x2878))+(((-1.0)*x2876*x2880))+(((-1.0)*x2870*x2880))+((r12*sj0*x2881))+((x2875*x2881))+(((-1.0)*x2873*x2883))+((r11*x2869*x2878))+(((-1.0)*r10*x2869*x2879))+(((-0.5)*x2876*x2879))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2885=r21*r21; +IkReal x2886=cj5*cj5; +IkReal x2887=r20*r20; +IkReal x2888=(r21*sj5); +IkReal x2889=((1.73205080756888)*cj5); +IkReal x2890=(cj5*r20); +IkReal x2891=((1.5)*x2886); +CheckValue x2892=IKPowWithIntegerCheck(IKsign(((((1.73205080756888)*r22*x2888))+(((-1.0)*x2887*x2891))+(((-1.0)*r20*r22*x2889))+(((0.5)*x2885))+(((2.0)*x2887))+(((3.0)*x2888*x2890))+(((1.5)*(r22*r22)))+((x2885*x2891)))),-1); +if(!x2892.valid){ +continue; +} +CheckValue x2893 = IKatan2WithCheck(IkReal((((r21*x2889))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((0.866025403784439)*x2888))+(((-0.866025403784439)*x2890)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2893.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2892.value)))+(x2893.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2894=IKcos(j4); +IkReal x2895=IKsin(j4); +IkReal x2896=((0.5)*r21); +IkReal x2897=(cj0*r10); +IkReal x2898=((0.5)*sj0); +IkReal x2899=(r10*sj0); +IkReal x2900=((0.5)*r20); +IkReal x2901=((1.0)*sj0); +IkReal x2902=(cj0*r01); +IkReal x2903=(r02*sj0); +IkReal x2904=(cj0*r02); +IkReal x2905=(cj0*r00); +IkReal x2906=(cj0*r11); +IkReal x2907=(sj5*x2895); +IkReal x2908=(cj5*x2895); +IkReal x2909=(sj5*x2894); +IkReal x2910=((0.866025403784439)*x2895); +IkReal x2911=((0.866025403784439)*x2894); +IkReal x2912=(cj5*x2894); +IkReal x2913=(r12*x2911); +evalcond[0]=(((x2900*x2908))+(((-1.0)*x2896*x2907))+((r21*x2912))+((r20*x2909))+(((-1.0)*r22*x2910))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x2896*x2909))+(((-1.0)*r20*x2907))+((x2900*x2912))+(((-1.0)*r21*x2908))+(((-1.0)*r22*x2911))); +evalcond[2]=(((r11*x2898*x2909))+((x2899*x2907))+((x2902*x2908))+(((-0.5)*x2905*x2912))+(((-1.0)*r10*x2898*x2912))+((x2905*x2907))+(((0.5)*x2902*x2909))+((sj0*x2913))+((r11*sj0*x2908))+((x2904*x2911))); +evalcond[3]=(((r00*sj0*x2909))+(((-1.0)*r01*x2898*x2907))+(((-1.0)*x2897*x2909))+((cj0*r12*x2910))+((r00*x2898*x2908))+(((0.5)*x2906*x2907))+(((-0.5)*x2897*x2908))+((r01*sj0*x2912))+(((-1.0)*x2906*x2912))+(((-1.0)*x2903*x2910))); +evalcond[4]=((0.5)+((cj0*x2913))+(((-1.0)*r00*x2901*x2907))+((x2906*x2908))+(((-1.0)*r01*x2898*x2909))+(((0.5)*x2906*x2909))+((x2897*x2907))+(((-0.5)*x2897*x2912))+(((-1.0)*r01*x2901*x2908))+((r00*x2898*x2912))+(((-1.0)*x2903*x2911))); +evalcond[5]=((-1.0)+((r11*x2898*x2907))+(((-1.0)*r11*x2901*x2912))+(((0.5)*x2902*x2907))+((r12*sj0*x2910))+(((-0.5)*x2905*x2908))+(((-1.0)*x2902*x2912))+((x2904*x2910))+(((-1.0)*x2899*x2909))+(((-1.0)*r10*x2898*x2908))+(((-1.0)*x2905*x2909))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2914=((0.866025403784439)*sj5); +IkReal x2915=((0.866025403784439)*cj5); +CheckValue x2916 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2916.valid){ +continue; +} +CheckValue x2917=IKPowWithIntegerCheck(IKsign(((((-0.5)*r02*sj0))+(((-1.0)*cj0*r11*x2914))+(((-1.0)*r00*sj0*x2915))+((cj0*r10*x2915))+((r01*sj0*x2914))+(((0.5)*cj0*r12)))),-1); +if(!x2917.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2916.value)+(((1.5707963267949)*(x2917.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2918=IKcos(j4); +IkReal x2919=IKsin(j4); +IkReal x2920=((0.5)*r21); +IkReal x2921=(cj0*r10); +IkReal x2922=((0.5)*sj0); +IkReal x2923=(r10*sj0); +IkReal x2924=((0.5)*r20); +IkReal x2925=((1.0)*sj0); +IkReal x2926=(cj0*r01); +IkReal x2927=(r02*sj0); +IkReal x2928=(cj0*r02); +IkReal x2929=(cj0*r00); +IkReal x2930=(cj0*r11); +IkReal x2931=(sj5*x2919); +IkReal x2932=(cj5*x2919); +IkReal x2933=(sj5*x2918); +IkReal x2934=((0.866025403784439)*x2919); +IkReal x2935=((0.866025403784439)*x2918); +IkReal x2936=(cj5*x2918); +IkReal x2937=(r12*x2935); +evalcond[0]=(((r21*x2936))+((x2924*x2932))+(((-1.0)*x2920*x2931))+((r20*x2933))+(((-1.0)*r22*x2934))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x2932))+((x2924*x2936))+(((-1.0)*x2920*x2933))+(((-1.0)*r20*x2931))+(((-1.0)*r22*x2935))); +evalcond[2]=(((x2929*x2931))+((x2928*x2935))+((r11*x2922*x2933))+(((-1.0)*r10*x2922*x2936))+(((-0.5)*x2929*x2936))+((r11*sj0*x2932))+((x2926*x2932))+(((0.5)*x2926*x2933))+((sj0*x2937))+((x2923*x2931))); +evalcond[3]=(((r00*sj0*x2933))+(((-1.0)*x2927*x2934))+(((-1.0)*r01*x2922*x2931))+((cj0*r12*x2934))+(((0.5)*x2930*x2931))+(((-1.0)*x2921*x2933))+((r00*x2922*x2932))+((r01*sj0*x2936))+(((-0.5)*x2921*x2932))+(((-1.0)*x2930*x2936))); +evalcond[4]=((0.5)+(((-1.0)*x2927*x2935))+((cj0*x2937))+(((-1.0)*r01*x2922*x2933))+(((0.5)*x2930*x2933))+((r00*x2922*x2936))+(((-1.0)*r00*x2925*x2931))+(((-1.0)*r01*x2925*x2932))+((x2921*x2931))+((x2930*x2932))+(((-0.5)*x2921*x2936))); +evalcond[5]=((-1.0)+((r12*sj0*x2934))+(((-1.0)*x2926*x2936))+((x2928*x2934))+((r11*x2922*x2931))+(((-1.0)*r10*x2922*x2932))+(((-0.5)*x2929*x2932))+(((-1.0)*r11*x2925*x2936))+(((-1.0)*x2923*x2933))+(((0.5)*x2926*x2931))+(((-1.0)*x2929*x2933))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2938=(cj0*r12); +IkReal x2939=((0.866025403784439)*sj5); +IkReal x2940=(r01*sj0); +IkReal x2941=(r02*sj0); +IkReal x2942=(cj0*r11); +IkReal x2943=((1.73205080756888)*sj5); +IkReal x2944=(cj0*cj5*r10); +IkReal x2945=(cj5*r00*sj0); +j4eval[0]=((((-1.0)*x2940*x2943))+(((1.73205080756888)*x2945))+(((-1.73205080756888)*x2944))+((x2942*x2943))+(((-1.0)*x2938))+x2941); +j4eval[1]=IKsign((((x2939*x2942))+(((-1.0)*x2939*x2940))+(((-0.5)*x2938))+(((0.866025403784439)*x2945))+(((0.5)*x2941))+(((-0.866025403784439)*x2944)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2946=r21*r21; +IkReal x2947=cj5*cj5; +IkReal x2948=r20*r20; +IkReal x2949=r22*r22; +IkReal x2950=(r21*sj5); +IkReal x2951=((1.73205080756888)*r20); +IkReal x2952=(cj5*r20); +IkReal x2953=((1.15470053837925)*r22); +IkReal x2954=(x2946*x2947); +IkReal x2955=(x2947*x2948); +j4eval[0]=((((-1.0)*x2955))+((x2950*x2953))+(((1.33333333333333)*x2948))+(((-1.0)*x2952*x2953))+(((2.0)*x2950*x2952))+(((0.333333333333333)*x2946))+x2954+x2949); +j4eval[1]=IKsign(((((2.0)*x2948))+(((-1.5)*x2955))+(((3.0)*x2950*x2952))+(((0.5)*x2946))+(((1.5)*x2949))+(((-1.0)*cj5*r22*x2951))+(((1.73205080756888)*r22*x2950))+(((1.5)*x2954)))); +j4eval[2]=((IKabs(((((-0.866025403784439)*x2952))+(((1.5)*r22))+(((0.866025403784439)*x2950)))))+(IKabs((((sj5*x2951))+(((1.73205080756888)*cj5*r21)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=1.5707963267949; +sj2=1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x2956=((1.73205080756888)*sj0); +IkReal x2957=((1.73205080756888)*cj0); +IkReal x2958=((((-1.0)*r11*sj5*x2957))+(((-1.0)*r02*sj0))+(((-1.0)*cj5*r00*x2956))+((cj5*r10*x2957))+((r01*sj5*x2956))+((cj0*r12))); +j4eval[0]=x2958; +j4eval[1]=IKsign(x2958); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2959=((1.73205080756888)*sj0); +IkReal x2960=(r01*sj5); +IkReal x2961=((0.866025403784439)*cj0); +IkReal x2962=(cj5*r00); +IkReal x2963=((0.866025403784439)*sj0); +IkReal x2964=(cj5*r10); +IkReal x2965=((1.73205080756888)*cj0); +IkReal x2966=(r11*sj5); +CheckValue x2967 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+(((-1.0)*x2963*x2964))+((x2960*x2961))+(((-1.0)*x2961*x2962))+(((1.5)*r12*sj0))+((x2963*x2966)))),IkReal(((((-1.0)*cj5*r11*x2959))+(((-1.0)*r10*sj5*x2959))+(((-1.0)*cj5*r01*x2965))+(((-1.0)*r00*sj5*x2965)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2967.valid){ +continue; +} +CheckValue x2968=IKPowWithIntegerCheck(IKsign(((((-1.0)*x2965*x2966))+(((-1.0)*x2959*x2962))+(((-1.0)*r02*sj0))+((x2959*x2960))+((x2964*x2965))+((cj0*r12)))),-1); +if(!x2968.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x2967.value)+(((1.5707963267949)*(x2968.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2969=IKcos(j4); +IkReal x2970=IKsin(j4); +IkReal x2971=((0.5)*r21); +IkReal x2972=(cj0*r10); +IkReal x2973=((0.5)*sj0); +IkReal x2974=(r10*sj0); +IkReal x2975=((0.5)*r20); +IkReal x2976=((1.0)*sj0); +IkReal x2977=(cj0*r01); +IkReal x2978=(r02*sj0); +IkReal x2979=(cj0*r02); +IkReal x2980=(cj0*r00); +IkReal x2981=(cj0*r11); +IkReal x2982=(sj5*x2970); +IkReal x2983=(cj5*x2970); +IkReal x2984=(sj5*x2969); +IkReal x2985=((0.866025403784439)*x2970); +IkReal x2986=((0.866025403784439)*x2969); +IkReal x2987=(cj5*x2969); +IkReal x2988=(r12*x2986); +evalcond[0]=(((r20*x2984))+(((-1.0)*r22*x2985))+(((-1.0)*x2971*x2982))+((x2975*x2983))+((r21*x2987))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x2983))+(((-1.0)*r20*x2982))+(((-1.0)*r22*x2986))+(((-1.0)*x2971*x2984))+((x2975*x2987))); +evalcond[2]=(((r11*sj0*x2983))+((x2974*x2982))+((x2977*x2983))+(((0.5)*x2977*x2984))+((x2980*x2982))+((r11*x2973*x2984))+(((-1.0)*r10*x2973*x2987))+((sj0*x2988))+((x2979*x2986))+(((-0.5)*x2980*x2987))); +evalcond[3]=((((0.5)*x2981*x2982))+(((-1.0)*r01*x2973*x2982))+(((-1.0)*x2978*x2985))+(((-1.0)*x2981*x2987))+((r00*sj0*x2984))+((r00*x2973*x2983))+(((-1.0)*x2972*x2984))+((cj0*r12*x2985))+((r01*sj0*x2987))+(((-0.5)*x2972*x2983))); +evalcond[4]=((-0.5)+(((0.5)*x2981*x2984))+((cj0*x2988))+(((-1.0)*r01*x2973*x2984))+(((-1.0)*x2978*x2986))+((x2981*x2983))+(((-1.0)*r00*x2976*x2982))+(((-1.0)*r01*x2976*x2983))+((r00*x2973*x2987))+((x2972*x2982))+(((-0.5)*x2972*x2987))); +evalcond[5]=((1.0)+(((-1.0)*x2974*x2984))+(((0.5)*x2977*x2982))+(((-1.0)*x2977*x2987))+((r11*x2973*x2982))+(((-1.0)*r10*x2973*x2983))+(((-1.0)*r11*x2976*x2987))+((x2979*x2985))+(((-1.0)*x2980*x2984))+(((-0.5)*x2980*x2983))+((r12*sj0*x2985))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x2989=r21*r21; +IkReal x2990=cj5*cj5; +IkReal x2991=r20*r20; +IkReal x2992=(r21*sj5); +IkReal x2993=((1.73205080756888)*cj5); +IkReal x2994=(cj5*r20); +IkReal x2995=((1.5)*x2990); +CheckValue x2996=IKPowWithIntegerCheck(IKsign(((((0.5)*x2989))+(((-1.0)*r20*r22*x2993))+(((3.0)*x2992*x2994))+(((1.73205080756888)*r22*x2992))+(((-1.0)*x2991*x2995))+((x2989*x2995))+(((2.0)*x2991))+(((1.5)*(r22*r22))))),-1); +if(!x2996.valid){ +continue; +} +CheckValue x2997 = IKatan2WithCheck(IkReal((((r21*x2993))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x2994))+(((1.5)*r22))+(((0.866025403784439)*x2992)))),IKFAST_ATAN2_MAGTHRESH); +if(!x2997.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x2996.value)))+(x2997.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x2998=IKcos(j4); +IkReal x2999=IKsin(j4); +IkReal x3000=((0.5)*r21); +IkReal x3001=(cj0*r10); +IkReal x3002=((0.5)*sj0); +IkReal x3003=(r10*sj0); +IkReal x3004=((0.5)*r20); +IkReal x3005=((1.0)*sj0); +IkReal x3006=(cj0*r01); +IkReal x3007=(r02*sj0); +IkReal x3008=(cj0*r02); +IkReal x3009=(cj0*r00); +IkReal x3010=(cj0*r11); +IkReal x3011=(sj5*x2999); +IkReal x3012=(cj5*x2999); +IkReal x3013=(sj5*x2998); +IkReal x3014=((0.866025403784439)*x2999); +IkReal x3015=((0.866025403784439)*x2998); +IkReal x3016=(cj5*x2998); +IkReal x3017=(r12*x3015); +evalcond[0]=((((-1.0)*x3000*x3011))+(((-1.0)*r22*x3014))+((x3004*x3012))+((r20*x3013))+((r21*x3016))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x3000*x3013))+(((-1.0)*r22*x3015))+(((-1.0)*r21*x3012))+(((-1.0)*r20*x3011))+((x3004*x3016))); +evalcond[2]=(((r11*sj0*x3012))+((x3009*x3011))+((x3008*x3015))+((r11*x3002*x3013))+((x3003*x3011))+((sj0*x3017))+(((0.5)*x3006*x3013))+(((-0.5)*x3009*x3016))+((x3006*x3012))+(((-1.0)*r10*x3002*x3016))); +evalcond[3]=((((-0.5)*x3001*x3012))+((cj0*r12*x3014))+(((-1.0)*x3001*x3013))+((r01*sj0*x3016))+((r00*x3002*x3012))+(((-1.0)*x3010*x3016))+(((-1.0)*x3007*x3014))+((r00*sj0*x3013))+(((-1.0)*r01*x3002*x3011))+(((0.5)*x3010*x3011))); +evalcond[4]=((-0.5)+((x3010*x3012))+(((-0.5)*x3001*x3016))+((cj0*x3017))+(((-1.0)*r01*x3005*x3012))+((x3001*x3011))+((r00*x3002*x3016))+(((-1.0)*x3007*x3015))+(((-1.0)*r00*x3005*x3011))+(((-1.0)*r01*x3002*x3013))+(((0.5)*x3010*x3013))); +evalcond[5]=((1.0)+(((-1.0)*x3009*x3013))+(((-1.0)*x3003*x3013))+((x3008*x3014))+((r11*x3002*x3011))+(((-1.0)*r11*x3005*x3016))+(((0.5)*x3006*x3011))+((r12*sj0*x3014))+(((-0.5)*x3009*x3012))+(((-1.0)*x3006*x3016))+(((-1.0)*r10*x3002*x3012))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3018=((0.866025403784439)*sj5); +IkReal x3019=((0.866025403784439)*cj5); +CheckValue x3020 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3020.valid){ +continue; +} +CheckValue x3021=IKPowWithIntegerCheck(IKsign((((cj0*r11*x3018))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+(((-1.0)*r01*sj0*x3018))+(((-1.0)*cj0*r10*x3019))+((r00*sj0*x3019)))),-1); +if(!x3021.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3020.value)+(((1.5707963267949)*(x3021.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3022=IKcos(j4); +IkReal x3023=IKsin(j4); +IkReal x3024=((0.5)*r21); +IkReal x3025=(cj0*r10); +IkReal x3026=((0.5)*sj0); +IkReal x3027=(r10*sj0); +IkReal x3028=((0.5)*r20); +IkReal x3029=((1.0)*sj0); +IkReal x3030=(cj0*r01); +IkReal x3031=(r02*sj0); +IkReal x3032=(cj0*r02); +IkReal x3033=(cj0*r00); +IkReal x3034=(cj0*r11); +IkReal x3035=(sj5*x3023); +IkReal x3036=(cj5*x3023); +IkReal x3037=(sj5*x3022); +IkReal x3038=((0.866025403784439)*x3023); +IkReal x3039=((0.866025403784439)*x3022); +IkReal x3040=(cj5*x3022); +IkReal x3041=(r12*x3039); +evalcond[0]=((((-1.0)*x3024*x3035))+((x3028*x3036))+((r21*x3040))+(((-1.0)*r22*x3038))+((r20*x3037))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x3024*x3037))+(((-1.0)*r20*x3035))+(((-1.0)*r21*x3036))+((x3028*x3040))+(((-1.0)*r22*x3039))); +evalcond[2]=(((x3030*x3036))+(((-1.0)*r10*x3026*x3040))+(((0.5)*x3030*x3037))+((r11*sj0*x3036))+((x3033*x3035))+(((-0.5)*x3033*x3040))+((x3032*x3039))+((sj0*x3041))+((r11*x3026*x3037))+((x3027*x3035))); +evalcond[3]=((((0.5)*x3034*x3035))+((r00*sj0*x3037))+(((-1.0)*x3025*x3037))+((r01*sj0*x3040))+((cj0*r12*x3038))+(((-0.5)*x3025*x3036))+(((-1.0)*x3031*x3038))+(((-1.0)*r01*x3026*x3035))+(((-1.0)*x3034*x3040))+((r00*x3026*x3036))); +evalcond[4]=((-0.5)+(((0.5)*x3034*x3037))+(((-0.5)*x3025*x3040))+((x3034*x3036))+(((-1.0)*r01*x3029*x3036))+((r00*x3026*x3040))+((x3025*x3035))+(((-1.0)*x3031*x3039))+(((-1.0)*r00*x3029*x3035))+(((-1.0)*r01*x3026*x3037))+((cj0*x3041))); +evalcond[5]=((1.0)+(((-1.0)*r11*x3029*x3040))+(((-1.0)*x3027*x3037))+(((-0.5)*x3033*x3036))+(((0.5)*x3030*x3035))+(((-1.0)*x3030*x3040))+((r12*sj0*x3038))+(((-1.0)*r10*x3026*x3036))+((x3032*x3038))+(((-1.0)*x3033*x3037))+((r11*x3026*x3035))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3042=cj5*cj5; +IkReal x3043=(r20*sj0); +IkReal x3044=(cj0*r01); +IkReal x3045=((0.866025403784439)*r22); +IkReal x3046=((0.5)*r21); +IkReal x3047=(cj0*r00); +IkReal x3048=((0.866025403784439)*r12); +IkReal x3049=(cj5*r20); +IkReal x3050=(r21*sj0); +IkReal x3051=((1.5)*sj5); +IkReal x3052=(cj3*sj5); +IkReal x3053=(cj0*r02); +IkReal x3054=(cj5*r21); +IkReal x3055=(cj5*r10); +IkReal x3056=((1.5)*r22); +IkReal x3057=(r11*sj0); +IkReal x3058=((1.5)*x3042); +CheckValue x3059 = IKatan2WithCheck(IkReal((((cj3*x3054))+((r20*x3052)))),IkReal(((((-0.5)*cj3*x3049))+((cj3*x3045))+((x3046*x3052)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3059.valid){ +continue; +} +CheckValue x3060=IKPowWithIntegerCheck(IKsign(((((-1.0)*sj5*x3044*x3045))+(((-2.0)*r20*x3047))+(((-1.0)*cj5*r11*x3043*x3051))+(((0.866025403784439)*x3049*x3053))+((cj5*x3045*x3047))+((sj0*x3045*x3055))+(((-0.866025403784439)*r21*sj5*x3053))+(((-1.0)*x3053*x3056))+(((-1.0)*sj5*x3045*x3057))+((r20*x3047*x3058))+(((-2.0)*r10*x3043))+(((-1.0)*r21*x3044*x3058))+(((-1.0)*x3050*x3051*x3055))+(((-1.0)*x3044*x3049*x3051))+(((-1.0)*x3046*x3057))+((r10*x3043*x3058))+(((-1.0)*x3047*x3051*x3054))+(((-1.0)*r11*x3050*x3058))+(((-1.0)*x3044*x3046))+(((-1.0)*r12*sj0*x3056))+(((-1.0)*sj5*x3048*x3050))+((cj5*x3043*x3048)))),-1); +if(!x3060.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3059.value)+(((1.5707963267949)*(x3060.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3061=IKcos(j4); +IkReal x3062=IKsin(j4); +IkReal x3063=((0.5)*cj5); +IkReal x3064=(r01*sj0); +IkReal x3065=((1.0)*sj5); +IkReal x3066=(r11*sj0); +IkReal x3067=(r02*sj0); +IkReal x3068=(r00*sj0); +IkReal x3069=(cj0*r02); +IkReal x3070=(cj0*r01); +IkReal x3071=(r10*sj0); +IkReal x3072=(cj0*r00); +IkReal x3073=(cj0*r11); +IkReal x3074=(cj0*r10); +IkReal x3075=(sj5*x3061); +IkReal x3076=(r20*x3062); +IkReal x3077=(cj5*x3062); +IkReal x3078=((0.866025403784439)*x3062); +IkReal x3079=((0.866025403784439)*x3061); +IkReal x3080=(cj5*x3061); +IkReal x3081=(x3062*x3074); +IkReal x3082=(x3062*x3071); +IkReal x3083=(r12*x3079); +IkReal x3084=((0.5)*sj5*x3062); +evalcond[0]=((((-1.0)*r22*x3078))+(((-1.0)*r21*x3084))+((r21*x3080))+((r20*x3075))+((x3063*x3076))); +evalcond[1]=((0.866025403784439)+((r20*x3061*x3063))+(((-1.0)*r22*x3079))+(((-1.0)*r21*x3077))+(((-1.0)*x3065*x3076))+(((-0.5)*r21*x3075))); +evalcond[2]=(((cj0*r12*x3078))+cj3+(((-1.0)*x3067*x3078))+((x3073*x3084))+(((-1.0)*x3064*x3084))+(((-1.0)*x3061*x3065*x3074))+(((-1.0)*x3063*x3081))+((x3068*x3075))+((x3064*x3080))+(((-1.0)*x3073*x3080))+((x3062*x3063*x3068))); +evalcond[3]=(((sj0*x3083))+(((0.5)*x3066*x3075))+((sj5*x3062*x3072))+(((0.5)*x3070*x3075))+(((0.5)*cj3))+((x3070*x3077))+((x3069*x3079))+(((-1.0)*x3061*x3063*x3072))+(((-1.0)*x3061*x3063*x3071))+((x3066*x3077))+((sj5*x3082))); +evalcond[4]=((((-1.0)*x3062*x3065*x3068))+(((-1.0)*x3067*x3079))+(((0.5)*x3073*x3075))+(((0.5)*sj3))+(((-1.0)*x3064*x3077))+(((-0.5)*x3064*x3075))+((x3061*x3063*x3068))+((cj0*x3083))+(((-1.0)*x3061*x3063*x3074))+((x3073*x3077))+((sj5*x3081))); +evalcond[5]=((((-1.0)*sj3))+((x3070*x3084))+(((-1.0)*x3070*x3080))+((x3066*x3084))+(((-1.0)*x3061*x3065*x3072))+(((-1.0)*x3061*x3065*x3071))+(((-1.0)*x3066*x3080))+(((-1.0)*x3063*x3082))+((x3069*x3078))+((r12*sj0*x3078))+(((-1.0)*x3062*x3063*x3072))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3085=r21*r21; +IkReal x3086=cj5*cj5; +IkReal x3087=r20*r20; +IkReal x3088=(r21*sj5); +IkReal x3089=((1.73205080756888)*cj5); +IkReal x3090=(cj5*r20); +IkReal x3091=((1.5)*x3086); +CheckValue x3092 = IKatan2WithCheck(IkReal((((r21*x3089))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x3088))+(((1.5)*r22))+(((-0.866025403784439)*x3090)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3092.valid){ +continue; +} +CheckValue x3093=IKPowWithIntegerCheck(IKsign(((((0.5)*x3085))+(((3.0)*x3088*x3090))+(((-1.0)*r20*r22*x3089))+((x3085*x3091))+(((1.5)*(r22*r22)))+(((1.73205080756888)*r22*x3088))+(((-1.0)*x3087*x3091))+(((2.0)*x3087)))),-1); +if(!x3093.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3092.value)+(((1.5707963267949)*(x3093.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3094=IKcos(j4); +IkReal x3095=IKsin(j4); +IkReal x3096=((0.5)*cj5); +IkReal x3097=(r01*sj0); +IkReal x3098=((1.0)*sj5); +IkReal x3099=(r11*sj0); +IkReal x3100=(r02*sj0); +IkReal x3101=(r00*sj0); +IkReal x3102=(cj0*r02); +IkReal x3103=(cj0*r01); +IkReal x3104=(r10*sj0); +IkReal x3105=(cj0*r00); +IkReal x3106=(cj0*r11); +IkReal x3107=(cj0*r10); +IkReal x3108=(sj5*x3094); +IkReal x3109=(r20*x3095); +IkReal x3110=(cj5*x3095); +IkReal x3111=((0.866025403784439)*x3095); +IkReal x3112=((0.866025403784439)*x3094); +IkReal x3113=(cj5*x3094); +IkReal x3114=(x3095*x3107); +IkReal x3115=(x3095*x3104); +IkReal x3116=(r12*x3112); +IkReal x3117=((0.5)*sj5*x3095); +evalcond[0]=((((-1.0)*r21*x3117))+((x3096*x3109))+((r21*x3113))+(((-1.0)*r22*x3111))+((r20*x3108))); +evalcond[1]=((0.866025403784439)+(((-0.5)*r21*x3108))+(((-1.0)*x3098*x3109))+(((-1.0)*r22*x3112))+(((-1.0)*r21*x3110))+((r20*x3094*x3096))); +evalcond[2]=(((x3097*x3113))+((cj0*r12*x3111))+cj3+(((-1.0)*x3106*x3113))+(((-1.0)*x3097*x3117))+((x3095*x3096*x3101))+((x3106*x3117))+(((-1.0)*x3094*x3098*x3107))+(((-1.0)*x3096*x3114))+((x3101*x3108))+(((-1.0)*x3100*x3111))); +evalcond[3]=(((sj0*x3116))+((x3099*x3110))+((x3102*x3112))+((x3103*x3110))+((sj5*x3115))+((sj5*x3095*x3105))+(((0.5)*cj3))+(((0.5)*x3103*x3108))+(((0.5)*x3099*x3108))+(((-1.0)*x3094*x3096*x3104))+(((-1.0)*x3094*x3096*x3105))); +evalcond[4]=(((x3094*x3096*x3101))+(((0.5)*sj3))+(((-1.0)*x3097*x3110))+((sj5*x3114))+(((0.5)*x3106*x3108))+(((-1.0)*x3095*x3098*x3101))+((cj0*x3116))+((x3106*x3110))+(((-1.0)*x3100*x3112))+(((-0.5)*x3097*x3108))+(((-1.0)*x3094*x3096*x3107))); +evalcond[5]=((((-1.0)*sj3))+(((-1.0)*x3099*x3113))+((x3099*x3117))+(((-1.0)*x3095*x3096*x3105))+((x3102*x3111))+((x3103*x3117))+((r12*sj0*x3111))+(((-1.0)*x3103*x3113))+(((-1.0)*x3094*x3098*x3105))+(((-1.0)*x3094*x3098*x3104))+(((-1.0)*x3096*x3115))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3118=((0.866025403784439)*cj0); +IkReal x3119=(cj3*r21); +IkReal x3120=(cj3*r20); +IkReal x3121=((0.866025403784439)*sj0); +CheckValue x3122=IKPowWithIntegerCheck(IKsign(((((0.5)*r12*sj0))+((cj5*r00*x3118))+(((0.5)*cj0*r02))+(((-1.0)*r11*sj5*x3121))+((cj5*r10*x3121))+(((-1.0)*r01*sj5*x3118)))),-1); +if(!x3122.valid){ +continue; +} +CheckValue x3123 = IKatan2WithCheck(IkReal((((sj5*x3120))+((cj5*x3119)))),IkReal(((((0.5)*sj5*x3119))+(((-0.5)*cj5*x3120))+(((0.866025403784439)*cj3*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3123.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3122.value)))+(x3123.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3124=IKcos(j4); +IkReal x3125=IKsin(j4); +IkReal x3126=((0.5)*cj5); +IkReal x3127=(r01*sj0); +IkReal x3128=((1.0)*sj5); +IkReal x3129=(r11*sj0); +IkReal x3130=(r02*sj0); +IkReal x3131=(r00*sj0); +IkReal x3132=(cj0*r02); +IkReal x3133=(cj0*r01); +IkReal x3134=(r10*sj0); +IkReal x3135=(cj0*r00); +IkReal x3136=(cj0*r11); +IkReal x3137=(cj0*r10); +IkReal x3138=(sj5*x3124); +IkReal x3139=(r20*x3125); +IkReal x3140=(cj5*x3125); +IkReal x3141=((0.866025403784439)*x3125); +IkReal x3142=((0.866025403784439)*x3124); +IkReal x3143=(cj5*x3124); +IkReal x3144=(x3125*x3137); +IkReal x3145=(x3125*x3134); +IkReal x3146=(r12*x3142); +IkReal x3147=((0.5)*sj5*x3125); +evalcond[0]=((((-1.0)*r22*x3141))+((r20*x3138))+((x3126*x3139))+(((-1.0)*r21*x3147))+((r21*x3143))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r22*x3142))+(((-1.0)*x3128*x3139))+(((-0.5)*r21*x3138))+((r20*x3124*x3126))+(((-1.0)*r21*x3140))); +evalcond[2]=(((x3131*x3138))+((x3127*x3143))+cj3+((x3125*x3126*x3131))+(((-1.0)*x3127*x3147))+(((-1.0)*x3126*x3144))+(((-1.0)*x3124*x3128*x3137))+((cj0*r12*x3141))+(((-1.0)*x3130*x3141))+(((-1.0)*x3136*x3143))+((x3136*x3147))); +evalcond[3]=(((sj5*x3145))+((sj0*x3146))+((x3129*x3140))+(((0.5)*x3133*x3138))+(((0.5)*x3129*x3138))+(((0.5)*cj3))+(((-1.0)*x3124*x3126*x3135))+(((-1.0)*x3124*x3126*x3134))+((x3133*x3140))+((x3132*x3142))+((sj5*x3125*x3135))); +evalcond[4]=(((sj5*x3144))+(((0.5)*x3136*x3138))+((cj0*x3146))+(((0.5)*sj3))+(((-1.0)*x3127*x3140))+(((-1.0)*x3124*x3126*x3137))+(((-1.0)*x3130*x3142))+((x3124*x3126*x3131))+(((-1.0)*x3125*x3128*x3131))+(((-0.5)*x3127*x3138))+((x3136*x3140))); +evalcond[5]=((((-1.0)*sj3))+(((-1.0)*x3129*x3143))+((x3129*x3147))+(((-1.0)*x3126*x3145))+(((-1.0)*x3124*x3128*x3135))+(((-1.0)*x3124*x3128*x3134))+((r12*sj0*x3141))+((x3133*x3147))+(((-1.0)*x3125*x3126*x3135))+((x3132*x3141))+(((-1.0)*x3133*x3143))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(j1, 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j2), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x3148=((1.0)*cj5); +IkReal x3149=(r01*sj5); +IkReal x3150=((0.577350269189626)*r02); +IkReal x3151=(r11*sj5); +IkReal x3152=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*sj0*x3150))+((sj0*x3149))+((cj0*cj5*r10))+((cj0*x3152))+(((-1.0)*cj0*x3151))+(((-1.0)*r00*sj0*x3148)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*r10*sj0*x3148))+(((-1.0)*sj0*x3152))+((cj0*x3149))+((sj0*x3151))+(((-1.0)*cj0*r00*x3148))+(((-1.0)*cj0*x3150)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*sj0*x3150))+((sj0*x3149))+((cj0*cj5*r10))+((cj0*x3152))+(((-1.0)*cj0*x3151))+(((-1.0)*r00*sj0*x3148))))+IKsqr(((((-1.0)*r10*sj0*x3148))+(((-1.0)*sj0*x3152))+((cj0*x3149))+((sj0*x3151))+(((-1.0)*cj0*r00*x3148))+(((-1.0)*cj0*x3150))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*sj0*x3150))+((sj0*x3149))+((cj0*cj5*r10))+((cj0*x3152))+(((-1.0)*cj0*x3151))+(((-1.0)*r00*sj0*x3148))), ((((-1.0)*r10*sj0*x3148))+(((-1.0)*sj0*x3152))+((cj0*x3149))+((sj0*x3151))+(((-1.0)*cj0*r00*x3148))+(((-1.0)*cj0*x3150)))); +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[2]; +IkReal x3153=((0.866025403784439)*sj5); +IkReal x3154=((0.5)*sj0); +IkReal x3155=((0.5)*cj0); +IkReal x3156=((0.866025403784439)*cj5*r10); +IkReal x3157=((0.866025403784439)*cj5*r00); +evalcond[0]=(((r02*x3154))+(((-1.0)*r01*sj0*x3153))+((sj0*x3157))+((cj0*r11*x3153))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*cj0*x3156))+(((-1.0)*r12*x3155))); +evalcond[1]=((((-1.0)*sj0*x3156))+(((-0.866025403784439)*(IKcos(j3))))+((r11*sj0*x3153))+(((-1.0)*cj0*x3157))+(((-1.0)*r02*x3155))+(((-1.0)*r12*x3154))+((cj0*r01*x3153))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x3158=(r12*sj0); +IkReal x3159=(cj3*sj5); +IkReal x3160=(cj0*r02); +IkReal x3161=(cj3*cj5); +IkReal x3162=(cj0*cj5*r00); +IkReal x3163=(r11*sj0*sj5); +IkReal x3164=(cj0*r01*sj5); +IkReal x3165=(cj5*r10*sj0); +j4eval[0]=(x3158+x3160+(((1.73205080756888)*x3162))+(((1.73205080756888)*x3165))+(((-1.73205080756888)*x3163))+(((-1.73205080756888)*x3164))); +j4eval[1]=((IKabs((((r20*x3159))+((r21*x3161)))))+(((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x3161))+((r21*x3159)))))))); +j4eval[2]=IKsign(((((0.5)*x3160))+(((0.5)*x3158))+(((0.866025403784439)*x3162))+(((0.866025403784439)*x3165))+(((-0.866025403784439)*x3163))+(((-0.866025403784439)*x3164)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x3166=r20*r20; +IkReal x3167=cj5*cj5; +IkReal x3168=r21*r21; +IkReal x3169=r22*r22; +IkReal x3170=(cj5*r20); +IkReal x3171=(r21*sj5); +IkReal x3172=((1.73205080756888)*r22); +IkReal x3173=((3.46410161513775)*r22); +IkReal x3174=(x3167*x3168); +IkReal x3175=(x3166*x3167); +j4eval[0]=((((-1.0)*x3168))+(((-3.0)*x3174))+(((3.0)*x3175))+((x3170*x3173))+(((-1.0)*x3171*x3173))+(((-6.0)*x3170*x3171))+(((-3.0)*x3169))+(((-4.0)*x3166))); +j4eval[1]=((IKabs(((((-0.866025403784439)*x3170))+(((1.5)*r22))+(((0.866025403784439)*x3171)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-2.0)*x3166))+(((-3.0)*x3170*x3171))+((x3170*x3172))+(((-0.5)*x3168))+(((1.5)*x3175))+(((-1.0)*x3171*x3172))+(((-1.5)*x3169))+(((-1.5)*x3174)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +IkReal x3176=cj5*cj5; +IkReal x3177=(cj0*r01); +IkReal x3178=((0.866025403784439)*r22); +IkReal x3179=(cj5*sj5); +IkReal x3180=(r11*sj0); +IkReal x3181=((0.5)*r21); +IkReal x3182=(cj0*r22); +IkReal x3183=(cj0*r00); +IkReal x3184=((2.0)*r20); +IkReal x3185=(cj5*r20); +IkReal x3186=((3.0)*r20); +IkReal x3187=((4.0)*r20); +IkReal x3188=((1.73205080756888)*cj5); +IkReal x3189=(r21*sj5); +IkReal x3190=((3.0)*sj0); +IkReal x3191=((1.5)*r20); +IkReal x3192=(r12*r22); +IkReal x3193=(cj0*r02); +IkReal x3194=(r10*sj0); +IkReal x3195=((3.0)*r21); +IkReal x3196=((1.5)*x3194); +IkReal x3197=(r20*x3176); +IkReal x3198=((0.866025403784439)*r12*sj0); +IkReal x3199=((1.73205080756888)*r22*sj5); +IkReal x3200=((1.73205080756888)*r12*sj0); +IkReal x3201=((1.5)*r21*x3176); +j4eval[0]=((((-1.0)*x3185*x3200))+((x3179*x3180*x3186))+((x3179*x3183*x3195))+(((1.73205080756888)*x3189*x3193))+(((-1.0)*x3176*x3186*x3194))+((r10*r21*x3179*x3190))+(((3.0)*r02*x3182))+((x3176*x3177*x3195))+((x3177*x3199))+((r21*x3177))+(((-1.73205080756888)*x3185*x3193))+(((-1.0)*r22*x3188*x3194))+((x3183*x3187))+(((-1.0)*x3176*x3183*x3186))+((x3187*x3194))+((x3190*x3192))+((x3180*x3199))+((x3176*x3180*x3195))+((x3177*x3179*x3186))+((r21*x3180))+((x3189*x3200))+(((-1.0)*r00*x3182*x3188))); +j4eval[1]=((IKabs((((cj3*r20*sj5))+((cj3*cj5*r21)))))+(((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*cj3*x3185))+((cj3*x3189)))))))); +j4eval[2]=IKsign(((((-1.0)*x3185*x3198))+((x3177*x3201))+((r21*x3179*x3196))+(((-1.0)*x3176*x3191*x3194))+(((0.866025403784439)*x3189*x3193))+(((-1.0)*cj5*x3178*x3194))+((sj5*x3177*x3178))+((x3184*x3194))+(((1.5)*r02*x3182))+(((-1.0)*cj5*x3178*x3183))+((x3177*x3181))+((x3180*x3181))+(((1.5)*r21*x3179*x3183))+((x3180*x3201))+((x3183*x3184))+(((1.5)*sj0*x3192))+((x3179*x3180*x3191))+(((-0.866025403784439)*x3185*x3193))+((sj5*x3178*x3180))+(((-1.0)*x3176*x3183*x3191))+((x3189*x3198))+((x3177*x3179*x3191)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3202=(cj0*r12); +IkReal x3203=((0.866025403784439)*sj5); +IkReal x3204=(r01*sj0); +IkReal x3205=(r02*sj0); +IkReal x3206=(cj0*r11); +IkReal x3207=((1.73205080756888)*sj5); +IkReal x3208=(cj0*cj5*r10); +IkReal x3209=(cj5*r00*sj0); +j4eval[0]=((((1.73205080756888)*x3209))+(((-1.0)*x3204*x3207))+(((-1.73205080756888)*x3208))+((x3206*x3207))+x3205+(((-1.0)*x3202))); +j4eval[1]=IKsign(((((0.866025403784439)*x3209))+(((-0.866025403784439)*x3208))+(((-1.0)*x3203*x3204))+(((-0.5)*x3202))+(((0.5)*x3205))+((x3203*x3206)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3210=r20*r20; +IkReal x3211=cj5*cj5; +IkReal x3212=r21*r21; +IkReal x3213=r22*r22; +IkReal x3214=(cj5*r20); +IkReal x3215=(r21*sj5); +IkReal x3216=((1.73205080756888)*r22); +IkReal x3217=((3.46410161513775)*r22); +IkReal x3218=(x3211*x3212); +IkReal x3219=(x3210*x3211); +j4eval[0]=((((-1.0)*x3215*x3217))+(((-3.0)*x3218))+(((-3.0)*x3213))+(((3.0)*x3219))+(((-1.0)*x3212))+((x3214*x3217))+(((-4.0)*x3210))+(((-6.0)*x3214*x3215))); +j4eval[1]=((IKabs(((((1.5)*r22))+(((0.866025403784439)*x3215))+(((-0.866025403784439)*x3214)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-1.0)*x3215*x3216))+(((1.5)*x3219))+((x3214*x3216))+(((-0.5)*x3212))+(((-3.0)*x3214*x3215))+(((-2.0)*x3210))+(((-1.5)*x3218))+(((-1.5)*x3213)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3220=((1.73205080756888)*sj0); +IkReal x3221=((1.73205080756888)*cj0); +IkReal x3222=((((-1.0)*r01*sj5*x3220))+((cj5*r00*x3220))+(((-1.0)*cj0*r12))+((r02*sj0))+((r11*sj5*x3221))+(((-1.0)*cj5*r10*x3221))); +j4eval[0]=x3222; +j4eval[1]=IKsign(x3222); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3223=(r01*sj5); +IkReal x3224=((0.866025403784439)*cj0); +IkReal x3225=((1.73205080756888)*sj0); +IkReal x3226=(cj5*r00); +IkReal x3227=((0.866025403784439)*sj0); +IkReal x3228=(cj5*r10); +IkReal x3229=((1.73205080756888)*cj0); +IkReal x3230=(r11*sj5); +CheckValue x3231 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+((x3223*x3224))+(((-1.0)*x3224*x3226))+(((-1.0)*x3227*x3228))+(((1.5)*r12*sj0))+((x3227*x3230)))),IkReal(((((-1.0)*r00*sj5*x3229))+(((-1.0)*cj5*r01*x3229))+(((-1.0)*r10*sj5*x3225))+(((-1.0)*cj5*r11*x3225)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3231.valid){ +continue; +} +CheckValue x3232=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*x3228*x3229))+((x3225*x3226))+((x3229*x3230))+(((-1.0)*x3223*x3225)))),-1); +if(!x3232.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3231.value)+(((1.5707963267949)*(x3232.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3233=IKcos(j4); +IkReal x3234=IKsin(j4); +IkReal x3235=((0.5)*r21); +IkReal x3236=(cj0*r10); +IkReal x3237=((0.5)*sj0); +IkReal x3238=(r10*sj0); +IkReal x3239=((0.5)*r20); +IkReal x3240=((1.0)*sj0); +IkReal x3241=(cj0*r01); +IkReal x3242=(r02*sj0); +IkReal x3243=(cj0*r02); +IkReal x3244=(cj0*r00); +IkReal x3245=(cj0*r11); +IkReal x3246=(sj5*x3234); +IkReal x3247=(cj5*x3234); +IkReal x3248=(sj5*x3233); +IkReal x3249=((0.866025403784439)*x3234); +IkReal x3250=((0.866025403784439)*x3233); +IkReal x3251=(cj5*x3233); +IkReal x3252=(r12*x3250); +evalcond[0]=((((-1.0)*x3235*x3246))+((r20*x3248))+((r21*x3251))+((x3239*x3247))+(((-1.0)*r22*x3249))); +evalcond[1]=((-0.866025403784439)+((x3239*x3251))+(((-1.0)*r20*x3246))+(((-1.0)*x3235*x3248))+(((-1.0)*r21*x3247))+(((-1.0)*r22*x3250))); +evalcond[2]=((((0.5)*x3241*x3248))+((r11*sj0*x3247))+((r11*x3237*x3248))+((sj0*x3252))+(((-0.5)*x3244*x3251))+((x3243*x3250))+((x3238*x3246))+((x3241*x3247))+(((-1.0)*r10*x3237*x3251))+((x3244*x3246))); +evalcond[3]=((((0.5)*x3245*x3246))+((r00*x3237*x3247))+(((-0.5)*x3236*x3247))+((cj0*r12*x3249))+(((-1.0)*r01*x3237*x3246))+((r00*sj0*x3248))+((r01*sj0*x3251))+(((-1.0)*x3236*x3248))+(((-1.0)*x3242*x3249))+(((-1.0)*x3245*x3251))); +evalcond[4]=((0.5)+((x3236*x3246))+(((0.5)*x3245*x3248))+(((-1.0)*r00*x3240*x3246))+((cj0*x3252))+(((-1.0)*x3242*x3250))+(((-1.0)*r01*x3237*x3248))+(((-1.0)*r01*x3240*x3247))+((r00*x3237*x3251))+(((-0.5)*x3236*x3251))+((x3245*x3247))); +evalcond[5]=((1.0)+(((0.5)*x3241*x3246))+(((-1.0)*r10*x3237*x3247))+((r12*sj0*x3249))+(((-1.0)*x3244*x3248))+((r11*x3237*x3246))+(((-1.0)*x3241*x3251))+(((-1.0)*r11*x3240*x3251))+(((-1.0)*x3238*x3248))+(((-0.5)*x3244*x3247))+((x3243*x3249))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3253=cj5*cj5; +IkReal x3254=r20*r20; +IkReal x3255=r21*r21; +IkReal x3256=(r21*sj5); +IkReal x3257=(cj5*r20); +IkReal x3258=((1.73205080756888)*r22); +IkReal x3259=((1.5)*x3253); +CheckValue x3260=IKPowWithIntegerCheck(IKsign(((((-0.5)*x3255))+(((-3.0)*x3256*x3257))+(((-1.5)*(r22*r22)))+(((-2.0)*x3254))+(((-1.0)*x3256*x3258))+((x3254*x3259))+((x3257*x3258))+(((-1.0)*x3255*x3259)))),-1); +if(!x3260.valid){ +continue; +} +CheckValue x3261 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x3256))+(((1.5)*r22))+(((-0.866025403784439)*x3257)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3261.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3260.value)))+(x3261.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3262=IKcos(j4); +IkReal x3263=IKsin(j4); +IkReal x3264=((0.5)*r21); +IkReal x3265=(cj0*r10); +IkReal x3266=((0.5)*sj0); +IkReal x3267=(r10*sj0); +IkReal x3268=((0.5)*r20); +IkReal x3269=((1.0)*sj0); +IkReal x3270=(cj0*r01); +IkReal x3271=(r02*sj0); +IkReal x3272=(cj0*r02); +IkReal x3273=(cj0*r00); +IkReal x3274=(cj0*r11); +IkReal x3275=(sj5*x3263); +IkReal x3276=(cj5*x3263); +IkReal x3277=(sj5*x3262); +IkReal x3278=((0.866025403784439)*x3263); +IkReal x3279=((0.866025403784439)*x3262); +IkReal x3280=(cj5*x3262); +IkReal x3281=(r12*x3279); +evalcond[0]=(((r20*x3277))+((r21*x3280))+(((-1.0)*x3264*x3275))+((x3268*x3276))+(((-1.0)*r22*x3278))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x3276))+((x3268*x3280))+(((-1.0)*x3264*x3277))+(((-1.0)*r22*x3279))+(((-1.0)*r20*x3275))); +evalcond[2]=(((r11*x3266*x3277))+((x3272*x3279))+((x3273*x3275))+((r11*sj0*x3276))+((x3267*x3275))+((sj0*x3281))+(((0.5)*x3270*x3277))+((x3270*x3276))+(((-0.5)*x3273*x3280))+(((-1.0)*r10*x3266*x3280))); +evalcond[3]=((((0.5)*x3274*x3275))+(((-1.0)*r01*x3266*x3275))+(((-0.5)*x3265*x3276))+(((-1.0)*x3274*x3280))+(((-1.0)*x3265*x3277))+((cj0*r12*x3278))+(((-1.0)*x3271*x3278))+((r01*sj0*x3280))+((r00*sj0*x3277))+((r00*x3266*x3276))); +evalcond[4]=((0.5)+((x3265*x3275))+(((0.5)*x3274*x3277))+(((-1.0)*r01*x3266*x3277))+(((-1.0)*r01*x3269*x3276))+((x3274*x3276))+(((-1.0)*r00*x3269*x3275))+((r00*x3266*x3280))+(((-0.5)*x3265*x3280))+((cj0*x3281))+(((-1.0)*x3271*x3279))); +evalcond[5]=((1.0)+(((-1.0)*x3267*x3277))+((r11*x3266*x3275))+(((-1.0)*x3270*x3280))+((x3272*x3278))+((r12*sj0*x3278))+(((-0.5)*x3273*x3276))+(((-1.0)*r11*x3269*x3280))+(((-1.0)*r10*x3266*x3276))+(((0.5)*x3270*x3275))+(((-1.0)*x3273*x3277))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3282=((0.866025403784439)*sj5); +IkReal x3283=((0.866025403784439)*cj5); +CheckValue x3284 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3284.valid){ +continue; +} +CheckValue x3285=IKPowWithIntegerCheck(IKsign(((((-1.0)*r01*sj0*x3282))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+((r00*sj0*x3283))+((cj0*r11*x3282))+(((-1.0)*cj0*r10*x3283)))),-1); +if(!x3285.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3284.value)+(((1.5707963267949)*(x3285.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3286=IKcos(j4); +IkReal x3287=IKsin(j4); +IkReal x3288=((0.5)*r21); +IkReal x3289=(cj0*r10); +IkReal x3290=((0.5)*sj0); +IkReal x3291=(r10*sj0); +IkReal x3292=((0.5)*r20); +IkReal x3293=((1.0)*sj0); +IkReal x3294=(cj0*r01); +IkReal x3295=(r02*sj0); +IkReal x3296=(cj0*r02); +IkReal x3297=(cj0*r00); +IkReal x3298=(cj0*r11); +IkReal x3299=(sj5*x3287); +IkReal x3300=(cj5*x3287); +IkReal x3301=(sj5*x3286); +IkReal x3302=((0.866025403784439)*x3287); +IkReal x3303=((0.866025403784439)*x3286); +IkReal x3304=(cj5*x3286); +IkReal x3305=(r12*x3303); +evalcond[0]=(((x3292*x3300))+((r21*x3304))+(((-1.0)*r22*x3302))+(((-1.0)*x3288*x3299))+((r20*x3301))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r20*x3299))+((x3292*x3304))+(((-1.0)*x3288*x3301))+(((-1.0)*r22*x3303))+(((-1.0)*r21*x3300))); +evalcond[2]=(((r11*x3290*x3301))+(((-1.0)*r10*x3290*x3304))+(((0.5)*x3294*x3301))+((r11*sj0*x3300))+((x3291*x3299))+((x3294*x3300))+((x3296*x3303))+(((-0.5)*x3297*x3304))+((x3297*x3299))+((sj0*x3305))); +evalcond[3]=((((-1.0)*x3298*x3304))+(((-1.0)*r01*x3290*x3299))+(((-0.5)*x3289*x3300))+(((-1.0)*x3289*x3301))+((r00*x3290*x3300))+(((0.5)*x3298*x3299))+((r00*sj0*x3301))+((cj0*r12*x3302))+(((-1.0)*x3295*x3302))+((r01*sj0*x3304))); +evalcond[4]=((0.5)+(((-1.0)*r01*x3293*x3300))+((x3289*x3299))+(((-0.5)*x3289*x3304))+(((0.5)*x3298*x3301))+(((-1.0)*r00*x3293*x3299))+((cj0*x3305))+((x3298*x3300))+((r00*x3290*x3304))+(((-1.0)*r01*x3290*x3301))+(((-1.0)*x3295*x3303))); +evalcond[5]=((1.0)+(((-1.0)*r10*x3290*x3300))+(((0.5)*x3294*x3299))+((r12*sj0*x3302))+(((-1.0)*x3297*x3301))+((x3296*x3302))+((r11*x3290*x3299))+(((-1.0)*r11*x3293*x3304))+(((-0.5)*x3297*x3300))+(((-1.0)*x3291*x3301))+(((-1.0)*x3294*x3304))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x3306=((0.866025403784439)*sj5); +IkReal x3307=(r01*sj0); +IkReal x3308=(r02*sj0); +IkReal x3309=(cj0*r11); +IkReal x3310=(cj0*r12); +IkReal x3311=((1.73205080756888)*sj5); +IkReal x3312=(cj0*cj5*r10); +IkReal x3313=(cj5*r00*sj0); +j4eval[0]=((((1.73205080756888)*x3312))+(((-1.73205080756888)*x3313))+x3310+(((-1.0)*x3309*x3311))+(((-1.0)*x3308))+((x3307*x3311))); +j4eval[1]=IKsign(((((-0.5)*x3308))+(((0.5)*x3310))+(((0.866025403784439)*x3312))+(((-1.0)*x3306*x3309))+((x3306*x3307))+(((-0.866025403784439)*x3313)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x3314=r20*r20; +IkReal x3315=cj5*cj5; +IkReal x3316=r21*r21; +IkReal x3317=r22*r22; +IkReal x3318=(cj5*r20); +IkReal x3319=(r21*sj5); +IkReal x3320=((1.73205080756888)*r22); +IkReal x3321=((3.46410161513775)*r22); +IkReal x3322=(x3315*x3316); +IkReal x3323=(x3314*x3315); +j4eval[0]=((((-3.0)*x3322))+(((-6.0)*x3318*x3319))+(((-1.0)*x3319*x3321))+(((-1.0)*x3316))+(((-3.0)*x3317))+(((3.0)*x3323))+(((-4.0)*x3314))+((x3318*x3321))); +j4eval[1]=((IKabs(((((1.5)*r22))+(((0.866025403784439)*x3319))+(((-0.866025403784439)*x3318)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-1.5)*x3317))+(((-3.0)*x3318*x3319))+(((-1.0)*x3319*x3320))+(((-2.0)*x3314))+(((1.5)*x3323))+(((-1.5)*x3322))+((x3318*x3320))+(((-0.5)*x3316)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=3.14159265358979; +sj1=0; +cj1=-1.0; +j2=-1.5707963267949; +sj2=-1.0; +cj2=0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x3324=((1.73205080756888)*sj0); +IkReal x3325=((1.73205080756888)*cj0); +IkReal x3326=((((-1.0)*cj0*r12))+((r02*sj0))+((r11*sj5*x3325))+(((-1.0)*r01*sj5*x3324))+((cj5*r00*x3324))+(((-1.0)*cj5*r10*x3325))); +j4eval[0]=x3326; +j4eval[1]=IKsign(x3326); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3327=(r01*sj5); +IkReal x3328=((0.866025403784439)*cj0); +IkReal x3329=((1.73205080756888)*sj0); +IkReal x3330=(cj5*r00); +IkReal x3331=((0.866025403784439)*sj0); +IkReal x3332=(cj5*r10); +IkReal x3333=((1.73205080756888)*cj0); +IkReal x3334=(r11*sj5); +CheckValue x3335=IKPowWithIntegerCheck(IKsign(((((-1.0)*x3327*x3329))+((x3333*x3334))+((x3329*x3330))+(((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*x3332*x3333)))),-1); +if(!x3335.valid){ +continue; +} +CheckValue x3336 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+(((-1.0)*x3331*x3332))+((x3331*x3334))+(((1.5)*r12*sj0))+((x3327*x3328))+(((-1.0)*x3328*x3330)))),IkReal(((((-1.0)*r10*sj5*x3329))+(((-1.0)*cj5*r01*x3333))+(((-1.0)*cj5*r11*x3329))+(((-1.0)*r00*sj5*x3333)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3336.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3335.value)))+(x3336.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3337=IKcos(j4); +IkReal x3338=IKsin(j4); +IkReal x3339=((0.5)*r21); +IkReal x3340=(cj0*r10); +IkReal x3341=((0.5)*sj0); +IkReal x3342=(r10*sj0); +IkReal x3343=((0.5)*r20); +IkReal x3344=((1.0)*sj0); +IkReal x3345=(cj0*r01); +IkReal x3346=(r02*sj0); +IkReal x3347=(cj0*r02); +IkReal x3348=(cj0*r00); +IkReal x3349=(cj0*r11); +IkReal x3350=(sj5*x3338); +IkReal x3351=(cj5*x3338); +IkReal x3352=(sj5*x3337); +IkReal x3353=((0.866025403784439)*x3338); +IkReal x3354=((0.866025403784439)*x3337); +IkReal x3355=(cj5*x3337); +IkReal x3356=(r12*x3354); +evalcond[0]=((((-1.0)*x3339*x3350))+((x3343*x3351))+((r20*x3352))+((r21*x3355))+(((-1.0)*r22*x3353))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*x3339*x3352))+((x3343*x3355))+(((-1.0)*r21*x3351))+(((-1.0)*r20*x3350))+(((-1.0)*r22*x3354))); +evalcond[2]=(((x3342*x3350))+((x3347*x3354))+((sj0*x3356))+(((0.5)*x3345*x3352))+((x3345*x3351))+(((-0.5)*x3348*x3355))+((r11*sj0*x3351))+((r11*x3341*x3352))+(((-1.0)*r10*x3341*x3355))+((x3348*x3350))); +evalcond[3]=((((-1.0)*x3340*x3352))+(((-1.0)*x3346*x3353))+(((-0.5)*x3340*x3351))+(((-1.0)*x3349*x3355))+(((-1.0)*r01*x3341*x3350))+((r00*x3341*x3351))+(((0.5)*x3349*x3350))+((r00*sj0*x3352))+((cj0*r12*x3353))+((r01*sj0*x3355))); +evalcond[4]=((-0.5)+((x3340*x3350))+(((-1.0)*x3346*x3354))+(((-0.5)*x3340*x3355))+(((-1.0)*r00*x3344*x3350))+(((-1.0)*r01*x3341*x3352))+((r00*x3341*x3355))+(((0.5)*x3349*x3352))+((cj0*x3356))+(((-1.0)*r01*x3344*x3351))+((x3349*x3351))); +evalcond[5]=((-1.0)+((x3347*x3353))+(((-1.0)*r11*x3344*x3355))+(((-1.0)*x3342*x3352))+((r12*sj0*x3353))+(((0.5)*x3345*x3350))+(((-1.0)*x3348*x3352))+(((-0.5)*x3348*x3351))+((r11*x3341*x3350))+(((-1.0)*r10*x3341*x3351))+(((-1.0)*x3345*x3355))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3357=cj5*cj5; +IkReal x3358=r20*r20; +IkReal x3359=r21*r21; +IkReal x3360=(r21*sj5); +IkReal x3361=(cj5*r20); +IkReal x3362=((1.73205080756888)*r22); +IkReal x3363=((1.5)*x3357); +CheckValue x3364 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((-0.866025403784439)*x3361))+(((0.866025403784439)*x3360)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3364.valid){ +continue; +} +CheckValue x3365=IKPowWithIntegerCheck(IKsign(((((-1.0)*x3360*x3362))+(((-1.0)*x3359*x3363))+(((-1.5)*(r22*r22)))+(((-0.5)*x3359))+((x3358*x3363))+((x3361*x3362))+(((-3.0)*x3360*x3361))+(((-2.0)*x3358)))),-1); +if(!x3365.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3364.value)+(((1.5707963267949)*(x3365.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3366=IKcos(j4); +IkReal x3367=IKsin(j4); +IkReal x3368=((0.5)*r21); +IkReal x3369=(cj0*r10); +IkReal x3370=((0.5)*sj0); +IkReal x3371=(r10*sj0); +IkReal x3372=((0.5)*r20); +IkReal x3373=((1.0)*sj0); +IkReal x3374=(cj0*r01); +IkReal x3375=(r02*sj0); +IkReal x3376=(cj0*r02); +IkReal x3377=(cj0*r00); +IkReal x3378=(cj0*r11); +IkReal x3379=(sj5*x3367); +IkReal x3380=(cj5*x3367); +IkReal x3381=(sj5*x3366); +IkReal x3382=((0.866025403784439)*x3367); +IkReal x3383=((0.866025403784439)*x3366); +IkReal x3384=(cj5*x3366); +IkReal x3385=(r12*x3383); +evalcond[0]=(((x3372*x3380))+(((-1.0)*r22*x3382))+((r20*x3381))+((r21*x3384))+(((-1.0)*x3368*x3379))); +evalcond[1]=((-0.866025403784439)+((x3372*x3384))+(((-1.0)*r22*x3383))+(((-1.0)*x3368*x3381))+(((-1.0)*r21*x3380))+(((-1.0)*r20*x3379))); +evalcond[2]=(((x3374*x3380))+((sj0*x3385))+((r11*x3370*x3381))+((x3377*x3379))+(((-1.0)*r10*x3370*x3384))+(((0.5)*x3374*x3381))+((x3371*x3379))+(((-0.5)*x3377*x3384))+((r11*sj0*x3380))+((x3376*x3383))); +evalcond[3]=(((r00*sj0*x3381))+((r01*sj0*x3384))+((cj0*r12*x3382))+((r00*x3370*x3380))+(((0.5)*x3378*x3379))+(((-1.0)*x3369*x3381))+(((-1.0)*x3375*x3382))+(((-0.5)*x3369*x3380))+(((-1.0)*x3378*x3384))+(((-1.0)*r01*x3370*x3379))); +evalcond[4]=((-0.5)+(((0.5)*x3378*x3381))+((r00*x3370*x3384))+((cj0*x3385))+(((-1.0)*r01*x3370*x3381))+(((-1.0)*x3375*x3383))+((x3369*x3379))+(((-0.5)*x3369*x3384))+(((-1.0)*r00*x3373*x3379))+(((-1.0)*r01*x3373*x3380))+((x3378*x3380))); +evalcond[5]=((-1.0)+(((0.5)*x3374*x3379))+(((-1.0)*r11*x3373*x3384))+(((-1.0)*r10*x3370*x3380))+((r12*sj0*x3382))+(((-1.0)*x3374*x3384))+(((-1.0)*x3371*x3381))+(((-1.0)*x3377*x3381))+((r11*x3370*x3379))+(((-0.5)*x3377*x3380))+((x3376*x3382))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3386=((0.866025403784439)*sj5); +IkReal x3387=((0.866025403784439)*cj5); +CheckValue x3388 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3388.valid){ +continue; +} +CheckValue x3389=IKPowWithIntegerCheck(IKsign((((r01*sj0*x3386))+(((-1.0)*cj0*r11*x3386))+(((-0.5)*r02*sj0))+((cj0*r10*x3387))+(((-1.0)*r00*sj0*x3387))+(((0.5)*cj0*r12)))),-1); +if(!x3389.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3388.value)+(((1.5707963267949)*(x3389.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3390=IKcos(j4); +IkReal x3391=IKsin(j4); +IkReal x3392=((0.5)*r21); +IkReal x3393=(cj0*r10); +IkReal x3394=((0.5)*sj0); +IkReal x3395=(r10*sj0); +IkReal x3396=((0.5)*r20); +IkReal x3397=((1.0)*sj0); +IkReal x3398=(cj0*r01); +IkReal x3399=(r02*sj0); +IkReal x3400=(cj0*r02); +IkReal x3401=(cj0*r00); +IkReal x3402=(cj0*r11); +IkReal x3403=(sj5*x3391); +IkReal x3404=(cj5*x3391); +IkReal x3405=(sj5*x3390); +IkReal x3406=((0.866025403784439)*x3391); +IkReal x3407=((0.866025403784439)*x3390); +IkReal x3408=(cj5*x3390); +IkReal x3409=(r12*x3407); +evalcond[0]=((((-1.0)*x3392*x3403))+((r21*x3408))+((r20*x3405))+((x3396*x3404))+(((-1.0)*r22*x3406))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x3404))+(((-1.0)*x3392*x3405))+((x3396*x3408))+(((-1.0)*r20*x3403))+(((-1.0)*r22*x3407))); +evalcond[2]=(((r11*sj0*x3404))+(((0.5)*x3398*x3405))+((sj0*x3409))+((r11*x3394*x3405))+((x3398*x3404))+((x3395*x3403))+((x3401*x3403))+((x3400*x3407))+(((-1.0)*r10*x3394*x3408))+(((-0.5)*x3401*x3408))); +evalcond[3]=((((-0.5)*x3393*x3404))+(((0.5)*x3402*x3403))+((r00*sj0*x3405))+(((-1.0)*x3402*x3408))+(((-1.0)*x3399*x3406))+((r01*sj0*x3408))+((cj0*r12*x3406))+((r00*x3394*x3404))+(((-1.0)*x3393*x3405))+(((-1.0)*r01*x3394*x3403))); +evalcond[4]=((-0.5)+(((-0.5)*x3393*x3408))+(((0.5)*x3402*x3405))+(((-1.0)*r01*x3397*x3404))+(((-1.0)*x3399*x3407))+(((-1.0)*r00*x3397*x3403))+((r00*x3394*x3408))+((cj0*x3409))+((x3402*x3404))+(((-1.0)*r01*x3394*x3405))+((x3393*x3403))); +evalcond[5]=((-1.0)+(((-1.0)*x3398*x3408))+(((-1.0)*x3395*x3405))+(((-1.0)*r11*x3397*x3408))+(((0.5)*x3398*x3403))+((r11*x3394*x3403))+(((-1.0)*x3401*x3405))+((x3400*x3406))+((r12*sj0*x3406))+(((-1.0)*r10*x3394*x3404))+(((-0.5)*x3401*x3404))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3410=cj5*cj5; +IkReal x3411=(r20*sj0); +IkReal x3412=(cj0*r01); +IkReal x3413=((0.866025403784439)*r22); +IkReal x3414=((0.5)*r21); +IkReal x3415=(cj0*r00); +IkReal x3416=((0.866025403784439)*r12); +IkReal x3417=(cj5*r20); +IkReal x3418=(r21*sj0); +IkReal x3419=((1.5)*sj5); +IkReal x3420=(cj3*sj5); +IkReal x3421=(cj0*r02); +IkReal x3422=(cj5*r21); +IkReal x3423=(cj5*r10); +IkReal x3424=((1.5)*r22); +IkReal x3425=(r11*sj0); +IkReal x3426=((1.5)*x3410); +CheckValue x3427=IKPowWithIntegerCheck(IKsign((((x3412*x3417*x3419))+((sj5*x3416*x3418))+(((-1.0)*cj5*x3413*x3415))+((sj5*x3413*x3425))+((sj5*x3412*x3413))+((cj5*r11*x3411*x3419))+((x3418*x3419*x3423))+((r21*x3412*x3426))+(((-0.866025403784439)*x3417*x3421))+(((2.0)*r10*x3411))+((x3421*x3424))+((x3414*x3425))+((x3415*x3419*x3422))+((x3412*x3414))+(((-1.0)*r20*x3415*x3426))+(((-1.0)*sj0*x3413*x3423))+(((0.866025403784439)*r21*sj5*x3421))+(((-1.0)*r10*x3411*x3426))+(((2.0)*r20*x3415))+((r11*x3418*x3426))+((r12*sj0*x3424))+(((-1.0)*cj5*x3411*x3416)))),-1); +if(!x3427.valid){ +continue; +} +CheckValue x3428 = IKatan2WithCheck(IkReal((((r20*x3420))+((cj3*x3422)))),IkReal((((cj3*x3413))+(((-0.5)*cj3*x3417))+((x3414*x3420)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3428.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3427.value)))+(x3428.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3429=IKcos(j4); +IkReal x3430=IKsin(j4); +IkReal x3431=((0.5)*cj5); +IkReal x3432=(r01*sj0); +IkReal x3433=((1.0)*sj5); +IkReal x3434=(r11*sj0); +IkReal x3435=(r02*sj0); +IkReal x3436=(r00*sj0); +IkReal x3437=(cj0*r02); +IkReal x3438=(cj0*r01); +IkReal x3439=(r10*sj0); +IkReal x3440=(cj0*r00); +IkReal x3441=(cj0*r11); +IkReal x3442=(cj0*r10); +IkReal x3443=(sj5*x3429); +IkReal x3444=(r20*x3430); +IkReal x3445=(cj5*x3430); +IkReal x3446=((0.866025403784439)*x3430); +IkReal x3447=((0.866025403784439)*x3429); +IkReal x3448=(cj5*x3429); +IkReal x3449=(x3430*x3442); +IkReal x3450=(x3430*x3439); +IkReal x3451=(r12*x3447); +IkReal x3452=((0.5)*sj5*x3430); +evalcond[0]=(((r21*x3448))+((x3431*x3444))+(((-1.0)*r21*x3452))+((r20*x3443))+(((-1.0)*r22*x3446))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*x3433*x3444))+(((-0.5)*r21*x3443))+(((-1.0)*r21*x3445))+((r20*x3429*x3431))+(((-1.0)*r22*x3447))); +evalcond[2]=((((-1.0)*x3432*x3452))+(((-1.0)*x3441*x3448))+cj3+((x3430*x3431*x3436))+(((-1.0)*x3435*x3446))+(((-1.0)*x3429*x3433*x3442))+((cj0*r12*x3446))+(((-1.0)*x3431*x3449))+((x3436*x3443))+((x3441*x3452))+((x3432*x3448))); +evalcond[3]=(((x3438*x3445))+((sj0*x3451))+(((-1.0)*x3429*x3431*x3440))+(((0.5)*x3434*x3443))+((x3434*x3445))+((x3437*x3447))+(((-1.0)*x3429*x3431*x3439))+(((-0.5)*cj3))+((sj5*x3430*x3440))+(((0.5)*x3438*x3443))+((sj5*x3450))); +evalcond[4]=(((x3429*x3431*x3436))+(((-1.0)*x3432*x3445))+(((-1.0)*x3429*x3431*x3442))+(((-1.0)*x3435*x3447))+(((0.5)*sj3))+(((0.5)*x3441*x3443))+(((-0.5)*x3432*x3443))+((sj5*x3449))+((cj0*x3451))+((x3441*x3445))+(((-1.0)*x3430*x3433*x3436))); +evalcond[5]=(sj3+((x3434*x3452))+((r12*sj0*x3446))+(((-1.0)*x3429*x3433*x3439))+(((-1.0)*x3430*x3431*x3440))+(((-1.0)*x3429*x3433*x3440))+((x3437*x3446))+(((-1.0)*x3431*x3450))+(((-1.0)*x3438*x3448))+((x3438*x3452))+(((-1.0)*x3434*x3448))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3453=cj5*cj5; +IkReal x3454=r20*r20; +IkReal x3455=r21*r21; +IkReal x3456=(r21*sj5); +IkReal x3457=(cj5*r20); +IkReal x3458=((1.73205080756888)*r22); +IkReal x3459=((1.5)*x3453); +CheckValue x3460 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x3456))+(((1.5)*r22))+(((-0.866025403784439)*x3457)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3460.valid){ +continue; +} +CheckValue x3461=IKPowWithIntegerCheck(IKsign((((x3457*x3458))+(((-0.5)*x3455))+(((-1.5)*(r22*r22)))+(((-1.0)*x3455*x3459))+((x3454*x3459))+(((-2.0)*x3454))+(((-3.0)*x3456*x3457))+(((-1.0)*x3456*x3458)))),-1); +if(!x3461.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3460.value)+(((1.5707963267949)*(x3461.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3462=IKcos(j4); +IkReal x3463=IKsin(j4); +IkReal x3464=((0.5)*cj5); +IkReal x3465=(r01*sj0); +IkReal x3466=((1.0)*sj5); +IkReal x3467=(r11*sj0); +IkReal x3468=(r02*sj0); +IkReal x3469=(r00*sj0); +IkReal x3470=(cj0*r02); +IkReal x3471=(cj0*r01); +IkReal x3472=(r10*sj0); +IkReal x3473=(cj0*r00); +IkReal x3474=(cj0*r11); +IkReal x3475=(cj0*r10); +IkReal x3476=(sj5*x3462); +IkReal x3477=(r20*x3463); +IkReal x3478=(cj5*x3463); +IkReal x3479=((0.866025403784439)*x3463); +IkReal x3480=((0.866025403784439)*x3462); +IkReal x3481=(cj5*x3462); +IkReal x3482=(x3463*x3475); +IkReal x3483=(x3463*x3472); +IkReal x3484=(r12*x3480); +IkReal x3485=((0.5)*sj5*x3463); +evalcond[0]=(((x3464*x3477))+(((-1.0)*r21*x3485))+((r20*x3476))+(((-1.0)*r22*x3479))+((r21*x3481))); +evalcond[1]=((-0.866025403784439)+((r20*x3462*x3464))+(((-0.5)*r21*x3476))+(((-1.0)*r22*x3480))+(((-1.0)*r21*x3478))+(((-1.0)*x3466*x3477))); +evalcond[2]=((((-1.0)*x3465*x3485))+((x3474*x3485))+cj3+(((-1.0)*x3462*x3466*x3475))+((cj0*r12*x3479))+((x3463*x3464*x3469))+((x3469*x3476))+(((-1.0)*x3474*x3481))+((x3465*x3481))+(((-1.0)*x3468*x3479))+(((-1.0)*x3464*x3482))); +evalcond[3]=(((x3470*x3480))+((x3471*x3478))+((x3467*x3478))+((sj5*x3483))+((sj0*x3484))+((sj5*x3463*x3473))+(((-1.0)*x3462*x3464*x3472))+(((-1.0)*x3462*x3464*x3473))+(((-0.5)*cj3))+(((0.5)*x3467*x3476))+(((0.5)*x3471*x3476))); +evalcond[4]=(((x3474*x3478))+(((-1.0)*x3463*x3466*x3469))+(((0.5)*sj3))+((sj5*x3482))+((cj0*x3484))+(((-1.0)*x3465*x3478))+(((-0.5)*x3465*x3476))+(((-1.0)*x3462*x3464*x3475))+(((0.5)*x3474*x3476))+(((-1.0)*x3468*x3480))+((x3462*x3464*x3469))); +evalcond[5]=(((r12*sj0*x3479))+((x3470*x3479))+(((-1.0)*x3467*x3481))+sj3+(((-1.0)*x3471*x3481))+(((-1.0)*x3462*x3466*x3472))+(((-1.0)*x3462*x3466*x3473))+((x3471*x3485))+((x3467*x3485))+(((-1.0)*x3463*x3464*x3473))+(((-1.0)*x3464*x3483))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3486=((0.866025403784439)*cj0); +IkReal x3487=(cj3*r21); +IkReal x3488=(cj3*r20); +IkReal x3489=((0.866025403784439)*sj0); +CheckValue x3490=IKPowWithIntegerCheck(IKsign(((((0.5)*r12*sj0))+(((-1.0)*r11*sj5*x3489))+((cj5*r10*x3489))+(((-1.0)*r01*sj5*x3486))+((cj5*r00*x3486))+(((0.5)*cj0*r02)))),-1); +if(!x3490.valid){ +continue; +} +CheckValue x3491 = IKatan2WithCheck(IkReal((((cj5*x3487))+((sj5*x3488)))),IkReal(((((0.5)*sj5*x3487))+(((0.866025403784439)*cj3*r22))+(((-0.5)*cj5*x3488)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3491.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3490.value)))+(x3491.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3492=IKcos(j4); +IkReal x3493=IKsin(j4); +IkReal x3494=((0.5)*cj5); +IkReal x3495=(r01*sj0); +IkReal x3496=((1.0)*sj5); +IkReal x3497=(r11*sj0); +IkReal x3498=(r02*sj0); +IkReal x3499=(r00*sj0); +IkReal x3500=(cj0*r02); +IkReal x3501=(cj0*r01); +IkReal x3502=(r10*sj0); +IkReal x3503=(cj0*r00); +IkReal x3504=(cj0*r11); +IkReal x3505=(cj0*r10); +IkReal x3506=(sj5*x3492); +IkReal x3507=(r20*x3493); +IkReal x3508=(cj5*x3493); +IkReal x3509=((0.866025403784439)*x3493); +IkReal x3510=((0.866025403784439)*x3492); +IkReal x3511=(cj5*x3492); +IkReal x3512=(x3493*x3505); +IkReal x3513=(x3493*x3502); +IkReal x3514=(r12*x3510); +IkReal x3515=((0.5)*sj5*x3493); +evalcond[0]=(((r21*x3511))+((r20*x3506))+(((-1.0)*r22*x3509))+((x3494*x3507))+(((-1.0)*r21*x3515))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x3508))+(((-1.0)*r22*x3510))+(((-1.0)*x3496*x3507))+((r20*x3492*x3494))+(((-0.5)*r21*x3506))); +evalcond[2]=((((-1.0)*x3492*x3496*x3505))+cj3+(((-1.0)*x3495*x3515))+((cj0*r12*x3509))+(((-1.0)*x3498*x3509))+((x3499*x3506))+(((-1.0)*x3494*x3512))+((x3504*x3515))+((x3495*x3511))+(((-1.0)*x3504*x3511))+((x3493*x3494*x3499))); +evalcond[3]=(((sj5*x3493*x3503))+((sj5*x3513))+((x3501*x3508))+(((-1.0)*x3492*x3494*x3503))+(((-1.0)*x3492*x3494*x3502))+(((0.5)*x3501*x3506))+((x3497*x3508))+((sj0*x3514))+(((-0.5)*cj3))+(((0.5)*x3497*x3506))+((x3500*x3510))); +evalcond[4]=(((x3492*x3494*x3499))+((sj5*x3512))+(((-0.5)*x3495*x3506))+(((-1.0)*x3493*x3496*x3499))+((cj0*x3514))+(((0.5)*sj3))+(((-1.0)*x3492*x3494*x3505))+(((-1.0)*x3498*x3510))+(((0.5)*x3504*x3506))+(((-1.0)*x3495*x3508))+((x3504*x3508))); +evalcond[5]=(((x3501*x3515))+(((-1.0)*x3497*x3511))+sj3+(((-1.0)*x3501*x3511))+((x3500*x3509))+(((-1.0)*x3492*x3496*x3503))+(((-1.0)*x3492*x3496*x3502))+((x3497*x3515))+(((-1.0)*x3494*x3513))+((r12*sj0*x3509))+(((-1.0)*x3493*x3494*x3503))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j2), 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x3516=((1.0)*cj5); +IkReal x3517=(r01*sj5); +IkReal x3518=((0.577350269189626)*r02); +IkReal x3519=(r11*sj5); +IkReal x3520=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*r00*sj0*x3516))+((cj0*x3520))+((cj0*cj5*r10))+(((-1.0)*sj0*x3518))+(((-1.0)*cj0*x3519))+((sj0*x3517)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj0*r00*x3516))+(((-1.0)*sj0*x3520))+((cj0*x3517))+(((-1.0)*cj0*x3518))+((sj0*x3519))+(((-1.0)*r10*sj0*x3516)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*r00*sj0*x3516))+((cj0*x3520))+((cj0*cj5*r10))+(((-1.0)*sj0*x3518))+(((-1.0)*cj0*x3519))+((sj0*x3517))))+IKsqr(((((-1.0)*cj0*r00*x3516))+(((-1.0)*sj0*x3520))+((cj0*x3517))+(((-1.0)*cj0*x3518))+((sj0*x3519))+(((-1.0)*r10*sj0*x3516))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*r00*sj0*x3516))+((cj0*x3520))+((cj0*cj5*r10))+(((-1.0)*sj0*x3518))+(((-1.0)*cj0*x3519))+((sj0*x3517))), ((((-1.0)*cj0*r00*x3516))+(((-1.0)*sj0*x3520))+((cj0*x3517))+(((-1.0)*cj0*x3518))+((sj0*x3519))+(((-1.0)*r10*sj0*x3516)))); +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[2]; +IkReal x3521=((0.866025403784439)*sj5); +IkReal x3522=((0.5)*sj0); +IkReal x3523=((0.5)*cj0); +IkReal x3524=((0.866025403784439)*cj5*r10); +IkReal x3525=((0.866025403784439)*cj5*r00); +evalcond[0]=(((r02*x3522))+((cj0*r11*x3521))+(((-1.0)*cj0*x3524))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r01*sj0*x3521))+((sj0*x3525))+(((-1.0)*r12*x3523))); +evalcond[1]=((((-1.0)*r02*x3523))+(((-0.866025403784439)*(IKcos(j3))))+(((-1.0)*sj0*x3524))+(((-1.0)*cj0*x3525))+((r11*sj0*x3521))+((cj0*r01*x3521))+(((-1.0)*r12*x3522))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x3526=(r12*sj0); +IkReal x3527=(cj3*sj5); +IkReal x3528=(cj0*r02); +IkReal x3529=(cj3*cj5); +IkReal x3530=(cj0*cj5*r00); +IkReal x3531=(r11*sj0*sj5); +IkReal x3532=(cj0*r01*sj5); +IkReal x3533=(cj5*r10*sj0); +j4eval[0]=(x3526+x3528+(((-1.73205080756888)*x3532))+(((-1.73205080756888)*x3531))+(((1.73205080756888)*x3533))+(((1.73205080756888)*x3530))); +j4eval[1]=((IKabs((((r21*x3529))+((r20*x3527)))))+(((0.5)*(IKabs(((((-1.0)*r20*x3529))+((r21*x3527))+(((1.73205080756888)*cj3*r22)))))))); +j4eval[2]=IKsign(((((0.5)*x3528))+(((0.5)*x3526))+(((-0.866025403784439)*x3532))+(((-0.866025403784439)*x3531))+(((0.866025403784439)*x3533))+(((0.866025403784439)*x3530)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x3534=r20*r20; +IkReal x3535=cj5*cj5; +IkReal x3536=r21*r21; +IkReal x3537=r22*r22; +IkReal x3538=(cj5*r20); +IkReal x3539=(r21*sj5); +IkReal x3540=((1.73205080756888)*r22); +IkReal x3541=((3.46410161513775)*r22); +IkReal x3542=(x3535*x3536); +IkReal x3543=(x3534*x3535); +j4eval[0]=((((-1.0)*x3539*x3541))+(((-4.0)*x3534))+(((-3.0)*x3542))+((x3538*x3541))+(((-6.0)*x3538*x3539))+(((-1.0)*x3536))+(((-3.0)*x3537))+(((3.0)*x3543))); +j4eval[1]=((IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))+(IKabs(((((1.5)*r22))+(((-0.866025403784439)*x3538))+(((0.866025403784439)*x3539)))))); +j4eval[2]=IKsign(((((-3.0)*x3538*x3539))+(((-1.0)*x3539*x3540))+(((-1.5)*x3542))+(((-2.0)*x3534))+((x3538*x3540))+(((1.5)*x3543))+(((-1.5)*x3537))+(((-0.5)*x3536)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +IkReal x3544=cj5*cj5; +IkReal x3545=(cj0*r01); +IkReal x3546=((0.866025403784439)*r22); +IkReal x3547=(cj5*sj5); +IkReal x3548=(r11*sj0); +IkReal x3549=((0.5)*r21); +IkReal x3550=(cj0*r22); +IkReal x3551=(cj0*r00); +IkReal x3552=((2.0)*r20); +IkReal x3553=(cj5*r20); +IkReal x3554=((3.0)*r20); +IkReal x3555=((4.0)*r20); +IkReal x3556=((1.73205080756888)*cj5); +IkReal x3557=(r21*sj5); +IkReal x3558=((3.0)*sj0); +IkReal x3559=((1.5)*r20); +IkReal x3560=(r12*r22); +IkReal x3561=(cj0*r02); +IkReal x3562=(r10*sj0); +IkReal x3563=((3.0)*r21); +IkReal x3564=((1.5)*x3562); +IkReal x3565=(r20*x3544); +IkReal x3566=((0.866025403784439)*r12*sj0); +IkReal x3567=((1.73205080756888)*r22*sj5); +IkReal x3568=((1.73205080756888)*r12*sj0); +IkReal x3569=((1.5)*r21*x3544); +j4eval[0]=((((3.0)*r02*x3550))+((x3547*x3551*x3563))+((r10*r21*x3547*x3558))+(((-1.0)*r22*x3556*x3562))+(((-1.0)*x3544*x3554*x3562))+((x3544*x3545*x3563))+((r21*x3545))+((r21*x3548))+((x3548*x3567))+((x3544*x3548*x3563))+(((-1.0)*r00*x3550*x3556))+(((-1.0)*x3544*x3551*x3554))+(((-1.73205080756888)*x3553*x3561))+((x3547*x3548*x3554))+((x3545*x3567))+(((1.73205080756888)*x3557*x3561))+((x3545*x3547*x3554))+((x3551*x3555))+(((-1.0)*x3553*x3568))+((x3555*x3562))+((x3557*x3568))+((x3558*x3560))); +j4eval[1]=((((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*cj3*x3553))+((cj3*x3557)))))))+(IKabs((((cj3*r20*sj5))+((cj3*cj5*r21)))))); +j4eval[2]=IKsign(((((-0.866025403784439)*x3553*x3561))+((sj5*x3545*x3546))+(((1.5)*r02*x3550))+(((-1.0)*cj5*x3546*x3551))+((x3548*x3569))+((x3548*x3549))+(((-1.0)*x3544*x3551*x3559))+(((1.5)*r21*x3547*x3551))+((x3547*x3548*x3559))+(((-1.0)*cj5*x3546*x3562))+((x3545*x3569))+((x3552*x3562))+((r21*x3547*x3564))+((x3545*x3547*x3559))+((sj5*x3546*x3548))+((x3551*x3552))+(((-1.0)*x3553*x3566))+((x3545*x3549))+(((-1.0)*x3544*x3559*x3562))+(((0.866025403784439)*x3557*x3561))+(((1.5)*sj0*x3560))+((x3557*x3566)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3570=(cj0*r12); +IkReal x3571=((0.866025403784439)*sj5); +IkReal x3572=(r01*sj0); +IkReal x3573=(r02*sj0); +IkReal x3574=(cj0*r11); +IkReal x3575=((1.73205080756888)*sj5); +IkReal x3576=(cj0*cj5*r10); +IkReal x3577=(cj5*r00*sj0); +j4eval[0]=(x3573+(((-1.73205080756888)*x3576))+(((-1.0)*x3572*x3575))+(((-1.0)*x3570))+((x3574*x3575))+(((1.73205080756888)*x3577))); +j4eval[1]=IKsign(((((-0.866025403784439)*x3576))+(((0.5)*x3573))+(((-0.5)*x3570))+((x3571*x3574))+(((0.866025403784439)*x3577))+(((-1.0)*x3571*x3572)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3578=r20*r20; +IkReal x3579=cj5*cj5; +IkReal x3580=r21*r21; +IkReal x3581=r22*r22; +IkReal x3582=(cj5*r20); +IkReal x3583=(r21*sj5); +IkReal x3584=((1.73205080756888)*r22); +IkReal x3585=((3.46410161513775)*r22); +IkReal x3586=(x3579*x3580); +IkReal x3587=(x3578*x3579); +j4eval[0]=((((3.0)*x3587))+(((-4.0)*x3578))+(((-6.0)*x3582*x3583))+(((-1.0)*x3583*x3585))+((x3582*x3585))+(((-1.0)*x3580))+(((-3.0)*x3581))+(((-3.0)*x3586))); +j4eval[1]=((IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))+(IKabs(((((-0.866025403784439)*x3582))+(((1.5)*r22))+(((0.866025403784439)*x3583)))))); +j4eval[2]=IKsign(((((-0.5)*x3580))+(((1.5)*x3587))+(((-1.5)*x3581))+(((-1.5)*x3586))+(((-3.0)*x3582*x3583))+(((-1.0)*x3583*x3584))+(((-2.0)*x3578))+((x3582*x3584)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3588=((1.73205080756888)*sj0); +IkReal x3589=((1.73205080756888)*cj0); +IkReal x3590=(((cj5*r00*x3588))+(((-1.0)*cj0*r12))+((r02*sj0))+((r11*sj5*x3589))+(((-1.0)*cj5*r10*x3589))+(((-1.0)*r01*sj5*x3588))); +j4eval[0]=x3590; +j4eval[1]=IKsign(x3590); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3591=(r01*sj5); +IkReal x3592=((0.866025403784439)*cj0); +IkReal x3593=((1.73205080756888)*sj0); +IkReal x3594=(cj5*r00); +IkReal x3595=((0.866025403784439)*sj0); +IkReal x3596=(cj5*r10); +IkReal x3597=((1.73205080756888)*cj0); +IkReal x3598=(r11*sj5); +CheckValue x3599=IKPowWithIntegerCheck(IKsign((((x3593*x3594))+(((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*x3591*x3593))+((x3597*x3598))+(((-1.0)*x3596*x3597)))),-1); +if(!x3599.valid){ +continue; +} +CheckValue x3600 = IKatan2WithCheck(IkReal(((((1.5)*cj0*r02))+((x3591*x3592))+((x3595*x3598))+(((-1.0)*x3595*x3596))+(((1.5)*r12*sj0))+(((-1.0)*x3592*x3594)))),IkReal(((((-1.0)*cj5*r11*x3593))+(((-1.0)*r00*sj5*x3597))+(((-1.0)*cj5*r01*x3597))+(((-1.0)*r10*sj5*x3593)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3600.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3599.value)))+(x3600.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3601=IKcos(j4); +IkReal x3602=IKsin(j4); +IkReal x3603=((0.5)*r21); +IkReal x3604=(cj0*r10); +IkReal x3605=((0.5)*sj0); +IkReal x3606=(r10*sj0); +IkReal x3607=((0.5)*r20); +IkReal x3608=((1.0)*sj0); +IkReal x3609=(cj0*r01); +IkReal x3610=(r02*sj0); +IkReal x3611=(cj0*r02); +IkReal x3612=(cj0*r00); +IkReal x3613=(cj0*r11); +IkReal x3614=(sj5*x3602); +IkReal x3615=(cj5*x3602); +IkReal x3616=(sj5*x3601); +IkReal x3617=((0.866025403784439)*x3602); +IkReal x3618=((0.866025403784439)*x3601); +IkReal x3619=(cj5*x3601); +IkReal x3620=(r12*x3618); +evalcond[0]=(((x3607*x3615))+(((-1.0)*x3603*x3614))+((r21*x3619))+(((-1.0)*r22*x3617))+((r20*x3616))); +evalcond[1]=((-0.866025403784439)+((x3607*x3619))+(((-1.0)*x3603*x3616))+(((-1.0)*r20*x3614))+(((-1.0)*r22*x3618))+(((-1.0)*r21*x3615))); +evalcond[2]=(((x3612*x3614))+((x3609*x3615))+((sj0*x3620))+(((-1.0)*r10*x3605*x3619))+((x3611*x3618))+((r11*x3605*x3616))+((r11*sj0*x3615))+(((-0.5)*x3612*x3619))+(((0.5)*x3609*x3616))+((x3606*x3614))); +evalcond[3]=((((-0.5)*x3604*x3615))+((cj0*r12*x3617))+(((-1.0)*x3613*x3619))+((r01*sj0*x3619))+(((-1.0)*x3604*x3616))+((r00*x3605*x3615))+(((-1.0)*r01*x3605*x3614))+(((0.5)*x3613*x3614))+(((-1.0)*x3610*x3617))+((r00*sj0*x3616))); +evalcond[4]=((0.5)+(((-0.5)*x3604*x3619))+((x3613*x3615))+((r00*x3605*x3619))+((cj0*x3620))+(((-1.0)*r01*x3605*x3616))+(((0.5)*x3613*x3616))+(((-1.0)*x3610*x3618))+(((-1.0)*r00*x3608*x3614))+(((-1.0)*r01*x3608*x3615))+((x3604*x3614))); +evalcond[5]=((1.0)+(((-1.0)*x3612*x3616))+(((-1.0)*r10*x3605*x3615))+(((-1.0)*r11*x3608*x3619))+((r12*sj0*x3617))+((x3611*x3617))+((r11*x3605*x3614))+(((-0.5)*x3612*x3615))+(((0.5)*x3609*x3614))+(((-1.0)*x3609*x3619))+(((-1.0)*x3606*x3616))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3621=cj5*cj5; +IkReal x3622=r20*r20; +IkReal x3623=r21*r21; +IkReal x3624=(r21*sj5); +IkReal x3625=(cj5*r20); +IkReal x3626=((1.73205080756888)*r22); +IkReal x3627=((1.5)*x3621); +CheckValue x3628 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x3625))+(((1.5)*r22))+(((0.866025403784439)*x3624)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3628.valid){ +continue; +} +CheckValue x3629=IKPowWithIntegerCheck(IKsign(((((-0.5)*x3623))+(((-1.0)*x3624*x3626))+(((-1.5)*(r22*r22)))+(((-1.0)*x3623*x3627))+(((-3.0)*x3624*x3625))+((x3625*x3626))+(((-2.0)*x3622))+((x3622*x3627)))),-1); +if(!x3629.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3628.value)+(((1.5707963267949)*(x3629.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3630=IKcos(j4); +IkReal x3631=IKsin(j4); +IkReal x3632=((0.5)*r21); +IkReal x3633=(cj0*r10); +IkReal x3634=((0.5)*sj0); +IkReal x3635=(r10*sj0); +IkReal x3636=((0.5)*r20); +IkReal x3637=((1.0)*sj0); +IkReal x3638=(cj0*r01); +IkReal x3639=(r02*sj0); +IkReal x3640=(cj0*r02); +IkReal x3641=(cj0*r00); +IkReal x3642=(cj0*r11); +IkReal x3643=(sj5*x3631); +IkReal x3644=(cj5*x3631); +IkReal x3645=(sj5*x3630); +IkReal x3646=((0.866025403784439)*x3631); +IkReal x3647=((0.866025403784439)*x3630); +IkReal x3648=(cj5*x3630); +IkReal x3649=(r12*x3647); +evalcond[0]=(((r20*x3645))+((x3636*x3644))+((r21*x3648))+(((-1.0)*r22*x3646))+(((-1.0)*x3632*x3643))); +evalcond[1]=((-0.866025403784439)+((x3636*x3648))+(((-1.0)*r21*x3644))+(((-1.0)*r22*x3647))+(((-1.0)*x3632*x3645))+(((-1.0)*r20*x3643))); +evalcond[2]=(((x3641*x3643))+(((-1.0)*r10*x3634*x3648))+((x3635*x3643))+(((0.5)*x3638*x3645))+((r11*sj0*x3644))+((x3638*x3644))+((sj0*x3649))+(((-0.5)*x3641*x3648))+((x3640*x3647))+((r11*x3634*x3645))); +evalcond[3]=(((r00*sj0*x3645))+((r01*sj0*x3648))+(((-1.0)*x3639*x3646))+(((0.5)*x3642*x3643))+(((-1.0)*x3633*x3645))+(((-1.0)*r01*x3634*x3643))+(((-0.5)*x3633*x3644))+((cj0*r12*x3646))+((r00*x3634*x3644))+(((-1.0)*x3642*x3648))); +evalcond[4]=((0.5)+((x3633*x3643))+(((-1.0)*x3639*x3647))+(((-1.0)*r00*x3637*x3643))+(((0.5)*x3642*x3645))+((x3642*x3644))+(((-1.0)*r01*x3634*x3645))+(((-0.5)*x3633*x3648))+((cj0*x3649))+(((-1.0)*r01*x3637*x3644))+((r00*x3634*x3648))); +evalcond[5]=((1.0)+(((-1.0)*x3638*x3648))+(((-1.0)*r10*x3634*x3644))+(((-1.0)*x3635*x3645))+(((0.5)*x3638*x3643))+(((-1.0)*x3641*x3645))+(((-0.5)*x3641*x3644))+((x3640*x3646))+((r11*x3634*x3643))+((r12*sj0*x3646))+(((-1.0)*r11*x3637*x3648))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3650=((0.866025403784439)*sj5); +IkReal x3651=((0.866025403784439)*cj5); +CheckValue x3652 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3652.valid){ +continue; +} +CheckValue x3653=IKPowWithIntegerCheck(IKsign(((((-1.0)*r01*sj0*x3650))+(((-0.5)*cj0*r12))+(((0.5)*r02*sj0))+((cj0*r11*x3650))+(((-1.0)*cj0*r10*x3651))+((r00*sj0*x3651)))),-1); +if(!x3653.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3652.value)+(((1.5707963267949)*(x3653.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3654=IKcos(j4); +IkReal x3655=IKsin(j4); +IkReal x3656=((0.5)*r21); +IkReal x3657=(cj0*r10); +IkReal x3658=((0.5)*sj0); +IkReal x3659=(r10*sj0); +IkReal x3660=((0.5)*r20); +IkReal x3661=((1.0)*sj0); +IkReal x3662=(cj0*r01); +IkReal x3663=(r02*sj0); +IkReal x3664=(cj0*r02); +IkReal x3665=(cj0*r00); +IkReal x3666=(cj0*r11); +IkReal x3667=(sj5*x3655); +IkReal x3668=(cj5*x3655); +IkReal x3669=(sj5*x3654); +IkReal x3670=((0.866025403784439)*x3655); +IkReal x3671=((0.866025403784439)*x3654); +IkReal x3672=(cj5*x3654); +IkReal x3673=(r12*x3671); +evalcond[0]=(((r20*x3669))+((r21*x3672))+(((-1.0)*r22*x3670))+(((-1.0)*x3656*x3667))+((x3660*x3668))); +evalcond[1]=((-0.866025403784439)+((x3660*x3672))+(((-1.0)*r21*x3668))+(((-1.0)*r22*x3671))+(((-1.0)*x3656*x3669))+(((-1.0)*r20*x3667))); +evalcond[2]=(((x3659*x3667))+(((-1.0)*r10*x3658*x3672))+((x3662*x3668))+((x3664*x3671))+(((-0.5)*x3665*x3672))+(((0.5)*x3662*x3669))+((r11*sj0*x3668))+((r11*x3658*x3669))+((sj0*x3673))+((x3665*x3667))); +evalcond[3]=(((r01*sj0*x3672))+(((-1.0)*r01*x3658*x3667))+(((-1.0)*x3663*x3670))+((r00*sj0*x3669))+((r00*x3658*x3668))+(((-1.0)*x3657*x3669))+(((-0.5)*x3657*x3668))+(((0.5)*x3666*x3667))+(((-1.0)*x3666*x3672))+((cj0*r12*x3670))); +evalcond[4]=((0.5)+(((-1.0)*r00*x3661*x3667))+(((-1.0)*r01*x3658*x3669))+((r00*x3658*x3672))+((x3657*x3667))+(((-1.0)*x3663*x3671))+(((-1.0)*r01*x3661*x3668))+(((-0.5)*x3657*x3672))+(((0.5)*x3666*x3669))+((cj0*x3673))+((x3666*x3668))); +evalcond[5]=((1.0)+((r12*sj0*x3670))+(((-1.0)*x3662*x3672))+(((-1.0)*r10*x3658*x3668))+((x3664*x3670))+(((-1.0)*r11*x3661*x3672))+(((-1.0)*x3665*x3669))+(((0.5)*x3662*x3667))+(((-0.5)*x3665*x3668))+(((-1.0)*x3659*x3669))+((r11*x3658*x3667))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x3674=((0.866025403784439)*sj5); +IkReal x3675=(r01*sj0); +IkReal x3676=(r02*sj0); +IkReal x3677=(cj0*r11); +IkReal x3678=(cj0*r12); +IkReal x3679=((1.73205080756888)*sj5); +IkReal x3680=(cj0*cj5*r10); +IkReal x3681=(cj5*r00*sj0); +j4eval[0]=((((-1.73205080756888)*x3681))+(((1.73205080756888)*x3680))+((x3675*x3679))+x3678+(((-1.0)*x3676))+(((-1.0)*x3677*x3679))); +j4eval[1]=IKsign(((((0.866025403784439)*x3680))+(((-0.5)*x3676))+(((-0.866025403784439)*x3681))+((x3674*x3675))+(((0.5)*x3678))+(((-1.0)*x3674*x3677)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x3682=r20*r20; +IkReal x3683=cj5*cj5; +IkReal x3684=r21*r21; +IkReal x3685=r22*r22; +IkReal x3686=(cj5*r20); +IkReal x3687=(r21*sj5); +IkReal x3688=((1.73205080756888)*r22); +IkReal x3689=((3.46410161513775)*r22); +IkReal x3690=(x3683*x3684); +IkReal x3691=(x3682*x3683); +j4eval[0]=((((-3.0)*x3690))+(((-4.0)*x3682))+((x3686*x3689))+(((-6.0)*x3686*x3687))+(((-1.0)*x3687*x3689))+(((-3.0)*x3685))+(((-1.0)*x3684))+(((3.0)*x3691))); +j4eval[1]=((IKabs(((((0.866025403784439)*x3687))+(((1.5)*r22))+(((-0.866025403784439)*x3686)))))+(IKabs(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))))); +j4eval[2]=IKsign(((((-3.0)*x3686*x3687))+(((-1.5)*x3690))+(((-1.5)*x3685))+((x3686*x3688))+(((-2.0)*x3682))+(((-1.0)*x3687*x3688))+(((1.5)*x3691))+(((-0.5)*x3684)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=0; +sj2=0; +cj2=1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x3692=((1.73205080756888)*sj0); +IkReal x3693=((1.73205080756888)*cj0); +IkReal x3694=((((-1.0)*cj5*r10*x3693))+(((-1.0)*r01*sj5*x3692))+(((-1.0)*cj0*r12))+((r02*sj0))+((cj5*r00*x3692))+((r11*sj5*x3693))); +j4eval[0]=x3694; +j4eval[1]=IKsign(x3694); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3695=(r01*sj5); +IkReal x3696=((0.866025403784439)*cj0); +IkReal x3697=((1.73205080756888)*sj0); +IkReal x3698=(cj5*r00); +IkReal x3699=((0.866025403784439)*sj0); +IkReal x3700=(cj5*r10); +IkReal x3701=((1.73205080756888)*cj0); +IkReal x3702=(r11*sj5); +CheckValue x3703=IKPowWithIntegerCheck(IKsign((((x3697*x3698))+(((-1.0)*x3700*x3701))+(((-1.0)*cj0*r12))+((r02*sj0))+(((-1.0)*x3695*x3697))+((x3701*x3702)))),-1); +if(!x3703.valid){ +continue; +} +CheckValue x3704 = IKatan2WithCheck(IkReal(((((-1.0)*x3699*x3700))+(((1.5)*cj0*r02))+((x3699*x3702))+((x3695*x3696))+(((-1.0)*x3696*x3698))+(((1.5)*r12*sj0)))),IkReal(((((-1.0)*cj5*r11*x3697))+(((-1.0)*r00*sj5*x3701))+(((-1.0)*cj5*r01*x3701))+(((-1.0)*r10*sj5*x3697)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3704.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3703.value)))+(x3704.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3705=IKcos(j4); +IkReal x3706=IKsin(j4); +IkReal x3707=((0.5)*r21); +IkReal x3708=(cj0*r10); +IkReal x3709=((0.5)*sj0); +IkReal x3710=(r10*sj0); +IkReal x3711=((0.5)*r20); +IkReal x3712=((1.0)*sj0); +IkReal x3713=(cj0*r01); +IkReal x3714=(r02*sj0); +IkReal x3715=(cj0*r02); +IkReal x3716=(cj0*r00); +IkReal x3717=(cj0*r11); +IkReal x3718=(sj5*x3706); +IkReal x3719=(cj5*x3706); +IkReal x3720=(sj5*x3705); +IkReal x3721=((0.866025403784439)*x3706); +IkReal x3722=((0.866025403784439)*x3705); +IkReal x3723=(cj5*x3705); +IkReal x3724=(r12*x3722); +evalcond[0]=((((-1.0)*r22*x3721))+((x3711*x3719))+((r20*x3720))+(((-1.0)*x3707*x3718))+((r21*x3723))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r22*x3722))+(((-1.0)*x3707*x3720))+(((-1.0)*r21*x3719))+((x3711*x3723))+(((-1.0)*r20*x3718))); +evalcond[2]=(((x3710*x3718))+((r11*sj0*x3719))+(((0.5)*x3713*x3720))+(((-1.0)*r10*x3709*x3723))+((sj0*x3724))+((r11*x3709*x3720))+(((-0.5)*x3716*x3723))+((x3715*x3722))+((x3716*x3718))+((x3713*x3719))); +evalcond[3]=(((cj0*r12*x3721))+(((-1.0)*x3717*x3723))+(((-1.0)*r01*x3709*x3718))+(((-1.0)*x3708*x3720))+(((-0.5)*x3708*x3719))+((r00*sj0*x3720))+(((0.5)*x3717*x3718))+((r01*sj0*x3723))+(((-1.0)*x3714*x3721))+((r00*x3709*x3719))); +evalcond[4]=((-0.5)+(((-1.0)*r01*x3709*x3720))+((x3717*x3719))+((x3708*x3718))+(((0.5)*x3717*x3720))+((cj0*x3724))+(((-1.0)*r01*x3712*x3719))+(((-0.5)*x3708*x3723))+((r00*x3709*x3723))+(((-1.0)*r00*x3712*x3718))+(((-1.0)*x3714*x3722))); +evalcond[5]=((-1.0)+(((-1.0)*x3710*x3720))+(((-1.0)*x3713*x3723))+(((-1.0)*r11*x3712*x3723))+(((-1.0)*r10*x3709*x3719))+(((-0.5)*x3716*x3719))+((x3715*x3721))+((r12*sj0*x3721))+((r11*x3709*x3718))+(((-1.0)*x3716*x3720))+(((0.5)*x3713*x3718))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3725=cj5*cj5; +IkReal x3726=r20*r20; +IkReal x3727=r21*r21; +IkReal x3728=(r21*sj5); +IkReal x3729=(cj5*r20); +IkReal x3730=((1.73205080756888)*r22); +IkReal x3731=((1.5)*x3725); +CheckValue x3732=IKPowWithIntegerCheck(IKsign(((((-1.5)*(r22*r22)))+((x3729*x3730))+(((-1.0)*x3727*x3731))+((x3726*x3731))+(((-3.0)*x3728*x3729))+(((-0.5)*x3727))+(((-1.0)*x3728*x3730))+(((-2.0)*x3726)))),-1); +if(!x3732.valid){ +continue; +} +CheckValue x3733 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((-0.866025403784439)*x3729))+(((0.866025403784439)*x3728)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3733.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3732.value)))+(x3733.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3734=IKcos(j4); +IkReal x3735=IKsin(j4); +IkReal x3736=((0.5)*r21); +IkReal x3737=(cj0*r10); +IkReal x3738=((0.5)*sj0); +IkReal x3739=(r10*sj0); +IkReal x3740=((0.5)*r20); +IkReal x3741=((1.0)*sj0); +IkReal x3742=(cj0*r01); +IkReal x3743=(r02*sj0); +IkReal x3744=(cj0*r02); +IkReal x3745=(cj0*r00); +IkReal x3746=(cj0*r11); +IkReal x3747=(sj5*x3735); +IkReal x3748=(cj5*x3735); +IkReal x3749=(sj5*x3734); +IkReal x3750=((0.866025403784439)*x3735); +IkReal x3751=((0.866025403784439)*x3734); +IkReal x3752=(cj5*x3734); +IkReal x3753=(r12*x3751); +evalcond[0]=((((-1.0)*r22*x3750))+((r20*x3749))+(((-1.0)*x3736*x3747))+((x3740*x3748))+((r21*x3752))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r20*x3747))+(((-1.0)*r22*x3751))+(((-1.0)*r21*x3748))+(((-1.0)*x3736*x3749))+((x3740*x3752))); +evalcond[2]=(((x3742*x3748))+((x3744*x3751))+((sj0*x3753))+((r11*x3738*x3749))+((r11*sj0*x3748))+(((0.5)*x3742*x3749))+(((-1.0)*r10*x3738*x3752))+(((-0.5)*x3745*x3752))+((x3739*x3747))+((x3745*x3747))); +evalcond[3]=((((-1.0)*x3746*x3752))+(((-0.5)*x3737*x3748))+(((-1.0)*x3743*x3750))+((r00*sj0*x3749))+((r01*sj0*x3752))+((r00*x3738*x3748))+(((0.5)*x3746*x3747))+(((-1.0)*x3737*x3749))+((cj0*r12*x3750))+(((-1.0)*r01*x3738*x3747))); +evalcond[4]=((-0.5)+((x3737*x3747))+(((-0.5)*x3737*x3752))+(((-1.0)*x3743*x3751))+((cj0*x3753))+(((-1.0)*r00*x3741*x3747))+(((-1.0)*r01*x3741*x3748))+((r00*x3738*x3752))+((x3746*x3748))+(((0.5)*x3746*x3749))+(((-1.0)*r01*x3738*x3749))); +evalcond[5]=((-1.0)+((x3744*x3750))+(((-1.0)*r11*x3741*x3752))+((r11*x3738*x3747))+((r12*sj0*x3750))+(((0.5)*x3742*x3747))+(((-1.0)*r10*x3738*x3748))+(((-1.0)*x3745*x3749))+(((-0.5)*x3745*x3748))+(((-1.0)*x3742*x3752))+(((-1.0)*x3739*x3749))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3754=((0.866025403784439)*sj5); +IkReal x3755=((0.866025403784439)*cj5); +CheckValue x3756 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3756.valid){ +continue; +} +CheckValue x3757=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj0*r11*x3754))+(((-0.5)*r02*sj0))+(((-1.0)*r00*sj0*x3755))+((r01*sj0*x3754))+((cj0*r10*x3755))+(((0.5)*cj0*r12)))),-1); +if(!x3757.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3756.value)+(((1.5707963267949)*(x3757.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3758=IKcos(j4); +IkReal x3759=IKsin(j4); +IkReal x3760=((0.5)*r21); +IkReal x3761=(cj0*r10); +IkReal x3762=((0.5)*sj0); +IkReal x3763=(r10*sj0); +IkReal x3764=((0.5)*r20); +IkReal x3765=((1.0)*sj0); +IkReal x3766=(cj0*r01); +IkReal x3767=(r02*sj0); +IkReal x3768=(cj0*r02); +IkReal x3769=(cj0*r00); +IkReal x3770=(cj0*r11); +IkReal x3771=(sj5*x3759); +IkReal x3772=(cj5*x3759); +IkReal x3773=(sj5*x3758); +IkReal x3774=((0.866025403784439)*x3759); +IkReal x3775=((0.866025403784439)*x3758); +IkReal x3776=(cj5*x3758); +IkReal x3777=(r12*x3775); +evalcond[0]=(((r21*x3776))+(((-1.0)*r22*x3774))+(((-1.0)*x3760*x3771))+((r20*x3773))+((x3764*x3772))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r21*x3772))+(((-1.0)*r22*x3775))+(((-1.0)*r20*x3771))+(((-1.0)*x3760*x3773))+((x3764*x3776))); +evalcond[2]=((((-1.0)*r10*x3762*x3776))+((x3769*x3771))+((r11*x3762*x3773))+(((-0.5)*x3769*x3776))+((x3768*x3775))+((r11*sj0*x3772))+((sj0*x3777))+(((0.5)*x3766*x3773))+((x3766*x3772))+((x3763*x3771))); +evalcond[3]=(((r00*sj0*x3773))+(((-1.0)*x3767*x3774))+(((-1.0)*r01*x3762*x3771))+((r00*x3762*x3772))+(((-1.0)*x3770*x3776))+((r01*sj0*x3776))+(((0.5)*x3770*x3771))+((cj0*r12*x3774))+(((-1.0)*x3761*x3773))+(((-0.5)*x3761*x3772))); +evalcond[4]=((-0.5)+(((-1.0)*r01*x3765*x3772))+((cj0*x3777))+(((-1.0)*r00*x3765*x3771))+(((-1.0)*x3767*x3775))+(((-1.0)*r01*x3762*x3773))+((r00*x3762*x3776))+(((0.5)*x3770*x3773))+((x3770*x3772))+((x3761*x3771))+(((-0.5)*x3761*x3776))); +evalcond[5]=((-1.0)+(((-1.0)*x3769*x3773))+(((-1.0)*r10*x3762*x3772))+((r12*sj0*x3774))+(((-1.0)*x3763*x3773))+((r11*x3762*x3771))+(((-1.0)*x3766*x3776))+(((-0.5)*x3769*x3772))+((x3768*x3774))+(((0.5)*x3766*x3771))+(((-1.0)*r11*x3765*x3776))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3778=cj5*cj5; +IkReal x3779=(r20*sj0); +IkReal x3780=(cj0*r01); +IkReal x3781=((0.866025403784439)*r22); +IkReal x3782=((0.5)*r21); +IkReal x3783=(cj0*r00); +IkReal x3784=((0.866025403784439)*r12); +IkReal x3785=(cj5*r20); +IkReal x3786=(r21*sj0); +IkReal x3787=((1.5)*sj5); +IkReal x3788=(cj3*sj5); +IkReal x3789=(cj0*r02); +IkReal x3790=(cj5*r21); +IkReal x3791=(cj5*r10); +IkReal x3792=((1.5)*r22); +IkReal x3793=(r11*sj0); +IkReal x3794=((1.5)*x3778); +CheckValue x3795=IKPowWithIntegerCheck(IKsign((((x3783*x3787*x3790))+((sj5*x3781*x3793))+((r21*x3780*x3794))+((sj5*x3780*x3781))+((x3789*x3792))+(((-1.0)*sj0*x3781*x3791))+(((2.0)*r20*x3783))+((x3782*x3793))+(((0.866025403784439)*r21*sj5*x3789))+((r11*x3786*x3794))+((cj5*r11*x3779*x3787))+(((-1.0)*cj5*x3779*x3784))+(((-0.866025403784439)*x3785*x3789))+((x3780*x3785*x3787))+((x3786*x3787*x3791))+((x3780*x3782))+(((-1.0)*cj5*x3781*x3783))+((r12*sj0*x3792))+(((-1.0)*r20*x3783*x3794))+(((-1.0)*r10*x3779*x3794))+(((2.0)*r10*x3779))+((sj5*x3784*x3786)))),-1); +if(!x3795.valid){ +continue; +} +CheckValue x3796 = IKatan2WithCheck(IkReal((((r20*x3788))+((cj3*x3790)))),IkReal(((((-0.5)*cj3*x3785))+((x3782*x3788))+((cj3*x3781)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3796.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3795.value)))+(x3796.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3797=IKcos(j4); +IkReal x3798=IKsin(j4); +IkReal x3799=((0.5)*cj5); +IkReal x3800=(r01*sj0); +IkReal x3801=((1.0)*sj5); +IkReal x3802=(r11*sj0); +IkReal x3803=(r02*sj0); +IkReal x3804=(r00*sj0); +IkReal x3805=(cj0*r02); +IkReal x3806=(cj0*r01); +IkReal x3807=(r10*sj0); +IkReal x3808=(cj0*r00); +IkReal x3809=(cj0*r11); +IkReal x3810=(cj0*r10); +IkReal x3811=(sj5*x3797); +IkReal x3812=(r20*x3798); +IkReal x3813=(cj5*x3798); +IkReal x3814=((0.866025403784439)*x3798); +IkReal x3815=((0.866025403784439)*x3797); +IkReal x3816=(cj5*x3797); +IkReal x3817=(x3798*x3810); +IkReal x3818=(x3798*x3807); +IkReal x3819=(r12*x3815); +IkReal x3820=((0.5)*sj5*x3798); +evalcond[0]=((((-1.0)*r22*x3814))+((r21*x3816))+(((-1.0)*r21*x3820))+((r20*x3811))+((x3799*x3812))); +evalcond[1]=((-0.866025403784439)+(((-1.0)*r22*x3815))+(((-1.0)*x3801*x3812))+(((-0.5)*r21*x3811))+(((-1.0)*r21*x3813))+((r20*x3797*x3799))); +evalcond[2]=((((-1.0)*x3800*x3820))+cj3+(((-1.0)*x3799*x3817))+((x3800*x3816))+((x3804*x3811))+(((-1.0)*x3803*x3814))+((x3809*x3820))+((cj0*r12*x3814))+((x3798*x3799*x3804))+(((-1.0)*x3797*x3801*x3810))+(((-1.0)*x3809*x3816))); +evalcond[3]=((((0.5)*x3802*x3811))+((x3805*x3815))+((x3806*x3813))+((sj5*x3818))+(((0.5)*x3806*x3811))+(((-1.0)*x3797*x3799*x3808))+(((-1.0)*x3797*x3799*x3807))+((sj0*x3819))+((sj5*x3798*x3808))+(((-0.5)*cj3))+((x3802*x3813))); +evalcond[4]=((((0.5)*x3809*x3811))+((x3797*x3799*x3804))+(((0.5)*sj3))+((sj5*x3817))+(((-0.5)*x3800*x3811))+(((-1.0)*x3797*x3799*x3810))+((x3809*x3813))+(((-1.0)*x3803*x3815))+((cj0*x3819))+(((-1.0)*x3800*x3813))+(((-1.0)*x3798*x3801*x3804))); +evalcond[5]=((((-1.0)*x3802*x3816))+sj3+((x3805*x3814))+(((-1.0)*x3798*x3799*x3808))+((x3806*x3820))+(((-1.0)*x3799*x3818))+(((-1.0)*x3797*x3801*x3808))+(((-1.0)*x3797*x3801*x3807))+(((-1.0)*x3806*x3816))+((x3802*x3820))+((r12*sj0*x3814))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3821=cj5*cj5; +IkReal x3822=r20*r20; +IkReal x3823=r21*r21; +IkReal x3824=(r21*sj5); +IkReal x3825=(cj5*r20); +IkReal x3826=((1.73205080756888)*r22); +IkReal x3827=((1.5)*x3821); +CheckValue x3828 = IKatan2WithCheck(IkReal(((((1.73205080756888)*cj5*r21))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x3825))+(((1.5)*r22))+(((0.866025403784439)*x3824)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3828.valid){ +continue; +} +CheckValue x3829=IKPowWithIntegerCheck(IKsign(((((-2.0)*x3822))+(((-3.0)*x3824*x3825))+(((-1.0)*x3824*x3826))+(((-1.5)*(r22*r22)))+((x3825*x3826))+(((-1.0)*x3823*x3827))+(((-0.5)*x3823))+((x3822*x3827)))),-1); +if(!x3829.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3828.value)+(((1.5707963267949)*(x3829.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3830=IKcos(j4); +IkReal x3831=IKsin(j4); +IkReal x3832=((0.5)*cj5); +IkReal x3833=(r01*sj0); +IkReal x3834=((1.0)*sj5); +IkReal x3835=(r11*sj0); +IkReal x3836=(r02*sj0); +IkReal x3837=(r00*sj0); +IkReal x3838=(cj0*r02); +IkReal x3839=(cj0*r01); +IkReal x3840=(r10*sj0); +IkReal x3841=(cj0*r00); +IkReal x3842=(cj0*r11); +IkReal x3843=(cj0*r10); +IkReal x3844=(sj5*x3830); +IkReal x3845=(r20*x3831); +IkReal x3846=(cj5*x3831); +IkReal x3847=((0.866025403784439)*x3831); +IkReal x3848=((0.866025403784439)*x3830); +IkReal x3849=(cj5*x3830); +IkReal x3850=(x3831*x3843); +IkReal x3851=(x3831*x3840); +IkReal x3852=(r12*x3848); +IkReal x3853=((0.5)*sj5*x3831); +evalcond[0]=((((-1.0)*r21*x3853))+((r21*x3849))+((r20*x3844))+(((-1.0)*r22*x3847))+((x3832*x3845))); +evalcond[1]=((-0.866025403784439)+(((-0.5)*r21*x3844))+((r20*x3830*x3832))+(((-1.0)*x3834*x3845))+(((-1.0)*r21*x3846))+(((-1.0)*r22*x3848))); +evalcond[2]=((((-1.0)*x3833*x3853))+(((-1.0)*x3832*x3850))+cj3+((x3837*x3844))+(((-1.0)*x3830*x3834*x3843))+((x3833*x3849))+((x3842*x3853))+(((-1.0)*x3842*x3849))+((x3831*x3832*x3837))+((cj0*r12*x3847))+(((-1.0)*x3836*x3847))); +evalcond[3]=(((sj5*x3851))+(((-1.0)*x3830*x3832*x3840))+(((-1.0)*x3830*x3832*x3841))+((x3839*x3846))+((sj5*x3831*x3841))+((x3838*x3848))+(((0.5)*x3835*x3844))+(((-0.5)*cj3))+((sj0*x3852))+(((0.5)*x3839*x3844))+((x3835*x3846))); +evalcond[4]=(((sj5*x3850))+(((-1.0)*x3830*x3832*x3843))+(((0.5)*x3842*x3844))+(((0.5)*sj3))+((x3830*x3832*x3837))+((x3842*x3846))+(((-1.0)*x3833*x3846))+((cj0*x3852))+(((-1.0)*x3831*x3834*x3837))+(((-1.0)*x3836*x3848))+(((-0.5)*x3833*x3844))); +evalcond[5]=((((-1.0)*x3839*x3849))+((x3839*x3853))+((x3835*x3853))+(((-1.0)*x3832*x3851))+sj3+((x3838*x3847))+(((-1.0)*x3835*x3849))+(((-1.0)*x3830*x3834*x3840))+(((-1.0)*x3830*x3834*x3841))+(((-1.0)*x3831*x3832*x3841))+((r12*sj0*x3847))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3854=((0.866025403784439)*cj0); +IkReal x3855=(cj3*r21); +IkReal x3856=(cj3*r20); +IkReal x3857=((0.866025403784439)*sj0); +CheckValue x3858=IKPowWithIntegerCheck(IKsign(((((0.5)*r12*sj0))+(((-1.0)*r11*sj5*x3857))+((cj5*r10*x3857))+(((-1.0)*r01*sj5*x3854))+(((0.5)*cj0*r02))+((cj5*r00*x3854)))),-1); +if(!x3858.valid){ +continue; +} +CheckValue x3859 = IKatan2WithCheck(IkReal((((cj5*x3855))+((sj5*x3856)))),IkReal(((((0.5)*sj5*x3855))+(((-0.5)*cj5*x3856))+(((0.866025403784439)*cj3*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3859.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3858.value)))+(x3859.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3860=IKcos(j4); +IkReal x3861=IKsin(j4); +IkReal x3862=((0.5)*cj5); +IkReal x3863=(r01*sj0); +IkReal x3864=((1.0)*sj5); +IkReal x3865=(r11*sj0); +IkReal x3866=(r02*sj0); +IkReal x3867=(r00*sj0); +IkReal x3868=(cj0*r02); +IkReal x3869=(cj0*r01); +IkReal x3870=(r10*sj0); +IkReal x3871=(cj0*r00); +IkReal x3872=(cj0*r11); +IkReal x3873=(cj0*r10); +IkReal x3874=(sj5*x3860); +IkReal x3875=(r20*x3861); +IkReal x3876=(cj5*x3861); +IkReal x3877=((0.866025403784439)*x3861); +IkReal x3878=((0.866025403784439)*x3860); +IkReal x3879=(cj5*x3860); +IkReal x3880=(x3861*x3873); +IkReal x3881=(x3861*x3870); +IkReal x3882=(r12*x3878); +IkReal x3883=((0.5)*sj5*x3861); +evalcond[0]=(((x3862*x3875))+((r20*x3874))+(((-1.0)*r21*x3883))+((r21*x3879))+(((-1.0)*r22*x3877))); +evalcond[1]=((-0.866025403784439)+((r20*x3860*x3862))+(((-1.0)*r22*x3878))+(((-0.5)*r21*x3874))+(((-1.0)*r21*x3876))+(((-1.0)*x3864*x3875))); +evalcond[2]=(((x3872*x3883))+((x3863*x3879))+(((-1.0)*x3866*x3877))+cj3+((x3867*x3874))+(((-1.0)*x3863*x3883))+((x3861*x3862*x3867))+(((-1.0)*x3872*x3879))+(((-1.0)*x3860*x3864*x3873))+((cj0*r12*x3877))+(((-1.0)*x3862*x3880))); +evalcond[3]=(((sj0*x3882))+((x3865*x3876))+(((0.5)*x3865*x3874))+((sj5*x3881))+(((-1.0)*x3860*x3862*x3870))+(((-1.0)*x3860*x3862*x3871))+((x3869*x3876))+((x3868*x3878))+(((-0.5)*cj3))+(((0.5)*x3869*x3874))+((sj5*x3861*x3871))); +evalcond[4]=((((-1.0)*x3866*x3878))+((sj5*x3880))+(((0.5)*sj3))+((x3872*x3876))+((cj0*x3882))+(((-1.0)*x3861*x3864*x3867))+((x3860*x3862*x3867))+(((-1.0)*x3860*x3862*x3873))+(((-0.5)*x3863*x3874))+(((-1.0)*x3863*x3876))+(((0.5)*x3872*x3874))); +evalcond[5]=((((-1.0)*x3869*x3879))+sj3+((x3869*x3883))+(((-1.0)*x3861*x3862*x3871))+(((-1.0)*x3865*x3879))+((r12*sj0*x3877))+((x3868*x3877))+((x3865*x3883))+(((-1.0)*x3860*x3864*x3871))+(((-1.0)*x3860*x3864*x3870))+(((-1.0)*x3862*x3881))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(((-3.14159265358979)+(IKfmod(j2, 6.28318530717959)))))+(IKabs(((-3.14159265358979)+(IKfmod(((4.71238898038469)+j1), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x3884=((0.577350269189626)*r02); +IkReal x3885=(cj5*sj0); +IkReal x3886=(sj0*sj5); +IkReal x3887=((1.0)*r11); +IkReal x3888=(cj0*cj5); +IkReal x3889=(cj0*sj5); +IkReal x3890=((0.577350269189626)*r12); +if( IKabs(((((-1.0)*r00*x3885))+((r10*x3888))+((cj0*x3890))+((r01*x3886))+(((-1.0)*sj0*x3884))+(((-1.0)*x3887*x3889)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((r10*x3885))+(((-1.0)*r01*x3889))+((sj0*x3890))+((cj0*x3884))+((r00*x3888))+(((-1.0)*x3886*x3887)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*r00*x3885))+((r10*x3888))+((cj0*x3890))+((r01*x3886))+(((-1.0)*sj0*x3884))+(((-1.0)*x3887*x3889))))+IKsqr((((r10*x3885))+(((-1.0)*r01*x3889))+((sj0*x3890))+((cj0*x3884))+((r00*x3888))+(((-1.0)*x3886*x3887))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*r00*x3885))+((r10*x3888))+((cj0*x3890))+((r01*x3886))+(((-1.0)*sj0*x3884))+(((-1.0)*x3887*x3889))), (((r10*x3885))+(((-1.0)*r01*x3889))+((sj0*x3890))+((cj0*x3884))+((r00*x3888))+(((-1.0)*x3886*x3887)))); +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[2]; +IkReal x3891=((0.866025403784439)*sj5); +IkReal x3892=((0.5)*sj0); +IkReal x3893=((0.5)*cj0); +IkReal x3894=((0.866025403784439)*cj5*r10); +IkReal x3895=((0.866025403784439)*cj5*r00); +evalcond[0]=(((sj0*x3895))+(((-1.0)*r12*x3893))+(((-1.0)*cj0*x3894))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r01*sj0*x3891))+((cj0*r11*x3891))+((r02*x3892))); +evalcond[1]=((((-1.0)*sj0*x3894))+(((-1.0)*r12*x3892))+(((-1.0)*cj0*x3895))+(((0.866025403784439)*(IKcos(j3))))+((r11*sj0*x3891))+((cj0*r01*x3891))+(((-1.0)*r02*x3893))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x3896=(r12*sj0); +IkReal x3897=(cj3*sj5); +IkReal x3898=(cj0*r02); +IkReal x3899=(cj3*cj5); +IkReal x3900=(cj0*cj5*r00); +IkReal x3901=(r11*sj0*sj5); +IkReal x3902=(cj0*r01*sj5); +IkReal x3903=(cj5*r10*sj0); +j4eval[0]=((((1.73205080756888)*x3903))+(((1.73205080756888)*x3900))+x3898+x3896+(((-1.73205080756888)*x3902))+(((-1.73205080756888)*x3901))); +j4eval[1]=((((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+(((-1.0)*r20*x3899))+((r21*x3897)))))))+(IKabs((((r20*x3897))+((r21*x3899)))))); +j4eval[2]=IKsign(((((0.5)*x3896))+(((0.5)*x3898))+(((-0.866025403784439)*x3902))+(((-0.866025403784439)*x3901))+(((0.866025403784439)*x3903))+(((0.866025403784439)*x3900)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x3904=r21*r21; +IkReal x3905=cj5*cj5; +IkReal x3906=r20*r20; +IkReal x3907=r22*r22; +IkReal x3908=(r21*sj5); +IkReal x3909=((1.73205080756888)*r20); +IkReal x3910=(cj5*r20); +IkReal x3911=((1.15470053837925)*r22); +IkReal x3912=(x3904*x3905); +IkReal x3913=(x3905*x3906); +j4eval[0]=((((2.0)*x3908*x3910))+(((-1.0)*x3913))+(((-1.0)*x3910*x3911))+x3907+x3912+(((1.33333333333333)*x3906))+((x3908*x3911))+(((0.333333333333333)*x3904))); +j4eval[1]=IKsign(((((0.5)*x3904))+(((-1.0)*cj5*r22*x3909))+(((1.5)*x3907))+(((1.73205080756888)*r22*x3908))+(((2.0)*x3906))+(((3.0)*x3908*x3910))+(((1.5)*x3912))+(((-1.5)*x3913)))); +j4eval[2]=((IKabs(((((1.5)*r22))+(((-0.866025403784439)*x3910))+(((0.866025403784439)*x3908)))))+(IKabs(((((1.73205080756888)*cj5*r21))+((sj5*x3909)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +IkReal x3914=cj5*cj5; +IkReal x3915=(cj0*r01); +IkReal x3916=((0.866025403784439)*r22); +IkReal x3917=(cj5*sj5); +IkReal x3918=((0.5)*r21); +IkReal x3919=(cj0*r22); +IkReal x3920=(cj0*r00); +IkReal x3921=((2.0)*r20); +IkReal x3922=(r20*sj0); +IkReal x3923=((0.866025403784439)*r12); +IkReal x3924=((3.0)*r11); +IkReal x3925=((3.0)*sj0); +IkReal x3926=((1.73205080756888)*cj5); +IkReal x3927=(r21*sj5); +IkReal x3928=(r11*sj0); +IkReal x3929=(r12*r22); +IkReal x3930=(r10*sj0); +IkReal x3931=(cj3*cj5); +IkReal x3932=((3.0)*r21); +IkReal x3933=((1.0)*r21); +IkReal x3934=(cj0*r02); +IkReal x3935=((1.5)*x3930); +IkReal x3936=(r20*x3914); +IkReal x3937=((1.73205080756888)*r22*sj5); +IkReal x3938=(r20*x3934); +IkReal x3939=((1.5)*r21*x3914); +j4eval[0]=((((-3.0)*r20*x3915*x3917))+(((-3.0)*r02*x3919))+(((-1.0)*x3914*x3915*x3932))+(((-4.0)*r20*x3920))+((r22*x3926*x3930))+(((-1.0)*x3915*x3933))+(((-1.0)*x3915*x3937))+(((-4.0)*r10*x3922))+(((-1.0)*x3925*x3929))+(((-1.0)*r21*sj0*x3914*x3924))+(((3.0)*x3920*x3936))+(((-1.0)*r10*r21*x3917*x3925))+(((3.0)*r10*x3914*x3922))+((r00*x3919*x3926))+((x3926*x3938))+(((-1.73205080756888)*x3927*x3934))+(((-1.0)*x3928*x3933))+(((-1.0)*x3928*x3937))+((r12*x3922*x3926))+(((-1.73205080756888)*r12*sj0*x3927))+(((-1.0)*x3917*x3922*x3924))+(((-1.0)*x3917*x3920*x3932))); +j4eval[1]=((((0.5)*(IKabs(((((1.73205080756888)*cj3*r22))+((cj3*x3927))+(((-1.0)*r20*x3931)))))))+(IKabs((((cj3*r20*sj5))+((r21*x3931)))))); +j4eval[2]=IKsign(((((-1.0)*x3915*x3918))+(((0.866025403784439)*cj5*x3938))+(((1.5)*r10*x3914*x3922))+(((-1.5)*sj0*x3929))+(((-1.0)*x3915*x3939))+(((-1.5)*r11*x3917*x3922))+((cj5*x3916*x3930))+(((-0.866025403784439)*x3927*x3934))+(((-1.5)*r02*x3919))+(((-1.0)*x3918*x3928))+(((-1.0)*x3921*x3930))+(((-1.0)*x3920*x3921))+(((-1.5)*r20*x3915*x3917))+(((-1.0)*x3928*x3939))+(((-1.0)*sj5*x3915*x3916))+(((-1.0)*sj0*x3923*x3927))+(((-1.0)*sj5*x3916*x3928))+((cj5*x3916*x3920))+(((-1.5)*r21*x3917*x3920))+(((-1.0)*r21*x3917*x3935))+((cj5*x3922*x3923))+(((1.5)*x3920*x3936)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3940=((0.866025403784439)*sj5); +IkReal x3941=(r01*sj0); +IkReal x3942=(r02*sj0); +IkReal x3943=(cj0*r11); +IkReal x3944=(cj0*r12); +IkReal x3945=((1.73205080756888)*sj5); +IkReal x3946=(cj0*cj5*r10); +IkReal x3947=(cj5*r00*sj0); +j4eval[0]=((((1.73205080756888)*x3946))+(((-1.0)*x3942))+((x3941*x3945))+x3944+(((-1.0)*x3943*x3945))+(((-1.73205080756888)*x3947))); +j4eval[1]=IKsign(((((0.866025403784439)*x3946))+(((-0.5)*x3942))+(((0.5)*x3944))+((x3940*x3941))+(((-1.0)*x3940*x3943))+(((-0.866025403784439)*x3947)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3948=r21*r21; +IkReal x3949=cj5*cj5; +IkReal x3950=r20*r20; +IkReal x3951=r22*r22; +IkReal x3952=(r21*sj5); +IkReal x3953=((1.73205080756888)*r20); +IkReal x3954=(cj5*r20); +IkReal x3955=((1.15470053837925)*r22); +IkReal x3956=(x3948*x3949); +IkReal x3957=(x3949*x3950); +j4eval[0]=((((-1.0)*x3954*x3955))+(((-1.0)*x3957))+(((2.0)*x3952*x3954))+(((1.33333333333333)*x3950))+(((0.333333333333333)*x3948))+x3951+x3956+((x3952*x3955))); +j4eval[1]=IKsign(((((-1.0)*cj5*r22*x3953))+(((1.73205080756888)*r22*x3952))+(((0.5)*x3948))+(((2.0)*x3950))+(((-1.5)*x3957))+(((3.0)*x3952*x3954))+(((1.5)*x3951))+(((1.5)*x3956)))); +j4eval[2]=((IKabs(((((1.73205080756888)*cj5*r21))+((sj5*x3953)))))+(IKabs(((((0.866025403784439)*x3952))+(((1.5)*r22))+(((-0.866025403784439)*x3954)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=1.0; +cj3=0; +j3=1.5707963267949; +IkReal x3958=((1.73205080756888)*sj0); +IkReal x3959=((1.73205080756888)*cj0); +IkReal x3960=(((r01*sj5*x3958))+(((-1.0)*cj5*r00*x3958))+(((-1.0)*r02*sj0))+((cj5*r10*x3959))+(((-1.0)*r11*sj5*x3959))+((cj0*r12))); +j4eval[0]=x3960; +j4eval[1]=IKsign(x3960); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3961=((1.73205080756888)*sj0); +IkReal x3962=(r01*sj5); +IkReal x3963=((0.866025403784439)*cj0); +IkReal x3964=(cj5*r00); +IkReal x3965=((0.866025403784439)*sj0); +IkReal x3966=(cj5*r10); +IkReal x3967=((1.73205080756888)*cj0); +IkReal x3968=(r11*sj5); +CheckValue x3969=IKPowWithIntegerCheck(IKsign((((x3961*x3962))+(((-1.0)*r02*sj0))+(((-1.0)*x3961*x3964))+((x3966*x3967))+(((-1.0)*x3967*x3968))+((cj0*r12)))),-1); +if(!x3969.valid){ +continue; +} +CheckValue x3970 = IKatan2WithCheck(IkReal((((x3962*x3963))+(((-1.0)*x3963*x3964))+(((1.5)*cj0*r02))+((x3965*x3968))+(((-1.0)*x3965*x3966))+(((1.5)*r12*sj0)))),IkReal(((((-1.0)*cj5*r11*x3961))+(((-1.0)*r10*sj5*x3961))+(((-1.0)*cj5*r01*x3967))+(((-1.0)*r00*sj5*x3967)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3970.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x3969.value)))+(x3970.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x3971=IKcos(j4); +IkReal x3972=IKsin(j4); +IkReal x3973=((0.5)*r21); +IkReal x3974=(cj0*r10); +IkReal x3975=((0.5)*sj0); +IkReal x3976=(r10*sj0); +IkReal x3977=((0.5)*r20); +IkReal x3978=((1.0)*sj0); +IkReal x3979=(cj0*r01); +IkReal x3980=(r02*sj0); +IkReal x3981=(cj0*r02); +IkReal x3982=(cj0*r00); +IkReal x3983=(cj0*r11); +IkReal x3984=(sj5*x3972); +IkReal x3985=(cj5*x3972); +IkReal x3986=(sj5*x3971); +IkReal x3987=((0.866025403784439)*x3972); +IkReal x3988=((0.866025403784439)*x3971); +IkReal x3989=(cj5*x3971); +IkReal x3990=(r12*x3988); +evalcond[0]=(((r21*x3989))+((x3977*x3985))+(((-1.0)*x3973*x3984))+(((-1.0)*r22*x3987))+((r20*x3986))); +evalcond[1]=((0.866025403784439)+((x3977*x3989))+(((-1.0)*x3973*x3986))+(((-1.0)*r22*x3988))+(((-1.0)*r20*x3984))+(((-1.0)*r21*x3985))); +evalcond[2]=((((-1.0)*r10*x3975*x3989))+((r11*sj0*x3985))+(((0.5)*x3979*x3986))+((x3981*x3988))+((x3976*x3984))+((r11*x3975*x3986))+((x3982*x3984))+((x3979*x3985))+((sj0*x3990))+(((-0.5)*x3982*x3989))); +evalcond[3]=(((r00*sj0*x3986))+(((-1.0)*x3983*x3989))+(((-1.0)*r01*x3975*x3984))+((r01*sj0*x3989))+(((-1.0)*x3974*x3986))+(((-0.5)*x3974*x3985))+(((0.5)*x3983*x3984))+(((-1.0)*x3980*x3987))+((cj0*r12*x3987))+((r00*x3975*x3985))); +evalcond[4]=((0.5)+((x3983*x3985))+(((-1.0)*r00*x3978*x3984))+(((-1.0)*r01*x3975*x3986))+(((-0.5)*x3974*x3989))+(((0.5)*x3983*x3986))+(((-1.0)*x3980*x3988))+((cj0*x3990))+((x3974*x3984))+(((-1.0)*r01*x3978*x3985))+((r00*x3975*x3989))); +evalcond[5]=((-1.0)+(((-1.0)*r10*x3975*x3985))+(((0.5)*x3979*x3984))+(((-1.0)*x3982*x3986))+(((-1.0)*x3979*x3989))+((x3981*x3987))+((r11*x3975*x3984))+(((-1.0)*x3976*x3986))+(((-1.0)*r11*x3978*x3989))+(((-0.5)*x3982*x3985))+((r12*sj0*x3987))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x3991=r21*r21; +IkReal x3992=cj5*cj5; +IkReal x3993=r20*r20; +IkReal x3994=(r21*sj5); +IkReal x3995=((1.73205080756888)*cj5); +IkReal x3996=(cj5*r20); +IkReal x3997=((1.5)*x3992); +CheckValue x3998 = IKatan2WithCheck(IkReal((((r21*x3995))+(((1.73205080756888)*r20*sj5)))),IkReal(((((-0.866025403784439)*x3996))+(((1.5)*r22))+(((0.866025403784439)*x3994)))),IKFAST_ATAN2_MAGTHRESH); +if(!x3998.valid){ +continue; +} +CheckValue x3999=IKPowWithIntegerCheck(IKsign(((((-1.0)*x3993*x3997))+(((0.5)*x3991))+(((3.0)*x3994*x3996))+(((-1.0)*r20*r22*x3995))+(((2.0)*x3993))+(((1.73205080756888)*r22*x3994))+(((1.5)*(r22*r22)))+((x3991*x3997)))),-1); +if(!x3999.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x3998.value)+(((1.5707963267949)*(x3999.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4000=IKcos(j4); +IkReal x4001=IKsin(j4); +IkReal x4002=((0.5)*r21); +IkReal x4003=(cj0*r10); +IkReal x4004=((0.5)*sj0); +IkReal x4005=(r10*sj0); +IkReal x4006=((0.5)*r20); +IkReal x4007=((1.0)*sj0); +IkReal x4008=(cj0*r01); +IkReal x4009=(r02*sj0); +IkReal x4010=(cj0*r02); +IkReal x4011=(cj0*r00); +IkReal x4012=(cj0*r11); +IkReal x4013=(sj5*x4001); +IkReal x4014=(cj5*x4001); +IkReal x4015=(sj5*x4000); +IkReal x4016=((0.866025403784439)*x4001); +IkReal x4017=((0.866025403784439)*x4000); +IkReal x4018=(cj5*x4000); +IkReal x4019=(r12*x4017); +evalcond[0]=((((-1.0)*r22*x4016))+(((-1.0)*x4002*x4013))+((r20*x4015))+((r21*x4018))+((x4006*x4014))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r20*x4013))+(((-1.0)*r22*x4017))+(((-1.0)*x4002*x4015))+(((-1.0)*r21*x4014))+((x4006*x4018))); +evalcond[2]=((((-1.0)*r10*x4004*x4018))+((x4010*x4017))+((sj0*x4019))+((x4005*x4013))+(((-0.5)*x4011*x4018))+((x4011*x4013))+((r11*x4004*x4015))+((x4008*x4014))+((r11*sj0*x4014))+(((0.5)*x4008*x4015))); +evalcond[3]=((((-1.0)*r01*x4004*x4013))+(((-1.0)*x4012*x4018))+(((-1.0)*x4003*x4015))+((r00*x4004*x4014))+(((-1.0)*x4009*x4016))+((cj0*r12*x4016))+((r00*sj0*x4015))+(((-0.5)*x4003*x4014))+(((0.5)*x4012*x4013))+((r01*sj0*x4018))); +evalcond[4]=((0.5)+((x4003*x4013))+(((-1.0)*r00*x4007*x4013))+((cj0*x4019))+(((-1.0)*r01*x4004*x4015))+((x4012*x4014))+((r00*x4004*x4018))+(((-1.0)*x4009*x4017))+(((-0.5)*x4003*x4018))+(((-1.0)*r01*x4007*x4014))+(((0.5)*x4012*x4015))); +evalcond[5]=((-1.0)+(((-1.0)*x4008*x4018))+(((-1.0)*r10*x4004*x4014))+((x4010*x4016))+(((-1.0)*r11*x4007*x4018))+(((-1.0)*x4005*x4015))+(((-0.5)*x4011*x4014))+(((-1.0)*x4011*x4015))+((r12*sj0*x4016))+((r11*x4004*x4013))+(((0.5)*x4008*x4013))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4020=((0.866025403784439)*sj5); +IkReal x4021=((0.866025403784439)*cj5); +CheckValue x4022 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4022.valid){ +continue; +} +CheckValue x4023=IKPowWithIntegerCheck(IKsign((((r01*sj0*x4020))+((cj0*r10*x4021))+(((-0.5)*r02*sj0))+(((-1.0)*r00*sj0*x4021))+(((-1.0)*cj0*r11*x4020))+(((0.5)*cj0*r12)))),-1); +if(!x4023.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4022.value)+(((1.5707963267949)*(x4023.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4024=IKcos(j4); +IkReal x4025=IKsin(j4); +IkReal x4026=((0.5)*r21); +IkReal x4027=(cj0*r10); +IkReal x4028=((0.5)*sj0); +IkReal x4029=(r10*sj0); +IkReal x4030=((0.5)*r20); +IkReal x4031=((1.0)*sj0); +IkReal x4032=(cj0*r01); +IkReal x4033=(r02*sj0); +IkReal x4034=(cj0*r02); +IkReal x4035=(cj0*r00); +IkReal x4036=(cj0*r11); +IkReal x4037=(sj5*x4025); +IkReal x4038=(cj5*x4025); +IkReal x4039=(sj5*x4024); +IkReal x4040=((0.866025403784439)*x4025); +IkReal x4041=((0.866025403784439)*x4024); +IkReal x4042=(cj5*x4024); +IkReal x4043=(r12*x4041); +evalcond[0]=(((r21*x4042))+(((-1.0)*x4026*x4037))+((r20*x4039))+(((-1.0)*r22*x4040))+((x4030*x4038))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r20*x4037))+(((-1.0)*x4026*x4039))+(((-1.0)*r21*x4038))+(((-1.0)*r22*x4041))+((x4030*x4042))); +evalcond[2]=(((x4032*x4038))+(((0.5)*x4032*x4039))+(((-1.0)*r10*x4028*x4042))+((x4035*x4037))+((x4029*x4037))+((r11*sj0*x4038))+((x4034*x4041))+((sj0*x4043))+((r11*x4028*x4039))+(((-0.5)*x4035*x4042))); +evalcond[3]=(((cj0*r12*x4040))+(((0.5)*x4036*x4037))+((r01*sj0*x4042))+(((-1.0)*r01*x4028*x4037))+((r00*sj0*x4039))+(((-1.0)*x4033*x4040))+(((-0.5)*x4027*x4038))+((r00*x4028*x4038))+(((-1.0)*x4027*x4039))+(((-1.0)*x4036*x4042))); +evalcond[4]=((0.5)+((r00*x4028*x4042))+(((-1.0)*r01*x4031*x4038))+(((0.5)*x4036*x4039))+(((-1.0)*r01*x4028*x4039))+((x4036*x4038))+((x4027*x4037))+(((-1.0)*x4033*x4041))+((cj0*x4043))+(((-1.0)*r00*x4031*x4037))+(((-0.5)*x4027*x4042))); +evalcond[5]=((-1.0)+(((-0.5)*x4035*x4038))+(((0.5)*x4032*x4037))+(((-1.0)*x4029*x4039))+((r12*sj0*x4040))+(((-1.0)*r10*x4028*x4038))+(((-1.0)*x4035*x4039))+((x4034*x4040))+(((-1.0)*r11*x4031*x4042))+(((-1.0)*x4032*x4042))+((r11*x4028*x4037))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x4044=(cj0*r12); +IkReal x4045=((0.866025403784439)*sj5); +IkReal x4046=(r01*sj0); +IkReal x4047=(r02*sj0); +IkReal x4048=(cj0*r11); +IkReal x4049=((1.73205080756888)*sj5); +IkReal x4050=(cj0*cj5*r10); +IkReal x4051=(cj5*r00*sj0); +j4eval[0]=((((1.73205080756888)*x4051))+(((-1.0)*x4046*x4049))+(((-1.73205080756888)*x4050))+x4047+(((-1.0)*x4044))+((x4048*x4049))); +j4eval[1]=IKsign(((((-0.5)*x4044))+((x4045*x4048))+(((-1.0)*x4045*x4046))+(((-0.866025403784439)*x4050))+(((0.5)*x4047))+(((0.866025403784439)*x4051)))); +j4eval[2]=((IKabs((((cj5*r21))+((r20*sj5)))))+(IKabs(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[3]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x4052=r21*r21; +IkReal x4053=cj5*cj5; +IkReal x4054=r20*r20; +IkReal x4055=r22*r22; +IkReal x4056=(r21*sj5); +IkReal x4057=((1.73205080756888)*r20); +IkReal x4058=(cj5*r20); +IkReal x4059=((1.15470053837925)*r22); +IkReal x4060=(x4052*x4053); +IkReal x4061=(x4053*x4054); +j4eval[0]=(x4055+x4060+(((2.0)*x4056*x4058))+(((-1.0)*x4058*x4059))+(((0.333333333333333)*x4052))+(((-1.0)*x4061))+(((1.33333333333333)*x4054))+((x4056*x4059))); +j4eval[1]=IKsign(((((-1.5)*x4061))+(((2.0)*x4054))+(((1.73205080756888)*r22*x4056))+(((3.0)*x4056*x4058))+(((0.5)*x4052))+(((1.5)*x4060))+(((-1.0)*cj5*r22*x4057))+(((1.5)*x4055)))); +j4eval[2]=((IKabs((((sj5*x4057))+(((1.73205080756888)*cj5*r21)))))+(IKabs(((((1.5)*r22))+(((-0.866025403784439)*x4058))+(((0.866025403784439)*x4056)))))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 || IKabs(j4eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +j1=-1.5707963267949; +sj1=-1.0; +cj1=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +IkReal x4062=((1.73205080756888)*sj0); +IkReal x4063=((1.73205080756888)*cj0); +IkReal x4064=((((-1.0)*cj5*r00*x4062))+((r01*sj5*x4062))+((cj5*r10*x4063))+(((-1.0)*r02*sj0))+((cj0*r12))+(((-1.0)*r11*sj5*x4063))); +j4eval[0]=x4064; +j4eval[1]=IKsign(x4064); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4065=((1.73205080756888)*sj0); +IkReal x4066=(r01*sj5); +IkReal x4067=((0.866025403784439)*cj0); +IkReal x4068=(cj5*r00); +IkReal x4069=((0.866025403784439)*sj0); +IkReal x4070=(cj5*r10); +IkReal x4071=((1.73205080756888)*cj0); +IkReal x4072=(r11*sj5); +CheckValue x4073=IKPowWithIntegerCheck(IKsign(((((-1.0)*x4071*x4072))+(((-1.0)*x4065*x4068))+((x4070*x4071))+(((-1.0)*r02*sj0))+((x4065*x4066))+((cj0*r12)))),-1); +if(!x4073.valid){ +continue; +} +CheckValue x4074 = IKatan2WithCheck(IkReal(((((-1.0)*x4069*x4070))+(((1.5)*cj0*r02))+((x4066*x4067))+(((-1.0)*x4067*x4068))+(((1.5)*r12*sj0))+((x4069*x4072)))),IkReal(((((-1.0)*r10*sj5*x4065))+(((-1.0)*r00*sj5*x4071))+(((-1.0)*cj5*r01*x4071))+(((-1.0)*cj5*r11*x4065)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4074.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x4073.value)))+(x4074.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4075=IKcos(j4); +IkReal x4076=IKsin(j4); +IkReal x4077=((0.5)*r21); +IkReal x4078=(cj0*r10); +IkReal x4079=((0.5)*sj0); +IkReal x4080=(r10*sj0); +IkReal x4081=((0.5)*r20); +IkReal x4082=((1.0)*sj0); +IkReal x4083=(cj0*r01); +IkReal x4084=(r02*sj0); +IkReal x4085=(cj0*r02); +IkReal x4086=(cj0*r00); +IkReal x4087=(cj0*r11); +IkReal x4088=(sj5*x4076); +IkReal x4089=(cj5*x4076); +IkReal x4090=(sj5*x4075); +IkReal x4091=((0.866025403784439)*x4076); +IkReal x4092=((0.866025403784439)*x4075); +IkReal x4093=(cj5*x4075); +IkReal x4094=(r12*x4092); +evalcond[0]=(((r20*x4090))+(((-1.0)*r22*x4091))+((r21*x4093))+((x4081*x4089))+(((-1.0)*x4077*x4088))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x4089))+(((-1.0)*r22*x4092))+(((-1.0)*r20*x4088))+(((-1.0)*x4077*x4090))+((x4081*x4093))); +evalcond[2]=(((r11*sj0*x4089))+(((-1.0)*r10*x4079*x4093))+((r11*x4079*x4090))+((x4083*x4089))+(((0.5)*x4083*x4090))+((x4086*x4088))+((x4085*x4092))+((x4080*x4088))+(((-0.5)*x4086*x4093))+((sj0*x4094))); +evalcond[3]=((((-1.0)*r01*x4079*x4088))+((r00*x4079*x4089))+(((0.5)*x4087*x4088))+(((-1.0)*x4084*x4091))+((cj0*r12*x4091))+(((-1.0)*x4078*x4090))+((r00*sj0*x4090))+(((-1.0)*x4087*x4093))+(((-0.5)*x4078*x4089))+((r01*sj0*x4093))); +evalcond[4]=((-0.5)+((x4087*x4089))+(((-1.0)*r01*x4082*x4089))+((r00*x4079*x4093))+(((-0.5)*x4078*x4093))+(((-1.0)*r00*x4082*x4088))+(((-1.0)*r01*x4079*x4090))+((cj0*x4094))+(((0.5)*x4087*x4090))+(((-1.0)*x4084*x4092))+((x4078*x4088))); +evalcond[5]=((1.0)+(((-1.0)*x4083*x4093))+(((-1.0)*x4086*x4090))+(((-1.0)*r10*x4079*x4089))+(((-1.0)*x4080*x4090))+((r11*x4079*x4088))+((r12*sj0*x4091))+((x4085*x4091))+(((-1.0)*r11*x4082*x4093))+(((0.5)*x4083*x4088))+(((-0.5)*x4086*x4089))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4095=r21*r21; +IkReal x4096=cj5*cj5; +IkReal x4097=r20*r20; +IkReal x4098=(r21*sj5); +IkReal x4099=((1.73205080756888)*cj5); +IkReal x4100=(cj5*r20); +IkReal x4101=((1.5)*x4096); +CheckValue x4102=IKPowWithIntegerCheck(IKsign(((((1.73205080756888)*r22*x4098))+(((0.5)*x4095))+(((2.0)*x4097))+(((3.0)*x4098*x4100))+(((-1.0)*r20*r22*x4099))+(((1.5)*(r22*r22)))+(((-1.0)*x4097*x4101))+((x4095*x4101)))),-1); +if(!x4102.valid){ +continue; +} +CheckValue x4103 = IKatan2WithCheck(IkReal((((r21*x4099))+(((1.73205080756888)*r20*sj5)))),IkReal(((((1.5)*r22))+(((-0.866025403784439)*x4100))+(((0.866025403784439)*x4098)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4103.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x4102.value)))+(x4103.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4104=IKcos(j4); +IkReal x4105=IKsin(j4); +IkReal x4106=((0.5)*r21); +IkReal x4107=(cj0*r10); +IkReal x4108=((0.5)*sj0); +IkReal x4109=(r10*sj0); +IkReal x4110=((0.5)*r20); +IkReal x4111=((1.0)*sj0); +IkReal x4112=(cj0*r01); +IkReal x4113=(r02*sj0); +IkReal x4114=(cj0*r02); +IkReal x4115=(cj0*r00); +IkReal x4116=(cj0*r11); +IkReal x4117=(sj5*x4105); +IkReal x4118=(cj5*x4105); +IkReal x4119=(sj5*x4104); +IkReal x4120=((0.866025403784439)*x4105); +IkReal x4121=((0.866025403784439)*x4104); +IkReal x4122=(cj5*x4104); +IkReal x4123=(r12*x4121); +evalcond[0]=(((r21*x4122))+(((-1.0)*r22*x4120))+((r20*x4119))+((x4110*x4118))+(((-1.0)*x4106*x4117))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r20*x4117))+(((-1.0)*r22*x4121))+(((-1.0)*r21*x4118))+((x4110*x4122))+(((-1.0)*x4106*x4119))); +evalcond[2]=(((x4112*x4118))+((sj0*x4123))+((x4109*x4117))+((x4114*x4121))+(((-0.5)*x4115*x4122))+((x4115*x4117))+((r11*x4108*x4119))+(((-1.0)*r10*x4108*x4122))+(((0.5)*x4112*x4119))+((r11*sj0*x4118))); +evalcond[3]=(((r00*sj0*x4119))+(((-1.0)*x4116*x4122))+((r00*x4108*x4118))+(((-1.0)*x4113*x4120))+(((-1.0)*x4107*x4119))+(((0.5)*x4116*x4117))+(((-0.5)*x4107*x4118))+((r01*sj0*x4122))+(((-1.0)*r01*x4108*x4117))+((cj0*r12*x4120))); +evalcond[4]=((-0.5)+(((-1.0)*r01*x4111*x4118))+((r00*x4108*x4122))+((cj0*x4123))+((x4116*x4118))+(((-1.0)*r00*x4111*x4117))+(((-1.0)*x4113*x4121))+((x4107*x4117))+(((0.5)*x4116*x4119))+(((-0.5)*x4107*x4122))+(((-1.0)*r01*x4108*x4119))); +evalcond[5]=((1.0)+(((-1.0)*x4112*x4122))+(((-1.0)*r11*x4111*x4122))+(((-1.0)*x4109*x4119))+(((-1.0)*x4115*x4119))+((x4114*x4120))+(((-0.5)*x4115*x4118))+((r11*x4108*x4117))+(((-1.0)*r10*x4108*x4118))+(((0.5)*x4112*x4117))+((r12*sj0*x4120))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4124=((0.866025403784439)*sj5); +IkReal x4125=((0.866025403784439)*cj5); +CheckValue x4126 = IKatan2WithCheck(IkReal((((cj5*r21))+((r20*sj5)))),IkReal(((((0.5)*r21*sj5))+(((-0.5)*cj5*r20))+(((0.866025403784439)*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4126.valid){ +continue; +} +CheckValue x4127=IKPowWithIntegerCheck(IKsign((((r00*sj0*x4125))+(((-0.5)*cj0*r12))+(((-1.0)*r01*sj0*x4124))+(((-1.0)*cj0*r10*x4125))+(((0.5)*r02*sj0))+((cj0*r11*x4124)))),-1); +if(!x4127.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4126.value)+(((1.5707963267949)*(x4127.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4128=IKcos(j4); +IkReal x4129=IKsin(j4); +IkReal x4130=((0.5)*r21); +IkReal x4131=(cj0*r10); +IkReal x4132=((0.5)*sj0); +IkReal x4133=(r10*sj0); +IkReal x4134=((0.5)*r20); +IkReal x4135=((1.0)*sj0); +IkReal x4136=(cj0*r01); +IkReal x4137=(r02*sj0); +IkReal x4138=(cj0*r02); +IkReal x4139=(cj0*r00); +IkReal x4140=(cj0*r11); +IkReal x4141=(sj5*x4129); +IkReal x4142=(cj5*x4129); +IkReal x4143=(sj5*x4128); +IkReal x4144=((0.866025403784439)*x4129); +IkReal x4145=((0.866025403784439)*x4128); +IkReal x4146=(cj5*x4128); +IkReal x4147=(r12*x4145); +evalcond[0]=(((r21*x4146))+((r20*x4143))+((x4134*x4142))+(((-1.0)*r22*x4144))+(((-1.0)*x4130*x4141))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x4142))+((x4134*x4146))+(((-1.0)*r22*x4145))+(((-1.0)*r20*x4141))+(((-1.0)*x4130*x4143))); +evalcond[2]=(((x4139*x4141))+(((0.5)*x4136*x4143))+((r11*x4132*x4143))+((sj0*x4147))+(((-0.5)*x4139*x4146))+((r11*sj0*x4142))+((x4138*x4145))+((x4133*x4141))+((x4136*x4142))+(((-1.0)*r10*x4132*x4146))); +evalcond[3]=(((cj0*r12*x4144))+(((0.5)*x4140*x4141))+((r01*sj0*x4146))+(((-1.0)*r01*x4132*x4141))+((r00*sj0*x4143))+((r00*x4132*x4142))+(((-1.0)*x4140*x4146))+(((-0.5)*x4131*x4142))+(((-1.0)*x4137*x4144))+(((-1.0)*x4131*x4143))); +evalcond[4]=((-0.5)+(((0.5)*x4140*x4143))+(((-1.0)*r00*x4135*x4141))+(((-1.0)*r01*x4132*x4143))+((cj0*x4147))+(((-1.0)*r01*x4135*x4142))+((x4131*x4141))+((r00*x4132*x4146))+(((-0.5)*x4131*x4146))+(((-1.0)*x4137*x4145))+((x4140*x4142))); +evalcond[5]=((1.0)+(((0.5)*x4136*x4141))+(((-1.0)*r11*x4135*x4146))+((r12*sj0*x4144))+((r11*x4132*x4141))+(((-1.0)*x4139*x4143))+(((-0.5)*x4139*x4142))+((x4138*x4144))+(((-1.0)*x4136*x4146))+(((-1.0)*x4133*x4143))+(((-1.0)*r10*x4132*x4142))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4148=cj5*cj5; +IkReal x4149=(r20*sj0); +IkReal x4150=(cj0*r01); +IkReal x4151=((0.866025403784439)*r22); +IkReal x4152=((0.5)*r21); +IkReal x4153=(cj0*r00); +IkReal x4154=((0.866025403784439)*r12); +IkReal x4155=(cj5*r20); +IkReal x4156=(r21*sj0); +IkReal x4157=((1.5)*sj5); +IkReal x4158=(cj3*sj5); +IkReal x4159=(cj0*r02); +IkReal x4160=(cj5*r21); +IkReal x4161=(cj5*r10); +IkReal x4162=((1.5)*r22); +IkReal x4163=(r11*sj0); +IkReal x4164=((1.5)*x4148); +CheckValue x4165 = IKatan2WithCheck(IkReal((((cj3*x4160))+((r20*x4158)))),IkReal((((x4152*x4158))+((cj3*x4151))+(((-0.5)*cj3*x4155)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4165.valid){ +continue; +} +CheckValue x4166=IKPowWithIntegerCheck(IKsign((((r20*x4153*x4164))+(((-1.0)*sj5*x4150*x4151))+(((-1.0)*sj5*x4151*x4163))+(((-1.0)*x4156*x4157*x4161))+((cj5*x4151*x4153))+(((0.866025403784439)*x4155*x4159))+((r10*x4149*x4164))+(((-1.0)*x4159*x4162))+(((-1.0)*r21*x4150*x4164))+(((-2.0)*r20*x4153))+((sj0*x4151*x4161))+((cj5*x4149*x4154))+(((-1.0)*x4150*x4152))+(((-0.866025403784439)*r21*sj5*x4159))+(((-1.0)*x4150*x4155*x4157))+(((-1.0)*cj5*r11*x4149*x4157))+(((-1.0)*x4152*x4163))+(((-1.0)*r11*x4156*x4164))+(((-1.0)*r12*sj0*x4162))+(((-1.0)*sj5*x4154*x4156))+(((-2.0)*r10*x4149))+(((-1.0)*x4153*x4157*x4160)))),-1); +if(!x4166.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4165.value)+(((1.5707963267949)*(x4166.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4167=IKcos(j4); +IkReal x4168=IKsin(j4); +IkReal x4169=((0.5)*cj5); +IkReal x4170=(r01*sj0); +IkReal x4171=((1.0)*sj5); +IkReal x4172=(r11*sj0); +IkReal x4173=(r02*sj0); +IkReal x4174=(r00*sj0); +IkReal x4175=(cj0*r02); +IkReal x4176=(cj0*r01); +IkReal x4177=(r10*sj0); +IkReal x4178=(cj0*r00); +IkReal x4179=(cj0*r11); +IkReal x4180=(cj0*r10); +IkReal x4181=(sj5*x4167); +IkReal x4182=(r20*x4168); +IkReal x4183=(cj5*x4168); +IkReal x4184=((0.866025403784439)*x4168); +IkReal x4185=((0.866025403784439)*x4167); +IkReal x4186=(cj5*x4167); +IkReal x4187=(x4168*x4180); +IkReal x4188=(x4168*x4177); +IkReal x4189=(r12*x4185); +IkReal x4190=((0.5)*sj5*x4168); +evalcond[0]=(((r20*x4181))+((r21*x4186))+(((-1.0)*r21*x4190))+(((-1.0)*r22*x4184))+((x4169*x4182))); +evalcond[1]=((0.866025403784439)+(((-1.0)*x4171*x4182))+((r20*x4167*x4169))+(((-1.0)*r21*x4183))+(((-0.5)*r21*x4181))+(((-1.0)*r22*x4185))); +evalcond[2]=(((x4168*x4169*x4174))+((x4179*x4190))+(((-1.0)*x4169*x4187))+cj3+(((-1.0)*x4179*x4186))+(((-1.0)*x4170*x4190))+((x4170*x4186))+((cj0*r12*x4184))+(((-1.0)*x4173*x4184))+(((-1.0)*x4167*x4171*x4180))+((x4174*x4181))); +evalcond[3]=((((-1.0)*x4167*x4169*x4177))+(((-1.0)*x4167*x4169*x4178))+((sj0*x4189))+(((0.5)*x4172*x4181))+(((0.5)*cj3))+((x4172*x4183))+(((0.5)*x4176*x4181))+((x4175*x4185))+((sj5*x4168*x4178))+((sj5*x4188))+((x4176*x4183))); +evalcond[4]=(((x4179*x4183))+(((-1.0)*x4168*x4171*x4174))+(((0.5)*sj3))+(((0.5)*x4179*x4181))+(((-1.0)*x4173*x4185))+(((-1.0)*x4170*x4183))+((x4167*x4169*x4174))+(((-1.0)*x4167*x4169*x4180))+((sj5*x4187))+((cj0*x4189))+(((-0.5)*x4170*x4181))); +evalcond[5]=((((-1.0)*sj3))+(((-1.0)*x4169*x4188))+((r12*sj0*x4184))+((x4172*x4190))+(((-1.0)*x4168*x4169*x4178))+(((-1.0)*x4172*x4186))+((x4175*x4184))+(((-1.0)*x4176*x4186))+(((-1.0)*x4167*x4171*x4178))+(((-1.0)*x4167*x4171*x4177))+((x4176*x4190))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4191=r21*r21; +IkReal x4192=cj5*cj5; +IkReal x4193=r20*r20; +IkReal x4194=(r21*sj5); +IkReal x4195=((1.73205080756888)*cj5); +IkReal x4196=(cj5*r20); +IkReal x4197=((1.5)*x4192); +CheckValue x4198 = IKatan2WithCheck(IkReal((((r21*x4195))+(((1.73205080756888)*r20*sj5)))),IkReal(((((0.866025403784439)*x4194))+(((1.5)*r22))+(((-0.866025403784439)*x4196)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4198.valid){ +continue; +} +CheckValue x4199=IKPowWithIntegerCheck(IKsign(((((1.73205080756888)*r22*x4194))+(((-1.0)*r20*r22*x4195))+((x4191*x4197))+(((3.0)*x4194*x4196))+(((-1.0)*x4193*x4197))+(((2.0)*x4193))+(((1.5)*(r22*r22)))+(((0.5)*x4191)))),-1); +if(!x4199.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4198.value)+(((1.5707963267949)*(x4199.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4200=IKcos(j4); +IkReal x4201=IKsin(j4); +IkReal x4202=((0.5)*cj5); +IkReal x4203=(r01*sj0); +IkReal x4204=((1.0)*sj5); +IkReal x4205=(r11*sj0); +IkReal x4206=(r02*sj0); +IkReal x4207=(r00*sj0); +IkReal x4208=(cj0*r02); +IkReal x4209=(cj0*r01); +IkReal x4210=(r10*sj0); +IkReal x4211=(cj0*r00); +IkReal x4212=(cj0*r11); +IkReal x4213=(cj0*r10); +IkReal x4214=(sj5*x4200); +IkReal x4215=(r20*x4201); +IkReal x4216=(cj5*x4201); +IkReal x4217=((0.866025403784439)*x4201); +IkReal x4218=((0.866025403784439)*x4200); +IkReal x4219=(cj5*x4200); +IkReal x4220=(x4201*x4213); +IkReal x4221=(x4201*x4210); +IkReal x4222=(r12*x4218); +IkReal x4223=((0.5)*sj5*x4201); +evalcond[0]=(((x4202*x4215))+(((-1.0)*r22*x4217))+((r20*x4214))+((r21*x4219))+(((-1.0)*r21*x4223))); +evalcond[1]=((0.866025403784439)+(((-1.0)*r21*x4216))+(((-1.0)*x4204*x4215))+(((-1.0)*r22*x4218))+((r20*x4200*x4202))+(((-0.5)*r21*x4214))); +evalcond[2]=(((x4201*x4202*x4207))+cj3+((x4203*x4219))+((x4212*x4223))+(((-1.0)*x4212*x4219))+((x4207*x4214))+(((-1.0)*x4206*x4217))+((cj0*r12*x4217))+(((-1.0)*x4203*x4223))+(((-1.0)*x4202*x4220))+(((-1.0)*x4200*x4204*x4213))); +evalcond[3]=(((sj5*x4201*x4211))+((x4205*x4216))+(((0.5)*x4205*x4214))+((x4209*x4216))+((sj0*x4222))+(((0.5)*cj3))+((sj5*x4221))+(((0.5)*x4209*x4214))+(((-1.0)*x4200*x4202*x4211))+(((-1.0)*x4200*x4202*x4210))+((x4208*x4218))); +evalcond[4]=((((0.5)*x4212*x4214))+(((-1.0)*x4201*x4204*x4207))+(((-0.5)*x4203*x4214))+(((0.5)*sj3))+(((-1.0)*x4203*x4216))+((x4212*x4216))+(((-1.0)*x4206*x4218))+((x4200*x4202*x4207))+((sj5*x4220))+(((-1.0)*x4200*x4202*x4213))+((cj0*x4222))); +evalcond[5]=((((-1.0)*sj3))+((x4205*x4223))+(((-1.0)*x4201*x4202*x4211))+((r12*sj0*x4217))+(((-1.0)*x4209*x4219))+((x4209*x4223))+(((-1.0)*x4205*x4219))+(((-1.0)*x4202*x4221))+((x4208*x4217))+(((-1.0)*x4200*x4204*x4210))+(((-1.0)*x4200*x4204*x4211))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4224=((0.866025403784439)*cj0); +IkReal x4225=(cj3*r21); +IkReal x4226=(cj3*r20); +IkReal x4227=((0.866025403784439)*sj0); +CheckValue x4228 = IKatan2WithCheck(IkReal((((cj5*x4225))+((sj5*x4226)))),IkReal(((((-0.5)*cj5*x4226))+(((0.5)*sj5*x4225))+(((0.866025403784439)*cj3*r22)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4228.valid){ +continue; +} +CheckValue x4229=IKPowWithIntegerCheck(IKsign((((cj5*r10*x4227))+(((0.5)*r12*sj0))+((cj5*r00*x4224))+(((-1.0)*r11*sj5*x4227))+(((-1.0)*r01*sj5*x4224))+(((0.5)*cj0*r02)))),-1); +if(!x4229.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4228.value)+(((1.5707963267949)*(x4229.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4230=IKcos(j4); +IkReal x4231=IKsin(j4); +IkReal x4232=((0.5)*cj5); +IkReal x4233=(r01*sj0); +IkReal x4234=((1.0)*sj5); +IkReal x4235=(r11*sj0); +IkReal x4236=(r02*sj0); +IkReal x4237=(r00*sj0); +IkReal x4238=(cj0*r02); +IkReal x4239=(cj0*r01); +IkReal x4240=(r10*sj0); +IkReal x4241=(cj0*r00); +IkReal x4242=(cj0*r11); +IkReal x4243=(cj0*r10); +IkReal x4244=(sj5*x4230); +IkReal x4245=(r20*x4231); +IkReal x4246=(cj5*x4231); +IkReal x4247=((0.866025403784439)*x4231); +IkReal x4248=((0.866025403784439)*x4230); +IkReal x4249=(cj5*x4230); +IkReal x4250=(x4231*x4243); +IkReal x4251=(x4231*x4240); +IkReal x4252=(r12*x4248); +IkReal x4253=((0.5)*sj5*x4231); +evalcond[0]=(((x4232*x4245))+((r20*x4244))+(((-1.0)*r22*x4247))+((r21*x4249))+(((-1.0)*r21*x4253))); +evalcond[1]=((0.866025403784439)+((r20*x4230*x4232))+(((-0.5)*r21*x4244))+(((-1.0)*r21*x4246))+(((-1.0)*r22*x4248))+(((-1.0)*x4234*x4245))); +evalcond[2]=(((cj0*r12*x4247))+(((-1.0)*x4242*x4249))+cj3+((x4237*x4244))+(((-1.0)*x4233*x4253))+((x4242*x4253))+(((-1.0)*x4236*x4247))+(((-1.0)*x4232*x4250))+(((-1.0)*x4230*x4234*x4243))+((x4231*x4232*x4237))+((x4233*x4249))); +evalcond[3]=((((0.5)*x4239*x4244))+((x4239*x4246))+((x4235*x4246))+((sj5*x4231*x4241))+((x4238*x4248))+(((-1.0)*x4230*x4232*x4241))+(((-1.0)*x4230*x4232*x4240))+(((0.5)*x4235*x4244))+(((0.5)*cj3))+((sj0*x4252))+((sj5*x4251))); +evalcond[4]=(((x4230*x4232*x4237))+((cj0*x4252))+(((0.5)*sj3))+(((-1.0)*x4230*x4232*x4243))+(((-1.0)*x4231*x4234*x4237))+(((-1.0)*x4233*x4246))+(((-0.5)*x4233*x4244))+(((-1.0)*x4236*x4248))+((x4242*x4246))+(((0.5)*x4242*x4244))+((sj5*x4250))); +evalcond[5]=(((r12*sj0*x4247))+(((-1.0)*sj3))+(((-1.0)*x4231*x4232*x4241))+((x4238*x4247))+((x4235*x4253))+(((-1.0)*x4235*x4249))+((x4239*x4253))+(((-1.0)*x4232*x4251))+(((-1.0)*x4230*x4234*x4240))+(((-1.0)*x4230*x4234*x4241))+(((-1.0)*x4239*x4249))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j3, j4] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} +} +} +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x4254=((0.866025403784439)*cj0); +IkReal x4255=(r01*sj5); +IkReal x4256=(cj5*r00); +IkReal x4257=(cj5*r10); +IkReal x4258=((0.866025403784439)*sj0); +IkReal x4259=(r11*sj5); +CheckValue x4260=IKPowWithIntegerCheck(((((-0.866025403784439)*cj1*sj2))+(((0.866025403784439)*cj2*sj1))),-1); +if(!x4260.valid){ +continue; +} +if( IKabs((((cj0*x4257))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*sj0*x4256))+(((-1.0)*cj0*x4259))+((sj0*x4255))+(((0.577350269189626)*cj0*r12)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x4260.value)*(((((0.5)*r12*sj0))+(((-1.0)*x4254*x4255))+(((0.5)*sj1*sj2))+(((-1.0)*x4258*x4259))+(((0.5)*cj0*r02))+((x4257*x4258))+((x4254*x4256))+(((0.5)*cj1*cj2)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*x4257))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*sj0*x4256))+(((-1.0)*cj0*x4259))+((sj0*x4255))+(((0.577350269189626)*cj0*r12))))+IKsqr(((x4260.value)*(((((0.5)*r12*sj0))+(((-1.0)*x4254*x4255))+(((0.5)*sj1*sj2))+(((-1.0)*x4258*x4259))+(((0.5)*cj0*r02))+((x4257*x4258))+((x4254*x4256))+(((0.5)*cj1*cj2))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*x4257))+(((-0.577350269189626)*r02*sj0))+(((-1.0)*sj0*x4256))+(((-1.0)*cj0*x4259))+((sj0*x4255))+(((0.577350269189626)*cj0*r12))), ((x4260.value)*(((((0.5)*r12*sj0))+(((-1.0)*x4254*x4255))+(((0.5)*sj1*sj2))+(((-1.0)*x4258*x4259))+(((0.5)*cj0*r02))+((x4257*x4258))+((x4254*x4256))+(((0.5)*cj1*cj2)))))); +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[3]; +IkReal x4261=IKcos(j3); +IkReal x4262=((0.5)*r12); +IkReal x4263=(cj2*sj1); +IkReal x4264=((0.866025403784439)*sj5); +IkReal x4265=((0.866025403784439)*cj5); +IkReal x4266=(cj1*sj2); +IkReal x4267=(cj1*cj2); +IkReal x4268=((0.5)*r02); +IkReal x4269=(sj1*sj2); +IkReal x4270=((0.866025403784439)*x4261); +evalcond[0]=((((0.5)*x4266))+(((-1.0)*r21*x4264))+(((0.5)*r22))+((r20*x4265))+(((-0.5)*x4263))+(((-1.0)*x4269*x4270))+(((-1.0)*x4267*x4270))); +evalcond[1]=((((-1.0)*cj0*r10*x4265))+(((-1.0)*cj0*x4262))+((cj0*r11*x4264))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r01*sj0*x4264))+((sj0*x4268))+((r00*sj0*x4265))); +evalcond[2]=(((cj0*r01*x4264))+(((-1.0)*r10*sj0*x4265))+((r11*sj0*x4264))+(((-1.0)*cj0*x4268))+((x4263*x4270))+(((-1.0)*sj0*x4262))+(((-1.0)*cj0*r00*x4265))+(((-0.5)*x4267))+(((-0.5)*x4269))+(((-1.0)*x4266*x4270))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[2]; +IkReal x4271=(r12*sj0); +IkReal x4272=(cj0*r02); +IkReal x4273=(cj0*cj5*r00); +IkReal x4274=(r11*sj0*sj5); +IkReal x4275=(cj0*r01*sj5); +IkReal x4276=(cj5*r10*sj0); +j4eval[0]=((((-1.73205080756888)*x4276))+(((-1.73205080756888)*x4273))+(((1.73205080756888)*x4274))+(((1.73205080756888)*x4275))+(((-1.0)*x4272))+(((-1.0)*x4271))); +j4eval[1]=IKsign(((((-0.866025403784439)*x4276))+(((-0.866025403784439)*x4273))+(((-0.5)*x4271))+(((-0.5)*x4272))+(((0.866025403784439)*x4274))+(((0.866025403784439)*x4275)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +IkReal x4277=r20*r20; +IkReal x4278=cj5*cj5; +IkReal x4279=r21*r21; +IkReal x4280=r22*r22; +IkReal x4281=(cj5*r20); +IkReal x4282=(r21*sj5); +IkReal x4283=((3.46410161513775)*r22); +IkReal x4284=((0.866025403784439)*r22); +IkReal x4285=(x4278*x4279); +IkReal x4286=(x4277*x4278); +j4eval[0]=((((-1.0)*x4282*x4283))+(((-6.0)*x4281*x4282))+(((3.0)*x4286))+((x4281*x4283))+(((-3.0)*x4285))+(((-3.0)*x4280))+(((-4.0)*x4277))+(((-1.0)*x4279))); +j4eval[1]=IKsign(((((-1.0)*x4282*x4284))+(((-0.75)*x4285))+(((-0.75)*x4280))+(((-1.5)*x4281*x4282))+((x4281*x4284))+(((-0.25)*x4279))+(((0.75)*x4286))+(((-1.0)*x4277)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +IkReal x4287=((0.866025403784439)*sj5); +IkReal x4288=(r01*sj0); +IkReal x4289=(r02*sj0); +IkReal x4290=(cj0*r11); +IkReal x4291=(cj0*r12); +IkReal x4292=((1.73205080756888)*sj5); +IkReal x4293=(cj0*cj5*r10); +IkReal x4294=(cj5*r00*sj0); +j4eval[0]=((((-1.73205080756888)*x4294))+(((1.73205080756888)*x4293))+((x4288*x4292))+(((-1.0)*x4290*x4292))+x4291+(((-1.0)*x4289))); +j4eval[1]=IKsign(((((-0.866025403784439)*x4294))+((x4287*x4288))+(((0.866025403784439)*x4293))+(((-0.5)*x4289))+(((0.5)*x4291))+(((-1.0)*x4287*x4290)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4295=((1.0)*sj5); +IkReal x4296=((0.866025403784439)*r12); +IkReal x4297=(sj2*sj3); +IkReal x4298=((0.5)*sj5); +IkReal x4299=(cj1*r21); +IkReal x4300=(cj0*sj1); +IkReal x4301=(sj0*sj1); +IkReal x4302=((0.5)*cj5); +IkReal x4303=((0.866025403784439)*r22); +IkReal x4304=((0.866025403784439)*cj0); +IkReal x4305=((1.0)*cj5); +IkReal x4306=(cj1*r20); +IkReal x4307=((0.866025403784439)*sj0); +IkReal x4308=(cj2*sj1*sj3); +IkReal x4309=(cj1*cj2*sj0*sj3); +IkReal x4310=(cj0*cj1*cj2*sj3); +CheckValue x4311=IKPowWithIntegerCheck(IKsign(((((-0.5)*r02*sj0))+(((-1.0)*r11*sj5*x4304))+((cj5*r10*x4304))+(((0.5)*cj0*r12))+(((-1.0)*cj5*r00*x4307))+((r01*sj5*x4307)))),-1); +if(!x4311.valid){ +continue; +} +CheckValue x4312 = IKatan2WithCheck(IkReal(((((-1.0)*r10*x4295*x4309))+(((-1.0)*r00*x4295*x4297*x4300))+(((-1.0)*r01*x4305*x4310))+((r20*sj5*x4308))+(((-1.0)*x4295*x4297*x4306))+(((-1.0)*r10*x4295*x4297*x4301))+(((-1.0)*x4297*x4299*x4305))+(((-1.0)*r00*x4295*x4310))+(((-1.0)*r11*x4305*x4309))+(((-1.0)*r01*x4297*x4300*x4305))+((cj5*r21*x4308))+(((-1.0)*r11*x4297*x4301*x4305)))),IkReal(((((-1.0)*r11*x4298*x4309))+(((-1.0)*r01*x4298*x4310))+(((-1.0)*x4296*x4309))+((x4297*x4302*x4306))+((r10*x4297*x4301*x4302))+(((-1.0)*x4296*x4297*x4301))+(((-1.0)*cj1*cj2*r02*sj3*x4304))+(((-1.0)*r11*x4297*x4298*x4301))+((r00*x4297*x4300*x4302))+(((-1.0)*x4297*x4298*x4299))+(((-1.0)*cj1*x4297*x4303))+(((-0.866025403784439)*r02*x4297*x4300))+(((-1.0)*r01*x4297*x4298*x4300))+(((-1.0)*r20*x4302*x4308))+((r10*x4302*x4309))+((x4303*x4308))+((r21*x4298*x4308))+((r00*x4302*x4310)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4312.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x4311.value)))+(x4312.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4313=IKcos(j4); +IkReal x4314=IKsin(j4); +IkReal x4315=((0.5)*cj5); +IkReal x4316=(r01*sj0); +IkReal x4317=(cj0*r01); +IkReal x4318=(cj2*sj1); +IkReal x4319=(sj2*sj3); +IkReal x4320=((1.0)*sj5); +IkReal x4321=((0.5)*cj3); +IkReal x4322=(r11*sj0); +IkReal x4323=(sj1*sj2); +IkReal x4324=((0.866025403784439)*r02); +IkReal x4325=(cj1*cj2); +IkReal x4326=((1.0)*cj0); +IkReal x4327=(cj1*sj2); +IkReal x4328=(cj0*r11); +IkReal x4329=(sj5*x4313); +IkReal x4330=((0.866025403784439)*cj0*r12); +IkReal x4331=(cj5*x4314); +IkReal x4332=(r20*x4314); +IkReal x4333=(sj0*x4314); +IkReal x4334=((0.866025403784439)*x4313); +IkReal x4335=(cj0*x4314); +IkReal x4336=(cj5*x4313); +IkReal x4337=(sj0*x4313); +IkReal x4338=(cj0*x4313); +IkReal x4339=(r10*x4335); +IkReal x4340=(r10*x4333); +IkReal x4341=((0.5)*sj5*x4314); +evalcond[0]=(((r20*x4329))+((r21*x4336))+((sj3*x4325))+((sj1*x4319))+((x4315*x4332))+(((-0.866025403784439)*r22*x4314))+(((-1.0)*r21*x4341))); +evalcond[1]=((((0.866025403784439)*x4318))+(((-1.0)*x4321*x4325))+(((-1.0)*x4321*x4323))+(((-0.5)*r21*x4329))+(((-1.0)*r22*x4334))+(((-1.0)*x4320*x4332))+(((-0.866025403784439)*x4327))+(((-1.0)*r21*x4331))+((r20*x4313*x4315))); +evalcond[2]=(((r00*sj0*x4329))+(((-1.0)*x4316*x4341))+(((-1.0)*r10*x4320*x4338))+(((-1.0)*x4315*x4339))+cj3+(((-1.0)*x4324*x4333))+((x4314*x4330))+((x4316*x4336))+((x4328*x4341))+((r00*x4315*x4333))+(((-1.0)*r11*x4326*x4336))); +evalcond[3]=(((sj5*x4339))+(((-1.0)*x4324*x4337))+(((-1.0)*r00*x4320*x4333))+(((0.5)*sj3))+(((0.5)*x4328*x4329))+(((-1.0)*r10*x4315*x4338))+((r00*x4315*x4337))+(((-0.5)*x4316*x4329))+(((-1.0)*x4316*x4331))+((x4313*x4330))+((x4328*x4331))); +evalcond[4]=((((-1.0)*x4315*x4340))+(((-1.0)*r10*x4320*x4337))+((cj1*x4319))+((x4324*x4335))+(((-1.0)*r00*x4320*x4338))+(((0.866025403784439)*r12*x4333))+((x4317*x4341))+(((-1.0)*x4317*x4336))+(((-1.0)*sj3*x4318))+(((-1.0)*x4322*x4336))+(((-1.0)*r00*x4315*x4335))+((x4322*x4341))); +evalcond[5]=((((0.5)*x4322*x4329))+((x4324*x4338))+((x4318*x4321))+((x4317*x4331))+((r00*sj5*x4335))+(((-1.0)*r10*x4315*x4337))+(((-1.0)*x4321*x4327))+(((0.866025403784439)*x4325))+(((0.866025403784439)*x4323))+((x4322*x4331))+((r12*sj0*x4334))+(((-1.0)*r00*x4315*x4338))+((sj5*x4340))+(((0.5)*x4317*x4329))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4342=r20*r20; +IkReal x4343=cj5*cj5; +IkReal x4344=r21*r21; +IkReal x4345=((0.5)*cj3); +IkReal x4346=(r20*sj5); +IkReal x4347=((0.866025403784439)*r22); +IkReal x4348=(r21*sj5); +IkReal x4349=(cj2*sj1); +IkReal x4350=((0.75)*r22); +IkReal x4351=(sj1*sj2); +IkReal x4352=(cj1*cj2); +IkReal x4353=((0.5)*sj3); +IkReal x4354=(cj5*r20); +IkReal x4355=(cj1*sj2); +IkReal x4356=((0.25)*cj3); +IkReal x4357=(cj5*r21); +IkReal x4358=((0.75)*x4343); +IkReal x4359=(sj3*x4352); +IkReal x4360=((0.866025403784439)*x4357); +IkReal x4361=((0.433012701892219)*cj3*r22); +IkReal x4362=(x4351*x4357); +IkReal x4363=(x4354*x4356); +CheckValue x4364 = IKatan2WithCheck(IkReal(((((0.866025403784439)*x4346*x4355))+((x4351*x4353*x4354))+(((-1.0)*sj3*x4347*x4351))+(((-1.0)*x4348*x4351*x4353))+((x4355*x4360))+(((-0.866025403784439)*x4346*x4349))+((x4345*x4362))+(((-1.0)*x4348*x4352*x4353))+((x4345*x4352*x4357))+(((-1.0)*x4347*x4359))+((x4345*x4346*x4352))+((x4345*x4346*x4351))+(((-1.0)*x4349*x4360))+((x4352*x4353*x4354)))),IkReal((((x4357*x4359))+((x4351*x4361))+((x4350*x4355))+((x4348*x4352*x4356))+(((0.433012701892219)*x4348*x4355))+((x4348*x4351*x4356))+(((0.433012701892219)*x4349*x4354))+(((-1.0)*x4349*x4350))+((sj3*x4362))+(((-1.0)*x4351*x4363))+(((-0.433012701892219)*x4354*x4355))+((sj3*x4346*x4351))+(((-1.0)*x4352*x4363))+((x4352*x4361))+(((-0.433012701892219)*x4348*x4349))+((x4346*x4359)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4364.valid){ +continue; +} +CheckValue x4365=IKPowWithIntegerCheck(IKsign(((((-1.0)*x4344*x4358))+((x4342*x4358))+((x4347*x4354))+(((-0.25)*x4344))+(((-1.5)*x4346*x4357))+(((-1.0)*x4347*x4348))+(((-1.0)*r22*x4350))+(((-1.0)*x4342)))),-1); +if(!x4365.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4364.value)+(((1.5707963267949)*(x4365.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4366=IKcos(j4); +IkReal x4367=IKsin(j4); +IkReal x4368=((0.5)*cj5); +IkReal x4369=(r01*sj0); +IkReal x4370=(cj0*r01); +IkReal x4371=(cj2*sj1); +IkReal x4372=(sj2*sj3); +IkReal x4373=((1.0)*sj5); +IkReal x4374=((0.5)*cj3); +IkReal x4375=(r11*sj0); +IkReal x4376=(sj1*sj2); +IkReal x4377=((0.866025403784439)*r02); +IkReal x4378=(cj1*cj2); +IkReal x4379=((1.0)*cj0); +IkReal x4380=(cj1*sj2); +IkReal x4381=(cj0*r11); +IkReal x4382=(sj5*x4366); +IkReal x4383=((0.866025403784439)*cj0*r12); +IkReal x4384=(cj5*x4367); +IkReal x4385=(r20*x4367); +IkReal x4386=(sj0*x4367); +IkReal x4387=((0.866025403784439)*x4366); +IkReal x4388=(cj0*x4367); +IkReal x4389=(cj5*x4366); +IkReal x4390=(sj0*x4366); +IkReal x4391=(cj0*x4366); +IkReal x4392=(r10*x4388); +IkReal x4393=(r10*x4386); +IkReal x4394=((0.5)*sj5*x4367); +evalcond[0]=(((sj1*x4372))+((x4368*x4385))+((r21*x4389))+((sj3*x4378))+(((-0.866025403784439)*r22*x4367))+((r20*x4382))+(((-1.0)*r21*x4394))); +evalcond[1]=((((-0.5)*r21*x4382))+(((-1.0)*x4373*x4385))+(((-0.866025403784439)*x4380))+(((0.866025403784439)*x4371))+(((-1.0)*r21*x4384))+(((-1.0)*r22*x4387))+(((-1.0)*x4374*x4378))+(((-1.0)*x4374*x4376))+((r20*x4366*x4368))); +evalcond[2]=((((-1.0)*x4369*x4394))+cj3+(((-1.0)*x4368*x4392))+(((-1.0)*r11*x4379*x4389))+((x4367*x4383))+((x4381*x4394))+((x4369*x4389))+((r00*x4368*x4386))+(((-1.0)*x4377*x4386))+((r00*sj0*x4382))+(((-1.0)*r10*x4373*x4391))); +evalcond[3]=(((sj5*x4392))+((x4366*x4383))+(((-1.0)*x4369*x4384))+(((0.5)*sj3))+(((0.5)*x4381*x4382))+(((-0.5)*x4369*x4382))+((x4381*x4384))+((r00*x4368*x4390))+(((-1.0)*r00*x4373*x4386))+(((-1.0)*x4377*x4390))+(((-1.0)*r10*x4368*x4391))); +evalcond[4]=(((cj1*x4372))+(((-1.0)*x4370*x4389))+(((-1.0)*x4368*x4393))+((x4377*x4388))+(((-1.0)*r00*x4368*x4388))+(((-1.0)*sj3*x4371))+((x4370*x4394))+(((-1.0)*r00*x4373*x4391))+((x4375*x4394))+(((0.866025403784439)*r12*x4386))+(((-1.0)*x4375*x4389))+(((-1.0)*r10*x4373*x4390))); +evalcond[5]=(((x4371*x4374))+((sj5*x4393))+(((0.866025403784439)*x4376))+(((0.866025403784439)*x4378))+((r12*sj0*x4387))+(((0.5)*x4370*x4382))+((x4377*x4391))+((x4370*x4384))+((r00*sj5*x4388))+(((-1.0)*r00*x4368*x4391))+(((0.5)*x4375*x4382))+((x4375*x4384))+(((-1.0)*r10*x4368*x4390))+(((-1.0)*x4374*x4380))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4395=((0.5)*sj0); +IkReal x4396=((0.866025403784439)*sj0); +IkReal x4397=(cj3*r20); +IkReal x4398=((1.0)*sj5); +IkReal x4399=((0.5)*sj5); +IkReal x4400=(cj3*r21); +IkReal x4401=(r01*sj5); +IkReal x4402=((0.866025403784439)*cj0); +IkReal x4403=(cj0*r11); +IkReal x4404=((1.0)*cj5); +IkReal x4405=(cj0*r10); +IkReal x4406=(cj5*r00); +IkReal x4407=(r01*sj0); +IkReal x4408=((0.5)*x4405); +IkReal x4409=(cj1*cj2*sj3); +IkReal x4410=(sj1*sj2*sj3); +IkReal x4411=(r00*sj0*sj5); +IkReal x4412=(cj5*x4410); +CheckValue x4413=IKPowWithIntegerCheck(IKsign(((((-1.0)*r12*x4395))+(((-0.5)*cj0*r02))+((x4401*x4402))+(((-1.0)*cj5*r10*x4396))+(((-1.0)*x4402*x4406))+((r11*sj5*x4396)))),-1); +if(!x4413.valid){ +continue; +} +CheckValue x4414 = IKatan2WithCheck(IkReal(((((-1.0)*x4398*x4405*x4410))+((x4410*x4411))+(((-1.0)*x4398*x4405*x4409))+((cj5*x4407*x4409))+(((-1.0)*x4400*x4404))+((x4407*x4412))+(((-1.0)*x4403*x4404*x4409))+(((-1.0)*x4403*x4404*x4410))+((x4409*x4411))+(((-1.0)*x4397*x4398)))),IkReal((((cj5*x4408*x4409))+((r02*x4396*x4409))+((r02*x4396*x4410))+(((-1.0)*x4399*x4403*x4409))+(((0.5)*cj5*x4397))+(((-1.0)*x4395*x4406*x4409))+(((-1.0)*x4395*x4406*x4410))+(((-1.0)*x4399*x4403*x4410))+((x4395*x4401*x4410))+((x4395*x4401*x4409))+(((-1.0)*x4399*x4400))+(((-0.866025403784439)*cj3*r22))+(((-1.0)*r12*x4402*x4409))+(((-1.0)*r12*x4402*x4410))+((x4408*x4412)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4414.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x4413.value)))+(x4414.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4415=IKcos(j4); +IkReal x4416=IKsin(j4); +IkReal x4417=((0.5)*cj5); +IkReal x4418=(r01*sj0); +IkReal x4419=(cj0*r01); +IkReal x4420=(cj2*sj1); +IkReal x4421=(sj2*sj3); +IkReal x4422=((1.0)*sj5); +IkReal x4423=((0.5)*cj3); +IkReal x4424=(r11*sj0); +IkReal x4425=(sj1*sj2); +IkReal x4426=((0.866025403784439)*r02); +IkReal x4427=(cj1*cj2); +IkReal x4428=((1.0)*cj0); +IkReal x4429=(cj1*sj2); +IkReal x4430=(cj0*r11); +IkReal x4431=(sj5*x4415); +IkReal x4432=((0.866025403784439)*cj0*r12); +IkReal x4433=(cj5*x4416); +IkReal x4434=(r20*x4416); +IkReal x4435=(sj0*x4416); +IkReal x4436=((0.866025403784439)*x4415); +IkReal x4437=(cj0*x4416); +IkReal x4438=(cj5*x4415); +IkReal x4439=(sj0*x4415); +IkReal x4440=(cj0*x4415); +IkReal x4441=(r10*x4437); +IkReal x4442=(r10*x4435); +IkReal x4443=((0.5)*sj5*x4416); +evalcond[0]=(((r20*x4431))+((sj1*x4421))+((x4417*x4434))+((sj3*x4427))+(((-0.866025403784439)*r22*x4416))+(((-1.0)*r21*x4443))+((r21*x4438))); +evalcond[1]=((((-1.0)*x4423*x4427))+(((-1.0)*x4423*x4425))+(((-0.866025403784439)*x4429))+(((-0.5)*r21*x4431))+(((-1.0)*r21*x4433))+((r20*x4415*x4417))+(((-1.0)*r22*x4436))+(((0.866025403784439)*x4420))+(((-1.0)*x4422*x4434))); +evalcond[2]=(((x4430*x4443))+(((-1.0)*x4417*x4441))+cj3+(((-1.0)*x4418*x4443))+((x4416*x4432))+((r00*x4417*x4435))+(((-1.0)*r11*x4428*x4438))+(((-1.0)*r10*x4422*x4440))+(((-1.0)*x4426*x4435))+((x4418*x4438))+((r00*sj0*x4431))); +evalcond[3]=((((-1.0)*r00*x4422*x4435))+(((0.5)*x4430*x4431))+((x4430*x4433))+((sj5*x4441))+(((0.5)*sj3))+(((-1.0)*r10*x4417*x4440))+((r00*x4417*x4439))+(((-1.0)*x4418*x4433))+((x4415*x4432))+(((-1.0)*x4426*x4439))+(((-0.5)*x4418*x4431))); +evalcond[4]=((((-1.0)*x4424*x4438))+((x4419*x4443))+(((-1.0)*x4417*x4442))+((cj1*x4421))+(((0.866025403784439)*r12*x4435))+(((-1.0)*sj3*x4420))+(((-1.0)*r00*x4422*x4440))+(((-1.0)*r00*x4417*x4437))+((x4426*x4437))+(((-1.0)*x4419*x4438))+((x4424*x4443))+(((-1.0)*r10*x4422*x4439))); +evalcond[5]=((((-1.0)*x4423*x4429))+((x4420*x4423))+(((0.5)*x4419*x4431))+((sj5*x4442))+(((-1.0)*r10*x4417*x4439))+((r12*sj0*x4436))+(((0.866025403784439)*x4425))+(((0.866025403784439)*x4427))+(((-1.0)*r00*x4417*x4440))+((r00*sj5*x4437))+((x4424*x4433))+((x4426*x4440))+(((0.5)*x4424*x4431))+((x4419*x4433))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x4444=IKPowWithIntegerCheck(((((-0.866025403784439)*cj1*cj2))+(((-0.866025403784439)*sj1*sj2))),-1); +if(!x4444.valid){ +continue; +} +if( IKabs((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x4444.value)*(((((0.5)*cj2*sj1))+(((-0.5)*cj1*sj2))+(((-0.5)*r22))+(((-0.866025403784439)*cj5*r20))+(((0.866025403784439)*r21*sj5)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))))+IKsqr(((x4444.value)*(((((0.5)*cj2*sj1))+(((-0.5)*cj1*sj2))+(((-0.5)*r22))+(((-0.866025403784439)*cj5*r20))+(((0.866025403784439)*r21*sj5))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((((cj0*cj5*r10))+(((-1.0)*cj5*r00*sj0))+(((-0.577350269189626)*r02*sj0))+(((0.577350269189626)*cj0*r12))+((r01*sj0*sj5))+(((-1.0)*cj0*r11*sj5))), ((x4444.value)*(((((0.5)*cj2*sj1))+(((-0.5)*cj1*sj2))+(((-0.5)*r22))+(((-0.866025403784439)*cj5*r20))+(((0.866025403784439)*r21*sj5)))))); +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[3]; +IkReal x4445=IKcos(j3); +IkReal x4446=((0.5)*r12); +IkReal x4447=(cj2*sj1); +IkReal x4448=((0.866025403784439)*sj5); +IkReal x4449=((0.866025403784439)*cj5); +IkReal x4450=(cj1*sj2); +IkReal x4451=(cj1*cj2); +IkReal x4452=((0.5)*r02); +IkReal x4453=(sj1*sj2); +IkReal x4454=((0.866025403784439)*x4445); +evalcond[0]=((((0.5)*x4450))+((r20*x4449))+(((0.5)*r22))+(((-0.5)*x4447))+(((-1.0)*r21*x4448))+(((-1.0)*x4451*x4454))+(((-1.0)*x4453*x4454))); +evalcond[1]=((((-1.0)*cj0*r10*x4449))+((r00*sj0*x4449))+(((-1.0)*cj0*x4446))+((cj0*r11*x4448))+(((0.866025403784439)*(IKsin(j3))))+(((-1.0)*r01*sj0*x4448))+((sj0*x4452))); +evalcond[2]=((((-1.0)*cj0*x4452))+(((-1.0)*cj0*r00*x4449))+(((-1.0)*sj0*x4446))+(((-1.0)*r10*sj0*x4449))+((cj0*r01*x4448))+(((-0.5)*x4451))+(((-0.5)*x4453))+(((-1.0)*x4450*x4454))+((x4447*x4454))+((r11*sj0*x4448))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j4eval[2]; +IkReal x4455=(r12*sj0); +IkReal x4456=(cj0*r02); +IkReal x4457=(cj0*cj5*r00); +IkReal x4458=(r11*sj0*sj5); +IkReal x4459=(cj0*r01*sj5); +IkReal x4460=(cj5*r10*sj0); +j4eval[0]=((((-1.73205080756888)*x4460))+(((1.73205080756888)*x4458))+(((1.73205080756888)*x4459))+(((-1.0)*x4456))+(((-1.0)*x4455))+(((-1.73205080756888)*x4457))); +j4eval[1]=IKsign(((((0.866025403784439)*x4458))+(((0.866025403784439)*x4459))+(((-0.866025403784439)*x4457))+(((-0.5)*x4455))+(((-0.5)*x4456))+(((-0.866025403784439)*x4460)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +IkReal x4461=r20*r20; +IkReal x4462=cj5*cj5; +IkReal x4463=r21*r21; +IkReal x4464=r22*r22; +IkReal x4465=(cj5*r20); +IkReal x4466=(r21*sj5); +IkReal x4467=((3.46410161513775)*r22); +IkReal x4468=((0.866025403784439)*r22); +IkReal x4469=(x4462*x4463); +IkReal x4470=(x4461*x4462); +j4eval[0]=((((-4.0)*x4461))+(((-1.0)*x4463))+(((-1.0)*x4466*x4467))+(((3.0)*x4470))+(((-3.0)*x4469))+(((-3.0)*x4464))+(((-6.0)*x4465*x4466))+((x4465*x4467))); +j4eval[1]=IKsign(((((0.75)*x4470))+(((-0.25)*x4463))+(((-1.0)*x4461))+(((-1.0)*x4466*x4468))+(((-1.5)*x4465*x4466))+((x4465*x4468))+(((-0.75)*x4469))+(((-0.75)*x4464)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j4eval[2]; +IkReal x4471=((0.866025403784439)*sj5); +IkReal x4472=(r01*sj0); +IkReal x4473=(r02*sj0); +IkReal x4474=(cj0*r11); +IkReal x4475=(cj0*r12); +IkReal x4476=((1.73205080756888)*sj5); +IkReal x4477=(cj0*cj5*r10); +IkReal x4478=(cj5*r00*sj0); +j4eval[0]=(x4475+(((-1.0)*x4474*x4476))+(((-1.73205080756888)*x4478))+((x4472*x4476))+(((1.73205080756888)*x4477))+(((-1.0)*x4473))); +j4eval[1]=IKsign((((x4471*x4472))+(((0.5)*x4475))+(((-0.866025403784439)*x4478))+(((0.866025403784439)*x4477))+(((-1.0)*x4471*x4474))+(((-0.5)*x4473)))); +if( IKabs(j4eval[0]) < 0.0000010000000000 || IKabs(j4eval[1]) < 0.0000010000000000 ) +{ +continue; // no branches [j4] + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4479=((1.0)*sj5); +IkReal x4480=((0.866025403784439)*r12); +IkReal x4481=(sj2*sj3); +IkReal x4482=((0.5)*sj5); +IkReal x4483=(cj1*r21); +IkReal x4484=(cj0*sj1); +IkReal x4485=(sj0*sj1); +IkReal x4486=((0.5)*cj5); +IkReal x4487=((0.866025403784439)*r22); +IkReal x4488=((0.866025403784439)*cj0); +IkReal x4489=((1.0)*cj5); +IkReal x4490=(cj1*r20); +IkReal x4491=((0.866025403784439)*sj0); +IkReal x4492=(cj2*sj1*sj3); +IkReal x4493=(cj1*cj2*sj0*sj3); +IkReal x4494=(cj0*cj1*cj2*sj3); +CheckValue x4495=IKPowWithIntegerCheck(IKsign((((cj5*r10*x4488))+(((-0.5)*r02*sj0))+((r01*sj5*x4491))+(((-1.0)*cj5*r00*x4491))+(((0.5)*cj0*r12))+(((-1.0)*r11*sj5*x4488)))),-1); +if(!x4495.valid){ +continue; +} +CheckValue x4496 = IKatan2WithCheck(IkReal(((((-1.0)*x4479*x4481*x4490))+(((-1.0)*r11*x4481*x4485*x4489))+(((-1.0)*r10*x4479*x4481*x4485))+((cj5*r21*x4492))+((r20*sj5*x4492))+(((-1.0)*r01*x4489*x4494))+(((-1.0)*r00*x4479*x4481*x4484))+(((-1.0)*x4481*x4483*x4489))+(((-1.0)*r11*x4489*x4493))+(((-1.0)*r01*x4481*x4484*x4489))+(((-1.0)*r10*x4479*x4493))+(((-1.0)*r00*x4479*x4494)))),IkReal(((((-1.0)*r11*x4482*x4493))+((r21*x4482*x4492))+((r10*x4481*x4485*x4486))+(((-1.0)*x4480*x4481*x4485))+((x4487*x4492))+(((-1.0)*x4480*x4493))+(((-1.0)*cj1*cj2*r02*sj3*x4488))+(((-0.866025403784439)*r02*x4481*x4484))+((r10*x4486*x4493))+((r00*x4486*x4494))+(((-1.0)*r11*x4481*x4482*x4485))+(((-1.0)*r01*x4481*x4482*x4484))+((r00*x4481*x4484*x4486))+(((-1.0)*r01*x4482*x4494))+(((-1.0)*x4481*x4482*x4483))+(((-1.0)*cj1*x4481*x4487))+(((-1.0)*r20*x4486*x4492))+((x4481*x4486*x4490)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4496.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x4495.value)))+(x4496.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4497=IKcos(j4); +IkReal x4498=IKsin(j4); +IkReal x4499=((0.5)*cj5); +IkReal x4500=(r01*sj0); +IkReal x4501=(cj0*r01); +IkReal x4502=(cj2*sj1); +IkReal x4503=(sj2*sj3); +IkReal x4504=((1.0)*sj5); +IkReal x4505=((0.5)*cj3); +IkReal x4506=(r11*sj0); +IkReal x4507=(sj1*sj2); +IkReal x4508=((0.866025403784439)*r02); +IkReal x4509=(cj1*cj2); +IkReal x4510=((1.0)*cj0); +IkReal x4511=(cj1*sj2); +IkReal x4512=(cj0*r11); +IkReal x4513=(sj5*x4497); +IkReal x4514=((0.866025403784439)*cj0*r12); +IkReal x4515=(cj5*x4498); +IkReal x4516=(r20*x4498); +IkReal x4517=(sj0*x4498); +IkReal x4518=((0.866025403784439)*x4497); +IkReal x4519=(cj0*x4498); +IkReal x4520=(cj5*x4497); +IkReal x4521=(sj0*x4497); +IkReal x4522=(cj0*x4497); +IkReal x4523=(r10*x4519); +IkReal x4524=(r10*x4517); +IkReal x4525=((0.5)*sj5*x4498); +evalcond[0]=(((r21*x4520))+((sj1*x4503))+(((-1.0)*r21*x4525))+((r20*x4513))+((x4499*x4516))+((sj3*x4509))+(((-0.866025403784439)*r22*x4498))); +evalcond[1]=((((-0.5)*r21*x4513))+(((-1.0)*x4504*x4516))+(((-1.0)*r21*x4515))+((r20*x4497*x4499))+(((-0.866025403784439)*x4511))+(((0.866025403784439)*x4502))+(((-1.0)*x4505*x4509))+(((-1.0)*x4505*x4507))+(((-1.0)*r22*x4518))); +evalcond[2]=((((-1.0)*x4500*x4525))+cj3+((r00*sj0*x4513))+(((-1.0)*r11*x4510*x4520))+((x4498*x4514))+(((-1.0)*r10*x4504*x4522))+(((-1.0)*x4499*x4523))+((x4512*x4525))+(((-1.0)*x4508*x4517))+((r00*x4499*x4517))+((x4500*x4520))); +evalcond[3]=((((-1.0)*r10*x4499*x4522))+(((-0.5)*x4500*x4513))+(((0.5)*x4512*x4513))+(((-1.0)*x4508*x4521))+(((0.5)*sj3))+((x4512*x4515))+((r00*x4499*x4521))+(((-1.0)*x4500*x4515))+((x4497*x4514))+(((-1.0)*r00*x4504*x4517))+((sj5*x4523))); +evalcond[4]=((((-1.0)*r00*x4499*x4519))+((x4501*x4525))+(((-1.0)*sj3*x4502))+((x4506*x4525))+(((-1.0)*r00*x4504*x4522))+(((-1.0)*x4501*x4520))+(((-1.0)*r10*x4504*x4521))+(((-1.0)*x4506*x4520))+((cj1*x4503))+(((0.866025403784439)*r12*x4517))+(((-1.0)*x4499*x4524))+((x4508*x4519))); +evalcond[5]=((((-1.0)*r10*x4499*x4521))+((r12*sj0*x4518))+(((-1.0)*x4505*x4511))+(((-1.0)*r00*x4499*x4522))+(((0.5)*x4506*x4513))+((x4508*x4522))+((x4501*x4515))+((r00*sj5*x4519))+(((0.5)*x4501*x4513))+((x4502*x4505))+((x4506*x4515))+(((0.866025403784439)*x4509))+(((0.866025403784439)*x4507))+((sj5*x4524))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4526=r20*r20; +IkReal x4527=cj5*cj5; +IkReal x4528=r21*r21; +IkReal x4529=((0.5)*cj3); +IkReal x4530=(r20*sj5); +IkReal x4531=((0.866025403784439)*r22); +IkReal x4532=(r21*sj5); +IkReal x4533=(cj2*sj1); +IkReal x4534=((0.75)*r22); +IkReal x4535=(sj1*sj2); +IkReal x4536=(cj1*cj2); +IkReal x4537=((0.5)*sj3); +IkReal x4538=(cj5*r20); +IkReal x4539=(cj1*sj2); +IkReal x4540=((0.25)*cj3); +IkReal x4541=(cj5*r21); +IkReal x4542=((0.75)*x4527); +IkReal x4543=(sj3*x4536); +IkReal x4544=((0.866025403784439)*x4541); +IkReal x4545=((0.433012701892219)*cj3*r22); +IkReal x4546=(x4535*x4541); +IkReal x4547=(x4538*x4540); +CheckValue x4548=IKPowWithIntegerCheck(IKsign((((x4531*x4538))+(((-1.0)*x4526))+(((-1.0)*x4531*x4532))+(((-1.5)*x4530*x4541))+((x4526*x4542))+(((-1.0)*r22*x4534))+(((-1.0)*x4528*x4542))+(((-0.25)*x4528)))),-1); +if(!x4548.valid){ +continue; +} +CheckValue x4549 = IKatan2WithCheck(IkReal((((x4535*x4537*x4538))+(((-1.0)*x4533*x4544))+(((-1.0)*x4532*x4535*x4537))+((x4536*x4537*x4538))+(((0.866025403784439)*x4530*x4539))+(((-1.0)*x4532*x4536*x4537))+((x4529*x4536*x4541))+(((-1.0)*x4531*x4543))+((x4529*x4530*x4536))+((x4529*x4530*x4535))+((x4529*x4546))+(((-1.0)*sj3*x4531*x4535))+(((-0.866025403784439)*x4530*x4533))+((x4539*x4544)))),IkReal((((x4532*x4535*x4540))+((x4532*x4536*x4540))+((sj3*x4530*x4535))+(((-0.433012701892219)*x4532*x4533))+(((-1.0)*x4535*x4547))+((sj3*x4546))+(((-1.0)*x4536*x4547))+(((0.433012701892219)*x4532*x4539))+(((-0.433012701892219)*x4538*x4539))+((x4530*x4543))+((x4535*x4545))+(((-1.0)*x4533*x4534))+(((0.433012701892219)*x4533*x4538))+((x4536*x4545))+((x4541*x4543))+((x4534*x4539)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4549.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x4548.value)))+(x4549.value)); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4550=IKcos(j4); +IkReal x4551=IKsin(j4); +IkReal x4552=((0.5)*cj5); +IkReal x4553=(r01*sj0); +IkReal x4554=(cj0*r01); +IkReal x4555=(cj2*sj1); +IkReal x4556=(sj2*sj3); +IkReal x4557=((1.0)*sj5); +IkReal x4558=((0.5)*cj3); +IkReal x4559=(r11*sj0); +IkReal x4560=(sj1*sj2); +IkReal x4561=((0.866025403784439)*r02); +IkReal x4562=(cj1*cj2); +IkReal x4563=((1.0)*cj0); +IkReal x4564=(cj1*sj2); +IkReal x4565=(cj0*r11); +IkReal x4566=(sj5*x4550); +IkReal x4567=((0.866025403784439)*cj0*r12); +IkReal x4568=(cj5*x4551); +IkReal x4569=(r20*x4551); +IkReal x4570=(sj0*x4551); +IkReal x4571=((0.866025403784439)*x4550); +IkReal x4572=(cj0*x4551); +IkReal x4573=(cj5*x4550); +IkReal x4574=(sj0*x4550); +IkReal x4575=(cj0*x4550); +IkReal x4576=(r10*x4572); +IkReal x4577=(r10*x4570); +IkReal x4578=((0.5)*sj5*x4551); +evalcond[0]=(((r21*x4573))+((x4552*x4569))+(((-0.866025403784439)*r22*x4551))+((r20*x4566))+((sj3*x4562))+(((-1.0)*r21*x4578))+((sj1*x4556))); +evalcond[1]=(((r20*x4550*x4552))+(((-0.5)*r21*x4566))+(((0.866025403784439)*x4555))+(((-1.0)*r21*x4568))+(((-1.0)*x4558*x4560))+(((-1.0)*x4558*x4562))+(((-1.0)*r22*x4571))+(((-0.866025403784439)*x4564))+(((-1.0)*x4557*x4569))); +evalcond[2]=(((x4565*x4578))+(((-1.0)*x4553*x4578))+cj3+(((-1.0)*x4552*x4576))+((x4551*x4567))+(((-1.0)*r11*x4563*x4573))+((x4553*x4573))+((r00*sj0*x4566))+(((-1.0)*x4561*x4570))+((r00*x4552*x4570))+(((-1.0)*r10*x4557*x4575))); +evalcond[3]=(((x4565*x4568))+(((-0.5)*x4553*x4566))+(((0.5)*sj3))+(((-1.0)*r10*x4552*x4575))+((x4550*x4567))+((sj5*x4576))+(((0.5)*x4565*x4566))+(((-1.0)*x4553*x4568))+(((-1.0)*r00*x4557*x4570))+(((-1.0)*x4561*x4574))+((r00*x4552*x4574))); +evalcond[4]=((((-1.0)*x4552*x4577))+(((-1.0)*r00*x4552*x4572))+(((-1.0)*sj3*x4555))+(((-1.0)*x4559*x4573))+((x4554*x4578))+((cj1*x4556))+(((0.866025403784439)*r12*x4570))+((x4561*x4572))+((x4559*x4578))+(((-1.0)*x4554*x4573))+(((-1.0)*r00*x4557*x4575))+(((-1.0)*r10*x4557*x4574))); +evalcond[5]=(((r00*sj5*x4572))+(((-1.0)*r00*x4552*x4575))+((x4554*x4568))+((x4555*x4558))+(((-1.0)*r10*x4552*x4574))+(((0.5)*x4559*x4566))+(((0.866025403784439)*x4560))+(((0.866025403784439)*x4562))+((sj5*x4577))+((x4561*x4575))+(((-1.0)*x4558*x4564))+(((0.5)*x4554*x4566))+((r12*sj0*x4571))+((x4559*x4568))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j4array[1], cj4array[1], sj4array[1]; +bool j4valid[1]={false}; +_nj4 = 1; +IkReal x4579=((0.5)*sj0); +IkReal x4580=((0.866025403784439)*sj0); +IkReal x4581=(cj3*r20); +IkReal x4582=((1.0)*sj5); +IkReal x4583=((0.5)*sj5); +IkReal x4584=(cj3*r21); +IkReal x4585=(r01*sj5); +IkReal x4586=((0.866025403784439)*cj0); +IkReal x4587=(cj0*r11); +IkReal x4588=((1.0)*cj5); +IkReal x4589=(cj0*r10); +IkReal x4590=(cj5*r00); +IkReal x4591=(r01*sj0); +IkReal x4592=((0.5)*x4589); +IkReal x4593=(cj1*cj2*sj3); +IkReal x4594=(sj1*sj2*sj3); +IkReal x4595=(r00*sj0*sj5); +IkReal x4596=(cj5*x4594); +CheckValue x4597 = IKatan2WithCheck(IkReal((((x4593*x4595))+((cj5*x4591*x4593))+(((-1.0)*x4582*x4589*x4594))+(((-1.0)*x4582*x4589*x4593))+(((-1.0)*x4581*x4582))+((x4591*x4596))+(((-1.0)*x4587*x4588*x4593))+(((-1.0)*x4587*x4588*x4594))+(((-1.0)*x4584*x4588))+((x4594*x4595)))),IkReal(((((-1.0)*r12*x4586*x4594))+(((-1.0)*r12*x4586*x4593))+((r02*x4580*x4594))+((r02*x4580*x4593))+(((-1.0)*x4583*x4584))+(((-1.0)*x4583*x4587*x4593))+(((-1.0)*x4583*x4587*x4594))+(((-1.0)*x4579*x4590*x4594))+(((-1.0)*x4579*x4590*x4593))+(((0.5)*cj5*x4581))+(((-0.866025403784439)*cj3*r22))+((x4579*x4585*x4593))+((x4579*x4585*x4594))+((cj5*x4592*x4593))+((x4592*x4596)))),IKFAST_ATAN2_MAGTHRESH); +if(!x4597.valid){ +continue; +} +CheckValue x4598=IKPowWithIntegerCheck(IKsign((((r11*sj5*x4580))+(((-0.5)*cj0*r02))+((x4585*x4586))+(((-1.0)*x4586*x4590))+(((-1.0)*r12*x4579))+(((-1.0)*cj5*r10*x4580)))),-1); +if(!x4598.valid){ +continue; +} +j4array[0]=((-1.5707963267949)+(x4597.value)+(((1.5707963267949)*(x4598.value)))); +sj4array[0]=IKsin(j4array[0]); +cj4array[0]=IKcos(j4array[0]); +if( j4array[0] > IKPI ) +{ + j4array[0]-=IK2PI; +} +else if( j4array[0] < -IKPI ) +{ j4array[0]+=IK2PI; +} +j4valid[0] = true; +for(int ij4 = 0; ij4 < 1; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 1; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; +{ +IkReal evalcond[6]; +IkReal x4599=IKcos(j4); +IkReal x4600=IKsin(j4); +IkReal x4601=((0.5)*cj5); +IkReal x4602=(r01*sj0); +IkReal x4603=(cj0*r01); +IkReal x4604=(cj2*sj1); +IkReal x4605=(sj2*sj3); +IkReal x4606=((1.0)*sj5); +IkReal x4607=((0.5)*cj3); +IkReal x4608=(r11*sj0); +IkReal x4609=(sj1*sj2); +IkReal x4610=((0.866025403784439)*r02); +IkReal x4611=(cj1*cj2); +IkReal x4612=((1.0)*cj0); +IkReal x4613=(cj1*sj2); +IkReal x4614=(cj0*r11); +IkReal x4615=(sj5*x4599); +IkReal x4616=((0.866025403784439)*cj0*r12); +IkReal x4617=(cj5*x4600); +IkReal x4618=(r20*x4600); +IkReal x4619=(sj0*x4600); +IkReal x4620=((0.866025403784439)*x4599); +IkReal x4621=(cj0*x4600); +IkReal x4622=(cj5*x4599); +IkReal x4623=(sj0*x4599); +IkReal x4624=(cj0*x4599); +IkReal x4625=(r10*x4621); +IkReal x4626=(r10*x4619); +IkReal x4627=((0.5)*sj5*x4600); +evalcond[0]=(((r20*x4615))+((sj3*x4611))+((x4601*x4618))+(((-0.866025403784439)*r22*x4600))+((sj1*x4605))+(((-1.0)*r21*x4627))+((r21*x4622))); +evalcond[1]=((((-1.0)*x4606*x4618))+(((-1.0)*r22*x4620))+(((-1.0)*r21*x4617))+(((-1.0)*x4607*x4611))+((r20*x4599*x4601))+(((0.866025403784439)*x4604))+(((-0.5)*r21*x4615))+(((-0.866025403784439)*x4613))+(((-1.0)*x4607*x4609))); +evalcond[2]=(((x4602*x4622))+cj3+(((-1.0)*x4601*x4625))+(((-1.0)*x4602*x4627))+((r00*x4601*x4619))+((x4614*x4627))+(((-1.0)*r10*x4606*x4624))+((x4600*x4616))+(((-1.0)*x4610*x4619))+(((-1.0)*r11*x4612*x4622))+((r00*sj0*x4615))); +evalcond[3]=(((r00*x4601*x4623))+(((-1.0)*x4602*x4617))+((sj5*x4625))+(((0.5)*sj3))+(((-0.5)*x4602*x4615))+(((-1.0)*r10*x4601*x4624))+(((0.5)*x4614*x4615))+((x4614*x4617))+(((-1.0)*x4610*x4623))+(((-1.0)*r00*x4606*x4619))+((x4599*x4616))); +evalcond[4]=(((x4603*x4627))+(((-1.0)*x4603*x4622))+(((-1.0)*x4601*x4626))+((x4608*x4627))+(((-1.0)*x4608*x4622))+(((0.866025403784439)*r12*x4619))+(((-1.0)*r00*x4606*x4624))+((x4610*x4621))+((cj1*x4605))+(((-1.0)*sj3*x4604))+(((-1.0)*r10*x4606*x4623))+(((-1.0)*r00*x4601*x4621))); +evalcond[5]=(((x4608*x4617))+(((0.5)*x4603*x4615))+((sj5*x4626))+((x4604*x4607))+((r12*sj0*x4620))+(((-1.0)*x4607*x4613))+(((-1.0)*r10*x4601*x4623))+((x4610*x4624))+(((0.866025403784439)*x4609))+((x4603*x4617))+(((0.5)*x4608*x4615))+((r00*sj5*x4621))+(((0.866025403784439)*x4611))+(((-1.0)*r00*x4601*x4624))); +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 ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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 = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} +} +} +} +} +// [(0, 0), (0, 1), (1, 0), (1, 1), (2, 0), (2, 1), (3, 0), (3, 1)] (original are [(0, 0), (0, 1), (1, 0), (1, 1), (2, 0), (2, 1)]) +static inline bool checkconsistency8(const IkReal* Breal) +{ + IkReal norm = 0.1; + for(int i = 0; i < 7; ++i) { + norm += IKabs(Breal[i]); + } + // HACK should be 1e-5*norm + IkReal tol = 1e-2*norm; // have to increase the threshold since many computations are involved + return IKabs(Breal[0]*Breal[1]-Breal[2]) < tol && IKabs(Breal[1]*Breal[1]-Breal[3]) < tol && IKabs(Breal[0]*Breal[3]-Breal[4]) < tol && IKabs(Breal[1]*Breal[3]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol; +} +/// \brief Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994) +/// +/// matcoeffs is of length 54*3, for 3 matrices +static inline void solvedialyticpoly8qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots) +{ + const IkReal tol = 128.0*std::numeric_limits::epsilon(); + IkReal IKFAST_ALIGNED16(M[16*16]) = {0}; + IkReal IKFAST_ALIGNED16(A[8*8]); + IkReal IKFAST_ALIGNED16(work[16*16*15]); + int ipiv[8]; + int info, coeffindex; + const int worksize=16*16*15; + const int matrixdim = 8; + const int matrixdim2 = 16; + numroots = 0; + // first setup M = [0 I; -C -B] and A + coeffindex = 0; + for(int j = 0; j < 4; ++j) { + for(int k = 0; k < 6; ++k) { + M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -matcoeffs[coeffindex++]; + } + } + for(int j = 0; j < 4; ++j) { + for(int k = 0; k < 6; ++k) { + M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++]; + } + } + for(int j = 0; j < 4; ++j) { + for(int k = 0; k < 6; ++k) { + A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = matcoeffs[coeffindex++]; + } + for(int k = 0; k < 2; ++k) { + A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0; + } + } + const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}}; + int lfindex = -1; + bool bsingular = true; + do { + dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info); + if( info == 0 ) { + bsingular = false; + for(int j = 0; j < matrixdim; ++j) { + if( IKabs(A[j*matrixdim+j]) < 100*tol ) { + bsingular = true; + break; + } + } + if( !bsingular ) { + break; + } + } + if( lfindex == 3 ) { + break; + } + // transform by the linear functional + lfindex++; + const IkReal* lf = lfpossibilities[lfindex]; + // have to reinitialize A + coeffindex = 0; + for(int j = 0; j < 4; ++j) { + for(int k = 0; k < 6; ++k) { + IkReal a = matcoeffs[coeffindex+48], b = matcoeffs[coeffindex+24], c = matcoeffs[coeffindex]; + A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c; + M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c); + M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c); + coeffindex++; + } + for(int k = 0; k < 2; ++k) { + A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0; + } + } + } while(lfindex<4); + + if( bsingular ) { + return; + } + dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info); + if( info != 0 ) { + return; + } + + // set identity in upper corner + for(int j = 0; j < matrixdim; ++j) { + M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1; + } + IkReal IKFAST_ALIGNED16(wr[16]); + IkReal IKFAST_ALIGNED16(wi[16]); + IkReal IKFAST_ALIGNED16(vr[16*16]); + int one=1; + dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info); + if( info != 0 ) { + return; + } + IkReal Breal[matrixdim-1]; + for(int i = 0; i < matrixdim2; ++i) { + // HACK should be tol*100 + if( IKabs(wi[i]) < 5e-5 ) { + IkReal* ev = vr+matrixdim2*i; + if( IKabs(wr[i]) > 1 ) { + ev += matrixdim; + } + // consistency has to be checked!! + if( IKabs(ev[0]) < tol ) { + continue; + } + IkReal iconst = 1/ev[0]; + for(int j = 1; j < matrixdim; ++j) { + Breal[j-1] = ev[j]*iconst; + } + if( checkconsistency8(Breal) ) { + if( lfindex >= 0 ) { + const IkReal* lf = lfpossibilities[lfindex]; + rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]); + } + else { + rawroots[numroots++] = wr[i]; + } + bool bsmall0=IKabs(ev[0]) > IKabs(ev[2]); + bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]); + if( bsmall0 && bsmall1 ) { + rawroots[numroots++] = ev[2]/ev[0]; + rawroots[numroots++] = ev[1]/ev[0]; + } + else if( bsmall0 && !bsmall1 ) { + rawroots[numroots++] = ev[3]/ev[1]; + rawroots[numroots++] = ev[1]/ev[0]; + } + else if( !bsmall0 && bsmall1 ) { + rawroots[numroots++] = ev[6]/ev[4]; + rawroots[numroots++] = ev[7]/ev[6]; + } + else if( !bsmall0 && !bsmall1 ) { + rawroots[numroots++] = ev[7]/ev[5]; + rawroots[numroots++] = ev[7]/ev[6]; + } + } + } + } +}}; + + +/// solves the inverse kinematics equations. +/// \param pfree is an array specifying the free joints of the chain. +IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions) { +IKSolver solver; +return solver.ComputeIk(eetrans,eerot,pfree,solutions); +} + +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 "0x1000004a"; } + +#ifdef IKFAST_NAMESPACE +} // end namespace +#endif + +#ifndef IKFAST_NO_MAIN +#include +#include +#ifdef IKFAST_NAMESPACE +using namespace IKFAST_NAMESPACE; +#endif +int main(int argc, char** argv) +{ + if( argc != 12+GetNumFreeParameters()+1 ) { + printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" + "Returns the ik solutions given the transformation of the end effector specified by\n" + "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" + "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters()); + return 1; + } + + IkSolutionList solutions; + std::vector vfree(GetNumFreeParameters()); + IkReal eerot[9],eetrans[3]; + eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]); + eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]); + eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]); + for(std::size_t i = 0; i < vfree.size(); ++i) + vfree[i] = atof(argv[13+i]); + bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + + if( !bSuccess ) { + fprintf(stderr,"Failed to get ik solution\n"); + return -1; + } + + printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions()); + std::vector solvalues(GetNumJoints()); + for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) { + const IkSolutionBase& sol = solutions.GetSolution(i); + printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); + for( std::size_t j = 0; j < solvalues.size(); ++j) + printf("%.15f, ", solvalues[j]); + printf("\n"); + } + return 0; +} + +#endif