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