Closed mhmiri closed 2 years ago
We just recently released a new version of MoveIt
and srdfdom
, which are not yet synced.
Thus you either need to build srdfdom
from source as well or use the debian packages from the ROS testing
repo.
Please use tripple backticks for multiline logs/code (see my edit of your original description).
Thank you @rhaschke for such a quick response. As I am very new to ROS, i would appreciate a more detailed explanation to what I should do to have IKFast working. I was not able to figure it out from the links you sent me. Also, sure I will use tripple backticks. Thank you
I found the model.h file which contains the necessary functions that don't exits in my current version. Now I'm trying to add those functions in my current model.h version. Would this be alright? Or did you mean that I already have this somewhere and should just build it (which I still dont know what it means)?
Also my current model.h is located at /opt/ros/noetic/include/srdfdom and when I try to change it, it tells me that this file is a read-only file and I have no change permission. can I use chmod +x to add permission??
Thanks again
I manually added the functions(members) from the source that you linked to my own model.h as follows : (Note that I added the following from your link to my own model.h as shown in the multilog bellow:
and added their return variables in the private section at the end of the file:
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. MM
*********************************************************************/
/ Author Ioan Sucan /
/// Main namespace namespace srdf { /* \brief Representation of semantic information about the robot / class Model { public: Model() { }
~Model() { }
/// \brief Load Model from XMLElement bool initXml(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement xml); /// \brief Load Model from XMLDocument bool initXml(const urdf::ModelInterface& urdf_model, tinyxml2::XMLDocument xml); /// \brief Load Model given a filename bool initFile(const urdf::ModelInterface& urdf_model, const std::string& filename); /// \brief Load Model from a XML-string bool initString(const urdf::ModelInterface& urdf_model, const std::string& xmlstring);
/* \brief A group consists of a set of joints and the corresponding descendant links. There are multiple ways to specify a group. Directly specifying joints, links or chains, or referring to other defined groups. / struct Group { /// The name of the group std::string name_;
/// Directly specified joints to be included in the
/// group. Descendent links should be implicitly
/// considered to be part of the group, although this
/// parsed does not add them to links_. The joints are
/// checked to be in the corresponding URDF.
std::vector<std::string> joints_;
/// Directly specified links to be included in the
/// group. Parent joints should be implicitly considered
/// to be part of the group. The links are checked to be
/// in the corresponding URDF.
std::vector<std::string> links_;
/// Specify a chain of links (and the implicit joints) to
/// be added to the group. Each chain is specified as a
/// pair of base link and tip link. It is checked that the
/// chain is indeed a chain in the specified URDF.
std::vector<std::pair<std::string, std::string> > chains_;
/// It is sometimes convenient to refer to the content of
/// another group. A group can include the content of the
/// referenced groups
std::vector<std::string> subgroups_;
};
/// In addition to the joints specified in the URDF it is /// sometimes convenient to add special (virtual) joints. For /// example, to connect the robot to the environment in a /// meaningful way. struct VirtualJoint { /// The name of the new joint std::string name_;
/// The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DOF)
std::string type_;
/// The transform applied by this joint to the robot model brings that model to a particular frame.
std::string parent_frame_;
/// The link this joint applies to
std::string child_link_;
};
/// Representation of an end effector struct EndEffector { /// The name of the end effector std::string name_;
/// The name of the link this end effector connects to
std::string parent_link_;
/// The name of the group to be considered the parent (this group should contain parent_link_)
/// If not specified, this member is empty.
std::string parent_group_;
/// The name of the group that includes the joints & links this end effector consists of
std::string component_group_;
};
/// A named state for a particular group struct GroupState { /// The name of the state std::string name_;
/// The name of the group this state is specified for
std::string group_;
/// The values of joints for this state. Each joint can have a value. We use a vector for the 'value' to support
/// multi-DOF joints
std::map<std::string, std::vector<double> > joint_values_;
};
/// The definition of a sphere struct Sphere { /// The center of the sphere in the link collision frame double centerx; double centery; double centerz;
/// The radius of the sphere
double radius_;
};
/// The definition of a list of spheres for a link. struct LinkSpheres { /// The name of the link (as in URDF). std::string link_;
/// The spheres for the link.
std::vector<Sphere> spheres_;
};
/// The definition of a disabled collision between two links struct DisabledCollision { /// The name of the first link (as in URDF) of the disabled collision std::string link1_;
/// The name of the second link (as in URDF) of the disabled collision
std::string link2_;
/// The reason why the collision check was disabled
std::string reason_;
};
/// MM added - To match model.h @ https://github.com/ros-planning/srdfdom/blob/noetic-devel/include/srdfdom/model.h /// The definition of a disabled/enabled collision between two links struct CollisionPair { /// The name of the first link (as in URDF) of the disabled collision std::string link1_;
/// The name of the second link (as in URDF) of the disabled collision
std::string link2_;
/// The reason why the collision check was disabled/enabled
std::string reason_;
}; /// END of MM added
// Some joints can be passive (not actuated). This structure specifies information about such joints struct PassiveJoint { /// The name of the new joint std::string name_; };
/// Get the name of this model const std::string& getName() const { return name_; }
/// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the
/// geometry and kinematics of the robot)
const std::vector
/// \deprecated{ Use the version returning DisabledCollision } [[deprecated]] std::vector<std::pair<std::string, std::string> > getDisabledCollisions() const;
/// MM added - To match model.h @ https://github.com/ros-planning/srdfdom/blob/noetic-devel/include/srdfdom/model.h
/// Get the list of links that should have collision checking disabled by default (and only selectively enabled)
const std::vector
/// Get the list of pairs of links for which we explicitly re-enable collision (after having disabled it via a default)
const std::vector
// /// Get the list of pairs of links for which we explicitly disable collision
// const std::vector
/// Get the list of groups defined for this model
const std::vector
/// Get the list of virtual joints defined for this model
const std::vector
/// Get the list of end effectors defined for this model
const std::vector
/// Get the list of group states defined for this model
const std::vector
/// Get the list of known passive joints
const std::vector
/// Get the collision spheres list
const std::vector
/// Clear the model void clear();
private: void loadVirtualJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement robot_xml); void loadGroups(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement robot_xml); void loadGroupStates(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement robot_xml); void loadEndEffectors(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement robot_xml); void loadLinkSphereApproximations(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement robot_xml); void loadDisabledCollisions(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement robot_xml); void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
std::string name;
std::vector
/// MM added
std::vector
};
typedef std::shared_ptr
and did catkin_make again and got the following error after 94% of the make completed (so pretty good progress i guess):
[ 94%] Building CXX object moveit/moveit_setup_assistant/CMakeFiles/moveit_setup_assistant_tools.dir/src/tools/collision_linear_model.cpp.o
[ 94%] Building CXX object moveit/moveit_setup_assistant/CMakeFiles/moveit_setup_assistant_tools.dir/src/tools/collision_matrix_model.cpp.o
[ 94%] Building CXX object moveit/moveit_setup_assistant/CMakeFiles/moveit_setup_assistant_tools.dir/src/tools/rotated_header_view.cpp.o
/home/mhmiri/wwrobot_ws_6DOF_moveit/src/moveit/moveit_setup_assistant/src/tools/moveit_config_data.cpp: In member function ‘void moveit_setup_assistant::MoveItConfigData::loadAllowedCollisionMatrix()’:
/home/mhmiri/wwrobot_ws_6DOF_moveit/src/moveit/moveit_setup_assistant/src/tools/moveit_config_data.cpp:146:41: error: ‘using element_type = class srdf::SRDFWriter’ {aka ‘class srdf::SRDFWriter’} has no member named ‘no_default_collisionlinks’
146 | for (const std::string& name : srdf_->no_default_collisionlinks)
| ^~~~~~~
/home/mhmiri/wwrobot_ws_6DOF_moveit/src/moveit/moveit_setup_assistant/src/tools/moveit_config_data.cpp:149:39: error: ‘using element_type = class srdf::SRDFWriter’ {aka ‘class srdf::SRDFWriter’} has no member named ‘enabled_collisionpairs’; did you mean ‘disabledcollisions’?
149 | for (auto const& collision : srdf_->enabled_collisionpairs)
| ^~~~~~~~
| disabledcollisions
/home/mhmiri/wwrobot_ws_6DOF_moveit/src/moveit/moveit_setup_assistant/src/tools/moveit_config_data.cpp:152:39: error: ‘using element_type = class srdf::SRDFWriter’ {aka ‘class srdf::SRDFWriter’} has no member named ‘disabled_collisionpairs’; did you mean ‘disabledcollisions’?
152 | for (auto const& collision : srdf_->disabled_collisionpairs)
| ^~~~~~~~~
| disabledcollisions
make[2]: [moveit/moveit_setup_assistant/CMakeFiles/moveit_setup_assistant_tools.dir/build.make:89: moveit/moveit_setup_assistant/CMakeFiles/moveit_setup_assistant_tools.dir/src/tools/moveit_config_data.cpp.o] Error 1
make[2]: Waiting for unfinished jobs....
make[1]: [CMakeFiles/Makefile2:22955: moveit/moveit_setup_assistant/CMakeFiles/moveit_setup_assistant_tools.dir/all] Error 2
make: [Makefile:141: all] Error 2
Invoking "make -j12 -l12" failed
What else should I do?
Thanks a lot
As I told you before, please build srdfdom
from source, i.e. include the full srdfdom package into your catkin workspace:
cd CATKIN_WS/src
git clone https://github.com/ros-planning/srdfdom
You should never ever modify system-installed files, e.g. in /opt/ros/ or /usr/!
Awesome! I removed all the changes I made to the model.h file and did just as you said with the git clone. Then built the workspace and worked just fine. Thank you very much
I was able to build, but in the next step where I have to create the IKFast plugin using "rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D $MYROBOT_NAME.urdf
~/wwrobot_ws_50_MM_6DOF/src/wwrobot_50$ rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D wwrobot_description/urdf/$MYROBOT_NAME.urdf brush_arm base_body_link brush_hand_pitch_link
Converting urdf to Collada
Building docker image
Sending build context to Docker daemon 2.56kB
Step 1/3 : FROM personalrobotics/ros-openrave
---> 3470beb7a703
Step 2/3 : RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && apt-get update && apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && apt-get clean && rm -rf /var/lib/apt/lists/*
---> Using cache
---> aa8eb41e926a
Step 3/3 : RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1
---> Using cache
---> 6100e2f191a6
Successfully built 6100e2f191a6
Successfully tagged fixed-openrave:latest
Successfully built docker image.
Converting urdf to Collada
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[rospack] Error: package 'wwrobot_description' not found
[librospack]: error while executing command
[ WARN] [1644352951.712886641]: failed to load resource package://wwrobot_description/meshes/base_body_v1.stl
[ WARN] [1644352951.718936528]: failed to load resource package://wwrobot_description/meshes/brush_arm_yaw_v1.stl
[ WARN] [1644352951.724116254]: failed to load resource package://wwrobot_description/meshes/brush_arm_pitch1_v1.stl
[ WARN] [1644352951.729627085]: failed to load resource package://wwrobot_description/meshes/brush_arm_pitch2_v1.stl
[ WARN] [1644352951.735054669]: failed to load resource package://wwrobot_description/meshes/brush_arm_roll_v1.stl
[ WARN] [1644352951.740179113]: failed to load resource package://wwrobot_description/meshes/brush_hand_v1.stl
[ WARN] [1644352951.746008413]: failed to load resource package://wwrobot_description/meshes/brush_hand_pitch_v1.stl
[ WARN] [1644352951.751642351]: failed to load resource package://wwrobot_description/meshes/brush_hand2_v1.stl
[ WARN] [1644352951.757140901]: failed to load resource package://wwrobot_description/meshes/left_upperleg_v1.stl
[ WARN] [1644352951.762382794]: failed to load resource package://wwrobot_description/meshes/left_lowerleg_v1.stl
[ WARN] [1644352951.767769405]: failed to load resource package://wwrobot_description/meshes/left_foot_base_v1.stl
[ WARN] [1644352951.773530961]: failed to load resource package://wwrobot_description/meshes/right_upperleg_v1.stl
[ WARN] [1644352951.779089683]: failed to load resource package://wwrobot_description/meshes/right_lowerleg_v1.stl
[ WARN] [1644352951.784625143]: failed to load resource package://wwrobot_description/meshes/right_foot_base_v1.stl
Document successfully written to robot.dae
Running openrave0.9.py --database inversekinematics --robot=/input/wrapper.xml --iktype=Transform6D --iktests=1000
could not import scipy.optimize.leastsq
2022-02-08 20:42:32,402 openrave [WARN] [plugindatabase.h:577 InterfaceBasePtr OpenRAVE::RaveDatabase::Create] Failed to create name fcl_, interface collisionchecker
openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip wwrobot: Transform6D [8, 9, 10, 11, 12, 13], precision=8, maxcasedepth=3 (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /input/.openrave/kinematics.f1fd94a4a0afedd9455752977ac0f6ea/ikfast0x10000049.Transform6D.8_9_10_11_12_13.cpp
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, -3/40] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 0, 97/500] to left
openravepy.ikfast: generateIkSolver, [[0, 0, 1, -3/8],[0, 1, 0, 0],[-1, 0, 0, -13/100]]
openravepy.ikfast: generateIkSolver, [[cos(j8), -sin(j8), 0, 0],[sin(j8), cos(j8), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 1, 0, -51/200],[0, 0, 1, 0],[1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j9), -sin(j9), 0, 0],[sin(j9), cos(j9), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1562501**(1/2)/1562501, 1250*1562501**(1/2)/1562501, 0, -3124999/6250000],[-1250*1562501**(1/2)/1562501, 1562501**(1/2)/1562501, 0, -4977/12500000],[0, 0, 1, -1/200]]
openravepy.ikfast: generateIkSolver, [[cos(j10), -sin(j10), 0, 0],[sin(j10), cos(j10), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, -1, -43/125],[0, 1, 0, 1/25]]
openravepy.ikfast: generateIkSolver, [[cos(j11), -sin(j11), 0, 0],[sin(j11), cos(j11), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1562501**(1/2)/1562501, 0, -1250*1562501**(1/2)/1562501, 0],[1250*1562501**(1/2)/1562501, 0, 1562501**(1/2)/1562501, 0],[0, -1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j12), -sin(j12), 0, 0],[sin(j12), cos(j12), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 0],[1, 0, 0, -3/100],[0, 1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j13), -sin(j13), 0, 0],[sin(j13), cos(j13), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
openravepy.ikfast: solveFullIK_6D, ikfast 6d: [j8, j9, j10, j11, j12, j13]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[0:5], vars=[j8, j9, j10]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[2:7], vars=[j9, j10, j11]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[4:9], vars=[j10, j11, j12]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[6:11], vars=[j11, j12, j13]
openravepy.ikfast: solveFullIK_6D, try first group 0/4
openravepy.ikfast: buildRaghavanRothEquations, computed in 6.183753s
openravepy.ikfast: buildRaghavanRothEquations, computed in 1.848626s
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, allowed indices: [0]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(1250*1562501**(1/2)/1562501*cj9*cj10*htj8 - 1562501**(1/2)/1562501*cj9*sj10*htj8 - 1562501**(1/2)/1562501*sj9*cj10*htj8 - 1250*1562501**(1/2)/1562501*sj9*sj10*htj8 - r20*cj12*cj13*htj8 + r21*cj12*sj13*htj8 - r22*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r20,r21,r22]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(430*1562501**(1/2)/1562501*cj9*cj10*htj8 - 43*1562501**(1/2)/195312625*cj9*sj10*htj8 + 3124999/6250000*cj9*htj8 - 43*1562501**(1/2)/195312625*sj9*cj10*htj8 - 430*1562501**(1/2)/1562501*sj9*sj10*htj8 - 4977/12500000*sj9*htj8 + 3*r20/100*cj13*htj8 - 3*r21/100*sj13*htj8 + pz*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-430*1562501**(1/2)/1562501*cj9*cj10 + 43*1562501**(1/2)/195312625*cj9*sj10 - 3124999/6250000*cj9 + 43*1562501**(1/2)/195312625*sj9*cj10 + 430*1562501**(1/2)/1562501*sj9*sj10 + 4977/12500000*sj9, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*r20/100*cj13 - 3*r21/100*sj13 + pz, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,pz]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-1562501**(1/2)/1562501*cj9*cj10*htj8**3 + 1562501**(1/2)/1562501*cj9*cj10*htj8 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj8**3 + 1250*1562501**(1/2)/1562501*cj9*sj10*htj8 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj8**3 + 1250*1562501**(1/2)/1562501*sj9*cj10*htj8 + 1562501**(1/2)/1562501*sj9*sj10*htj8**3 - 1562501**(1/2)/1562501*sj9*sj10*htj8 - r00*cj12*cj13*htj8**3 - r00*cj12*cj13*htj8 + r01*cj12*sj13*htj8**3 + r01*cj12*sj13*htj8 - r02*sj12*htj8**3 - r02*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(1562501**(1/2)/1562501*cj9*cj10*htj8**2 - 1562501**(1/2)/1562501*cj9*cj10 + 1250*1562501**(1/2)/1562501*cj9*sj10*htj8**2 - 1250*1562501**(1/2)/1562501*cj9*sj10 + 1250*1562501**(1/2)/1562501*sj9*cj10*htj8**2 - 1250*1562501**(1/2)/1562501*sj9*cj10 - 1562501**(1/2)/1562501*sj9*sj10*htj8**2 + 1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r00*cj12*cj13*htj8**2 - r00*cj12*cj13 + r01*cj12*sj13*htj8**2 + r01*cj12*sj13 - r02*sj12*htj8**2 - r02*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r00,r01,r02]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(2*1562501**(1/2)/1562501*cj9*cj10*htj8**2 + 2500*1562501**(1/2)/1562501*cj9*sj10*htj8**2 + 2500*1562501**(1/2)/1562501*sj9*cj10*htj8**2 - 2*1562501**(1/2)/1562501*sj9*sj10*htj8**2 - r10*cj12*cj13*htj8**3 - r10*cj12*cj13*htj8 + r11*cj12*sj13*htj8**3 + r11*cj12*sj13*htj8 - r12*sj12*htj8**3 - r12*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-2*1562501**(1/2)/1562501*cj9*cj10*htj8 - 2500*1562501**(1/2)/1562501*cj9*sj10*htj8 - 2500*1562501**(1/2)/1562501*sj9*cj10*htj8 + 2*1562501**(1/2)/1562501*sj9*sj10*htj8, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r10*cj12*cj13*htj8**2 - r10*cj12*cj13 + r11*cj12*sj13*htj8**2 + r11*cj12*sj13 - r12*sj12*htj8**2 - r12*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r10,r11,r12]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-43*1562501**(1/2)/195312625*cj9*cj10*htj8**3 + 43*1562501**(1/2)/195312625*cj9*cj10*htj8 - 430*1562501**(1/2)/1562501*cj9*sj10*htj8**3 + 430*1562501**(1/2)/1562501*cj9*sj10*htj8 - 4977/12500000*cj9*htj8**3 + 4977/12500000*cj9*htj8 - 430*1562501**(1/2)/1562501*sj9*cj10*htj8**3 + 430*1562501**(1/2)/1562501*sj9*cj10*htj8 + 43*1562501**(1/2)/195312625*sj9*sj10*htj8**3 - 43*1562501**(1/2)/195312625*sj9*sj10*htj8 - 3124999/6250000*sj9*htj8**3 + 3124999/6250000*sj9*htj8 + 3*r00/100*cj13*htj8**3 + 3*r00/100*cj13*htj8 - 3*r01/100*sj13*htj8**3 - 3*r01/100*sj13*htj8 + (px - 51/200)*htj8**3 + 7/100*htj8**2 + (px + 51/200)*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(86*1562501**(1/2)/195312625*cj9*cj10*htj8**2 + 860*1562501**(1/2)/1562501*cj9*sj10*htj8**2 + 4977/6250000*cj9*htj8**2 + 860*1562501**(1/2)/1562501*sj9*cj10*htj8**2 - 86*1562501**(1/2)/195312625*sj9*sj10*htj8**2 + 3124999/3125000*sj9*htj8**2 + 3*r10/100*cj13*htj8**3 + 3*r10/100*cj13*htj8 - 3*r11/100*sj13*htj8**3 - 3*r11/100*sj13*htj8 + (py + 7/200)*htj8**3 + 51/100*htj8**2 + (py - 7/200)*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r20,r21,r22]')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(1250*1562501**(1/2)/1562501*cj9*cj10*htj8 - 1562501**(1/2)/1562501*cj9*sj10*htj8 - 1562501**(1/2)/1562501*sj9*cj10*htj8 - 1250*1562501**(1/2)/1562501*sj9*sj10*htj8 - r20*cj12*cj13*htj8 + r21*cj12*sj13*htj8 - r22*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r20,r21,r22]'))
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-430*1562501**(1/2)/1562501*cj9*cj10 + 43*1562501**(1/2)/195312625*cj9*sj10 - 3124999/6250000*cj9 + 43*1562501**(1/2)/195312625*sj9*cj10 + 430*1562501**(1/2)/1562501*sj9*sj10 + 4977/12500000*sj9, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*r20/100*cj13 - 3*r21/100*sj13 + pz, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,pz]')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(430*1562501**(1/2)/1562501*cj9*cj10*htj8 - 43*1562501**(1/2)/195312625*cj9*sj10*htj8 + 3124999/6250000*cj9*htj8 - 43*1562501**(1/2)/195312625*sj9*cj10*htj8 - 430*1562501**(1/2)/1562501*sj9*sj10*htj8 - 4977/12500000*sj9*htj8 + 3*r20/100*cj13*htj8 - 3*r21/100*sj13*htj8 + pz*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-430*1562501**(1/2)/1562501*cj9*cj10 + 43*1562501**(1/2)/195312625*cj9*sj10 - 3124999/6250000*cj9 + 43*1562501**(1/2)/195312625*sj9*cj10 + 430*1562501**(1/2)/1562501*sj9*sj10 + 4977/12500000*sj9, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*r20/100*cj13 - 3*r21/100*sj13 + pz, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,pz]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(7*1562501**(1/2)/312500200*cj9*cj10*htj8 + 175*1562501**(1/2)/6250004*cj9*sj10*htj8 + 175*1562501**(1/2)/6250004*sj9*cj10*htj8 - 7*1562501**(1/2)/312500200*sj9*sj10*htj8 - rxp0_2*cj12*cj13*htj8 + rxp1_2*cj12*sj13*htj8 - 3*r21/100*sj12*cj13*htj8 - 3*r20/100*sj12*sj13*htj8 - rxp2_2*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-7*1562501**(1/2)/312500200*cj9*cj10 - 175*1562501**(1/2)/6250004*cj9*sj10 - 175*1562501**(1/2)/6250004*sj9*cj10 + 7*1562501**(1/2)/312500200*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-rxp0_2*cj12*cj13 + rxp1_2*cj12*sj13 - 3*r21/100*sj12*cj13 - 3*r20/100*sj12*sj13 - rxp2_2*sj12, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,rxp0_2,rxp1_2,rxp2_2]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-2193*1562501**(1/2)/19531262500*cj9*cj10*htj8 - 2193*1562501**(1/2)/15625010*cj9*sj10*htj8 - 253827/1250000000*cj9*htj8 - 2193*1562501**(1/2)/15625010*sj9*cj10*htj8 + 2193*1562501**(1/2)/19531262500*sj9*sj10*htj8 - 159374949/625000000*sj9*htj8 - 335937606511*1562501**(1/2)/1220703906250000*cj10*htj8 + 309041*1562501**(1/2)/305175976562500*sj10*htj8 + 3*npx/50*cj13*htj8 - 3*npy/50*sj13*htj8 + (pp - 67763437270533/156250000000000)*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(2193*1562501**(1/2)/19531262500*cj9*cj10 + 2193*1562501**(1/2)/15625010*cj9*sj10 + 253827/1250000000*cj9 + 2193*1562501**(1/2)/15625010*sj9*cj10 - 2193*1562501**(1/2)/19531262500*sj9*sj10 + 159374949/625000000*sj9 + 335937606511*1562501**(1/2)/1220703906250000*cj10 - 309041*1562501**(1/2)/305175976562500*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*npx/50*cj13 - 3*npy/50*sj13 + pp - 67763437270533/156250000000000, cj12, sj12, cj13, sj13, htj8, domain='QQ[npx,npy,pp]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-51*1562501**(1/2)/312500200*cj9*cj10*htj8 - 1275*1562501**(1/2)/6250004*cj9*sj10*htj8 - 1275*1562501**(1/2)/6250004*sj9*cj10*htj8 + 51*1562501**(1/2)/312500200*sj9*sj10*htj8 - 7812502477*1562501**(1/2)/19531262500000*cj10*htj8 + 7187*1562501**(1/2)/4882815625000*sj10*htj8 - npx*cj12*cj13*htj8 + npy*cj12*sj13*htj8 - 3/100*cj12*htj8 - npz*sj12*htj8 - 43/125*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(51*1562501**(1/2)/312500200*cj9*cj10 + 1275*1562501**(1/2)/6250004*cj9*sj10 + 1275*1562501**(1/2)/6250004*sj9*cj10 - 51*1562501**(1/2)/312500200*sj9*sj10 + 7812502477*1562501**(1/2)/19531262500000*cj10 - 7187*1562501**(1/2)/4882815625000*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-npx*cj12*cj13 + npy*cj12*sj13 - 3/100*cj12 - npz*sj12 - 43/125, cj12, sj12, cj13, sj13, htj8, domain='QQ[npx,npy,npz]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-29500585937161921*1562501**(1/2)/122070390625000000000*cj9*cj10*htj8 - 1222588495421*1562501**(1/2)/9765631250000000000*cj9*sj10*htj8 - 134374957/390625000*cj9*htj8 + 1873663495421*1562501**(1/2)/9765631250000000000*sj9*cj10*htj8 - 19327539062161921*1562501**(1/2)/122070390625000000000*sj9*sj10*htj8 + 214011/781250000*sj9*htj8 - 366537*1562501**(1/2)/488281562500000*cj10*htj8 - 398437626327*1562501**(1/2)/1953126250000000*sj10*htj8 + (2*npx*pz - pp*r20 + 9*r20/10000)*cj12*cj13*htj8 + (-2*npy*pz + pp*r21 - 9*r21/10000)*cj12*sj13*htj8 + 3*pz/50*cj12*htj8 + 3*rxp1_2/50*sj12*cj13*htj8 + 3*rxp0_2/50*sj12*sj13*htj8 + (2*npz*pz - pp*r22 - 9*r22/10000)*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(29500585937161921*1562501**(1/2)/122070390625000000000*cj9*cj10 + 1222588495421*1562501**(1/2)/9765631250000000000*cj9*sj10 + 134374957/390625000*cj9 - 1873663495421*1562501**(1/2)/9765631250000000000*sj9*cj10 + 19327539062161921*1562501**(1/2)/122070390625000000000*sj9*sj10 - 214011/781250000*sj9 + 366537*1562501**(1/2)/488281562500000*cj10 + 398437626327*1562501**(1/2)/1953126250000000*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly((2*npx*pz - pp*r20 + 9*r20/10000)*cj12*cj13 + (-2*npy*pz + pp*r21 - 9*r21/10000)*cj12*sj13 + 3*pz/50*cj12 + 3*rxp1_2/50*sj12*cj13 + 3*rxp0_2/50*sj12*sj13 + (2*npz*pz - pp*r22 - 9*r22/10000)*sj12, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,r22,npx,npy,npz,pp,pz,rxp0_2,rxp1_2]'))
openravepy.ikfast: solveLiWoernleHiller, matrix has 0 symbols
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 2, numausymbols=3, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 4, numausymbols=0, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 5, numausymbols=0, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 7, numausymbols=3, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 12, numausymbols=26, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 14, numausymbols=35, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 17, numausymbols=50, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, computed non-singular AU matrix
openravepy.ikfast: solveLiWoernleHiller, reducing 10 equations
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1739), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1794), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1797), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1788), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1847), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (2109), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (2172), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, failed with leftvar htj12: CannotSolveError: solveDialytically: more unknowns than equations 12>10
openravepy.ikfast: solveFullIK_6DGeneral, solving (cj8, sj8, cj9, sj9, cj10, sj10): CannotSolveError: failed to solve dialytically
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, allowed indices: [0]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-r20*cj12*cj13*htj11 + r21*cj12*sj13*htj11 - r22*sj12*htj11 + 1250*1562501**(1/2)/1562501*cj9*cj10*htj11 - 1562501**(1/2)/1562501*cj9*sj10*htj11 - 1562501**(1/2)/1562501*sj9*cj10*htj11 - 1250*1562501**(1/2)/1562501*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(r20*cj12*cj13 - r21*cj12*sj13 + r22*sj12, cj12, sj12, cj13, sj13, htj11, domain='ZZ[r20,r21,r22]'), Poly(1250*1562501**(1/2)/1562501*cj9*cj10 - 1562501**(1/2)/1562501*cj9*sj10 - 1562501**(1/2)/1562501*sj9*cj10 - 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(3*r20/100*cj13*htj11 - 3*r21/100*sj13*htj11 + 430*1562501**(1/2)/1562501*cj9*cj10*htj11 - 43*1562501**(1/2)/195312625*cj9*sj10*htj11 + 3124999/6250000*cj9*htj11 - 43*1562501**(1/2)/195312625*sj9*cj10*htj11 - 430*1562501**(1/2)/1562501*sj9*sj10*htj11 - 4977/12500000*sj9*htj11 + pz*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-3*r20/100*cj13 + 3*r21/100*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21]'), Poly(430*1562501**(1/2)/1562501*cj9*cj10 - 43*1562501**(1/2)/195312625*cj9*sj10 + 3124999/6250000*cj9 - 43*1562501**(1/2)/195312625*sj9*cj10 - 430*1562501**(1/2)/1562501*sj9*sj10 - 4977/12500000*sj9 + pz, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-1250*1562501**(1/2)*r22/1562501*cj12*htj11**3 + 2*1562501**(1/2)*r22/1562501*cj12*htj11**2 + 1250*1562501**(1/2)*r22/1562501*cj12*htj11 + 1250*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**3 - 2*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 - 1250*1562501**(1/2)*r20/1562501*sj12*cj13*htj11 - 1250*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**3 + 2*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 + 1250*1562501**(1/2)*r21/1562501*sj12*sj13*htj11 - 1562501**(1/2)*r21/1562501*cj13*htj11**3 - 2500*1562501**(1/2)*r21/1562501*cj13*htj11**2 + 1562501**(1/2)*r21/1562501*cj13*htj11 - 1562501**(1/2)*r20/1562501*sj13*htj11**3 - 2500*1562501**(1/2)*r20/1562501*sj13*htj11**2 + 1562501**(1/2)*r20/1562501*sj13*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(1250*1562501**(1/2)*r22/1562501*cj12*htj11**2 - 2*1562501**(1/2)*r22/1562501*cj12*htj11 - 1250*1562501**(1/2)*r22/1562501*cj12 - 1250*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 + 2*1562501**(1/2)*r20/1562501*sj12*cj13*htj11 + 1250*1562501**(1/2)*r20/1562501*sj12*cj13 + 1250*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 - 2*1562501**(1/2)*r21/1562501*sj12*sj13*htj11 - 1250*1562501**(1/2)*r21/1562501*sj12*sj13 + 1562501**(1/2)*r21/1562501*cj13*htj11**2 + 2500*1562501**(1/2)*r21/1562501*cj13*htj11 - 1562501**(1/2)*r21/1562501*cj13 + 1562501**(1/2)*r20/1562501*sj13*htj11**2 + 2500*1562501**(1/2)*r20/1562501*sj13*htj11 - 1562501**(1/2)*r20/1562501*sj13, cj12, sj12, cj13, sj13, htj11, domain='EX'), Poly(0, cj9, sj9, cj10, sj10, htj11, domain='ZZ'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-1562501**(1/2)*r22/1562501*cj12*htj11**3 - 2500*1562501**(1/2)*r22/1562501*cj12*htj11**2 + 1562501**(1/2)*r22/1562501*cj12*htj11 + 1562501**(1/2)*r20/1562501*sj12*cj13*htj11**3 + 2500*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 - 1562501**(1/2)*r20/1562501*sj12*cj13*htj11 - 1562501**(1/2)*r21/1562501*sj12*sj13*htj11**3 - 2500*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 + 1562501**(1/2)*r21/1562501*sj12*sj13*htj11 + 1250*1562501**(1/2)*r21/1562501*cj13*htj11**3 - 2*1562501**(1/2)*r21/1562501*cj13*htj11**2 - 1250*1562501**(1/2)*r21/1562501*cj13*htj11 + 1250*1562501**(1/2)*r20/1562501*sj13*htj11**3 - 2*1562501**(1/2)*r20/1562501*sj13*htj11**2 - 1250*1562501**(1/2)*r20/1562501*sj13*htj11 - 1562501**(1/2)/1562501*cj9*cj10*htj11**3 - 1562501**(1/2)/1562501*cj9*cj10*htj11 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj11**3 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj11 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj11**3 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj11 + 1562501**(1/2)/1562501*sj9*sj10*htj11**3 + 1562501**(1/2)/1562501*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(1562501**(1/2)*r22/1562501*cj12*htj11**2 + 2500*1562501**(1/2)*r22/1562501*cj12*htj11 - 1562501**(1/2)*r22/1562501*cj12 - 1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 - 2500*1562501**(1/2)*r20/1562501*sj12*cj13*htj11 + 1562501**(1/2)*r20/1562501*sj12*cj13 + 1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 + 2500*1562501**(1/2)*r21/1562501*sj12*sj13*htj11 - 1562501**(1/2)*r21/1562501*sj12*sj13 - 1250*1562501**(1/2)*r21/1562501*cj13*htj11**2 + 2*1562501**(1/2)*r21/1562501*cj13*htj11 + 1250*1562501**(1/2)*r21/1562501*cj13 - 1250*1562501**(1/2)*r20/1562501*sj13*htj11**2 + 2*1562501**(1/2)*r20/1562501*sj13*htj11 + 1250*1562501**(1/2)*r20/1562501*sj13, cj12, sj12, cj13, sj13, htj11, domain='EX'), Poly(-1562501**(1/2)/1562501*cj9*cj10*htj11**2 - 1562501**(1/2)/1562501*cj9*cj10 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj11**2 - 1250*1562501**(1/2)/1562501*cj9*sj10 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj11**2 - 1250*1562501**(1/2)/1562501*sj9*cj10 + 1562501**(1/2)/1562501*sj9*sj10*htj11**2 + 1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj11, domain='ZZ[r20,r21,r22]'), Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(r20*cj12*cj13*htj11 - r21*cj12*sj13*htj11 + r22*sj12*htj11 - 1250*1562501**(1/2)/1562501*cj9*cj10*htj11 + 1562501**(1/2)/1562501*cj9*sj10*htj11 + 1562501**(1/2)/1562501*sj9*cj10*htj11 + 1250*1562501**(1/2)/1562501*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj11, domain='ZZ[r20,r21,r22]'), Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-npx*cj12*cj13*htj11 + npy*cj12*sj13*htj11 - 3/100*cj12*htj11 - npz*sj12*htj11 - 51*1562501**(1/2)/312500200*cj9*cj10*htj11 - 1275*1562501**(1/2)/6250004*cj9*sj10*htj11 - 1275*1562501**(1/2)/6250004*sj9*cj10*htj11 + 51*1562501**(1/2)/312500200*sj9*sj10*htj11 - 7812502477*1562501**(1/2)/19531262500000*cj10*htj11 + 7187*1562501**(1/2)/4882815625000*sj10*htj11 - 43/125*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(npx*cj12*cj13 - npy*cj12*sj13 + 3/100*cj12 + npz*sj12, cj12, sj12, cj13, sj13, htj11, domain='QQ[npx,npy,npz]'), Poly(-51*1562501**(1/2)/312500200*cj9*cj10 - 1275*1562501**(1/2)/6250004*cj9*sj10 - 1275*1562501**(1/2)/6250004*sj9*cj10 + 51*1562501**(1/2)/312500200*sj9*sj10 - 7812502477*1562501**(1/2)/19531262500000*cj10 + 7187*1562501**(1/2)/4882815625000*sj10 - 43/125, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(rxp0_2*cj12*cj13*htj11 - rxp1_2*cj12*sj13*htj11 + 3*r21/100*sj12*cj13*htj11 + 3*r20/100*sj12*sj13*htj11 + rxp2_2*sj12*htj11 - 7*1562501**(1/2)/312500200*cj9*cj10*htj11 - 175*1562501**(1/2)/6250004*cj9*sj10*htj11 - 175*1562501**(1/2)/6250004*sj9*cj10*htj11 + 7*1562501**(1/2)/312500200*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-rxp0_2*cj12*cj13 + rxp1_2*cj12*sj13 - 3*r21/100*sj12*cj13 - 3*r20/100*sj12*sj13 - rxp2_2*sj12, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21,rxp0_2,rxp1_2,rxp2_2]'), Poly(-7*1562501**(1/2)/312500200*cj9*cj10 - 175*1562501**(1/2)/6250004*cj9*sj10 - 175*1562501**(1/2)/6250004*sj9*cj10 + 7*1562501**(1/2)/312500200*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-3*npx/50*cj13*htj11 + 3*npy/50*sj13*htj11 + 2193*1562501**(1/2)/19531262500*cj9*cj10*htj11 + 2193*1562501**(1/2)/15625010*cj9*sj10*htj11 + 253827/1250000000*cj9*htj11 + 2193*1562501**(1/2)/15625010*sj9*cj10*htj11 - 2193*1562501**(1/2)/19531262500*sj9*sj10*htj11 + 159374949/625000000*sj9*htj11 + 335937606511*1562501**(1/2)/1220703906250000*cj10*htj11 - 309041*1562501**(1/2)/305175976562500*sj10*htj11 + (-pp + 67763437270533/156250000000000)*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(3*npx/50*cj13 - 3*npy/50*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[npx,npy]'), Poly(2193*1562501**(1/2)/19531262500*cj9*cj10 + 2193*1562501**(1/2)/15625010*cj9*sj10 + 253827/1250000000*cj9 + 2193*1562501**(1/2)/15625010*sj9*cj10 - 2193*1562501**(1/2)/19531262500*sj9*sj10 + 159374949/625000000*sj9 + 335937606511*1562501**(1/2)/1220703906250000*cj10 - 309041*1562501**(1/2)/305175976562500*sj10 - pp + 67763437270533/156250000000000, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-3*r20/100*cj13 + 3*r21/100*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[pz,r20,r21]'), Poly(430*1562501**(1/2)/1562501*cj9*cj10 - 43*1562501**(1/2)/195312625*cj9*sj10 + 3124999/6250000*cj9 - 43*1562501**(1/2)/195312625*sj9*cj10 - 430*1562501**(1/2)/1562501*sj9*sj10 - 4977/12500000*sj9 + pz, cj9, sj9, cj10, sj10, htj11, domain='EX')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(3*r20/100*cj13*htj11 - 3*r21/100*sj13*htj11 + 430*1562501**(1/2)/1562501*cj9*cj10*htj11 - 43*1562501**(1/2)/195312625*cj9*sj10*htj11 + 3124999/6250000*cj9*htj11 - 43*1562501**(1/2)/195312625*sj9*cj10*htj11 - 430*1562501**(1/2)/1562501*sj9*sj10*htj11 - 4977/12500000*sj9*htj11 + pz*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-3*r20/100*cj13 + 3*r21/100*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21]'), Poly(430*1562501**(1/2)/1562501*cj9*cj10 - 43*1562501**(1/2)/195312625*cj9*sj10 + 3124999/6250000*cj9 - 43*1562501**(1/2)/195312625*sj9*cj10 - 430*1562501**(1/2)/1562501*sj9*sj10 - 4977/12500000*sj9 + pz, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly((-2*npx*pz + pp*r20 - 9*r20/10000)*cj12*cj13*htj11 + (2*npy*pz - pp*r21 + 9*r21/10000)*cj12*sj13*htj11 - 3*pz/50*cj12*htj11 - 3*rxp1_2/50*sj12*cj13*htj11 - 3*rxp0_2/50*sj12*sj13*htj11 + (-2*npz*pz + pp*r22 + 9*r22/10000)*sj12*htj11 + 29500585937161921*1562501**(1/2)/122070390625000000000*cj9*cj10*htj11 + 1222588495421*1562501**(1/2)/9765631250000000000*cj9*sj10*htj11 + 134374957/390625000*cj9*htj11 - 1873663495421*1562501**(1/2)/9765631250000000000*sj9*cj10*htj11 + 19327539062161921*1562501**(1/2)/122070390625000000000*sj9*sj10*htj11 - 214011/781250000*sj9*htj11 + 366537*1562501**(1/2)/488281562500000*cj10*htj11 + 398437626327*1562501**(1/2)/1953126250000000*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly((2*npx*pz - pp*r20 + 9*r20/10000)*cj12*cj13 + (-2*npy*pz + pp*r21 - 9*r21/10000)*cj12*sj13 + 3*pz/50*cj12 + 3*rxp1_2/50*sj12*cj13 + 3*rxp0_2/50*sj12*sj13 + (2*npz*pz - pp*r22 - 9*r22/10000)*sj12, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21,r22,npx,npy,npz,pp,pz,rxp0_2,rxp1_2]'), Poly(29500585937161921*1562501**(1/2)/122070390625000000000*cj9*cj10 + 1222588495421*1562501**(1/2)/9765631250000000000*cj9*sj10 + 134374957/390625000*cj9 - 1873663495421*1562501**(1/2)/9765631250000000000*sj9*cj10 + 19327539062161921*1562501**(1/2)/122070390625000000000*sj9*sj10 - 214011/781250000*sj9 + 366537*1562501**(1/2)/488281562500000*cj10 + 398437626327*1562501**(1/2)/1953126250000000*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, matrix has 82 symbols
at this point i pressed Ctrl+C to end and gave me the following messages:
^CTraceback (most recent call last):
File "/usr/bin/openrave0.9.py", line 126, in <module>
database.run(args=args)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 1123, in run
InverseKinematicsModel.RunFromParser(*args,**kwargs)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 1088, in RunFromParser
model = DatabaseGenerator.RunFromParser(Model=Model,parser=parser,robotatts=robotatts,args=args,**kwargs)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/__init__.py", line 262, in RunFromParser
model.autogenerate(options=options)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 653, in autogenerate
self.generate(iktype=iktype,freejoints=freejoints,precision=precision,forceikbuild=forceikbuild,outputlang=outputlang,ipython=ipython,ikfastmaxcasedepth=ikfastmaxcasedepth)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 884, in generate
chaintree = solver.generateIkSolver(baselink=baselink,eelink=eelink,freeindices=self.freeindices,solvefn=solvefn)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 2281, in generateIkSolver
chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 746, in solveFullIK_6D
return self.ikfast.IKFastSolver.solveFullIK_6D(*args,**kwargs)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 2936, in solveFullIK_6D
tree = self.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree, usesolvers=1)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 3250, in solveFullIK_6DGeneral
coupledsolutions,usedvars = solvemethod(rawpolyeqs,solvejointvars,endbranchtree=endbranchtree,AllEquationsExtra=AllEquationsExtraPruned)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 4911, in solveLiWoernleHiller
AUdetvalue = AUvalue.det()
File "/usr/local/lib/python2.7/dist-packages/sympy/matrices/matrices.py", line 1840, in det
return self.det_bareis()
File "/usr/local/lib/python2.7/dist-packages/sympy/matrices/matrices.py", line 1892, in det_bareis
M[i, j] = cancel(D)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/polytools.py", line 5220, in cancel
c, P, Q = F.cancel(G)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/polytools.py", line 3022, in cancel
result = F.cancel(G, include=include)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/polyclasses.py", line 630, in cancel
cF, cG, F, G = dmp_cancel(F, G, lev, dom, include=False)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/euclidtools.py", line 1846, in dmp_cancel
_, p, q = dmp_inner_gcd(f, g, u, K)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/euclidtools.py", line 1553, in dmp_inner_gcd
return dup_inner_gcd(f, g, K)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/euclidtools.py", line 1506, in dup_inner_gcd
return dup_zz_heu_gcd(f, g, K)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/euclidtools.py", line 1213, in dup_zz_heu_gcd
h = K.gcd(ff, gg)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/domains/pythonintegerring.py", line 95, in gcd
return python_gcd(a, b)
File "/usr/local/lib/python2.7/dist-packages/sympy/core/numbers.py", line 50, in igcd
a, b = b, a % b
KeyboardInterrupt
docker run --rm --user 1002:1002 -v /tmp/ikfast.fJarO0:/input --workdir /input -e HOME=/input fixed-openrave:latest openrave0.9.py --database inversekinematics --robot=/input/wrapper.xml --iktype=Transform6D --iktests=1000\nfailed with exec code 1:
cat: /tmp/ikfast.fJarO0/ifast.log: No such file or directory
by the way all my packages (including moveit which i installed using the tutorial, git clone https://github.com/ros-planning/moveit.git) are inside the /src/wwrobot_50 of my workspace. Hence ~/my_ws/src/wwrobot_50 is also the directpory which I ran the rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh ... command. But I dont know why it is not able to find the wwrobot_description package even though it is in the smae directory.
Also, do I need to git clone https://github.com/ros-planning/moveit.git at all? because I already have a MoveIt! package created by the Moveit Setup Assistant located in the same directory (/src/wwrobot_50), so do I need another package moveit? My urdf includes a whole robot, but the group which I am including in the command is a 6DOF arm group (by indicating the start anhd end links). Also, I have my mass and length values very precise (some have about 10 decimal places in the urdf file, would this cause a problem?)
Sorry I didnt know if I should continue the question here or create a new issue. Thanks a lot.
Please read the doc: https://ros-planning.github.io/moveit_tutorials/doc/ikfast/ikfast_tutorial.html#tweaking-the-creation-process Looks like you should reduce precision as explained there. The failures to find your robot meshes isn't a problematic (they are warnings only!), because IKFast cares about the kinematics only, but not the geometry.
I did that right now and rounded all numbers in my .dae file to 5 decimal points (using the .py file) and ran the auto_create_ikfast_moveit_plugin.sh again. At first it gave me an error that I have to sepcify --name so I did that as an option as follows and did rosrun: rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D --name wwrobot wwrobot_description/urdf/$MYROBOT_NAME.rounded.dae brush_arm base_body_link brush_hand_pitch_link
rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D --name wwrobot wwrobot_description/urdf/$MYROBOT_NAME.rounded.dae brush_arm base_body_link brush_hand_pitch_link
Building docker image
Sending build context to Docker daemon 24.57MB
Step 1/3 : FROM personalrobotics/ros-openrave
---> 3470beb7a703
Step 2/3 : RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && apt-get update && apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && apt-get clean && rm -rf /var/lib/apt/lists/*
---> Using cache
---> aa8eb41e926a
Step 3/3 : RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1
---> Using cache
---> 6100e2f191a6
Successfully built 6100e2f191a6
Successfully tagged fixed-openrave:latest
Successfully built docker image.
Running openrave0.9.py --database inversekinematics --robot=/input/wrapper.xml --iktype=Transform6D --iktests=1000
could not import scipy.optimize.leastsq
2022-02-08 23:39:40,441 openrave [WARN] [plugindatabase.h:577 InterfaceBasePtr OpenRAVE::RaveDatabase::Create] Failed to create name fcl_, interface collisionchecker
openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip wwrobot: Transform6D [8, 9, 10, 11, 12, 13], precision=8, maxcasedepth=3 (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /input/.openrave/kinematics.f1fd94a4a0afedd9455752977ac0f6ea/ikfast0x10000049.Transform6D.8_9_10_11_12_13.cpp
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, -3/40] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 0, 97/500] to left
openravepy.ikfast: generateIkSolver, [[0, 0, 1, -3/8],[0, 1, 0, 0],[-1, 0, 0, -13/100]]
openravepy.ikfast: generateIkSolver, [[cos(j8), -sin(j8), 0, 0],[sin(j8), cos(j8), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 1, 0, -51/200],[0, 0, 1, 0],[1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j9), -sin(j9), 0, 0],[sin(j9), cos(j9), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1562501**(1/2)/1562501, 1250*1562501**(1/2)/1562501, 0, -3124999/6250000],[-1250*1562501**(1/2)/1562501, 1562501**(1/2)/1562501, 0, -1991/5000000],[0, 0, 1, -1/200]]
openravepy.ikfast: generateIkSolver, [[cos(j10), -sin(j10), 0, 0],[sin(j10), cos(j10), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, -1, -43/125],[0, 1, 0, 1/25]]
openravepy.ikfast: generateIkSolver, [[cos(j11), -sin(j11), 0, 0],[sin(j11), cos(j11), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1562501**(1/2)/1562501, 0, -1250*1562501**(1/2)/1562501, 0],[1250*1562501**(1/2)/1562501, 0, 1562501**(1/2)/1562501, 0],[0, -1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j12), -sin(j12), 0, 0],[sin(j12), cos(j12), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 0],[1, 0, 0, -3/100],[0, 1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j13), -sin(j13), 0, 0],[sin(j13), cos(j13), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
openravepy.ikfast: solveFullIK_6D, ikfast 6d: [j8, j9, j10, j11, j12, j13]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[0:5], vars=[j8, j9, j10]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[2:7], vars=[j9, j10, j11]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[4:9], vars=[j10, j11, j12]
openravepy.ikfast: iterateThreeNonIntersectingAxes, found 3 consecutive non-intersecting axes links[6:11], vars=[j11, j12, j13]
openravepy.ikfast: solveFullIK_6D, try first group 0/4
openravepy.ikfast: buildRaghavanRothEquations, computed in 6.171393s
openravepy.ikfast: buildRaghavanRothEquations, computed in 1.903061s
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, allowed indices: [0]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(1250*1562501**(1/2)/1562501*cj9*cj10*htj8 - 1562501**(1/2)/1562501*cj9*sj10*htj8 - 1562501**(1/2)/1562501*sj9*cj10*htj8 - 1250*1562501**(1/2)/1562501*sj9*sj10*htj8 - r20*cj12*cj13*htj8 + r21*cj12*sj13*htj8 - r22*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r20,r21,r22]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(430*1562501**(1/2)/1562501*cj9*cj10*htj8 - 43*1562501**(1/2)/195312625*cj9*sj10*htj8 + 3124999/6250000*cj9*htj8 - 43*1562501**(1/2)/195312625*sj9*cj10*htj8 - 430*1562501**(1/2)/1562501*sj9*sj10*htj8 - 1991/5000000*sj9*htj8 + 3*r20/100*cj13*htj8 - 3*r21/100*sj13*htj8 + pz*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-430*1562501**(1/2)/1562501*cj9*cj10 + 43*1562501**(1/2)/195312625*cj9*sj10 - 3124999/6250000*cj9 + 43*1562501**(1/2)/195312625*sj9*cj10 + 430*1562501**(1/2)/1562501*sj9*sj10 + 1991/5000000*sj9, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*r20/100*cj13 - 3*r21/100*sj13 + pz, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,pz]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-1562501**(1/2)/1562501*cj9*cj10*htj8**3 + 1562501**(1/2)/1562501*cj9*cj10*htj8 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj8**3 + 1250*1562501**(1/2)/1562501*cj9*sj10*htj8 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj8**3 + 1250*1562501**(1/2)/1562501*sj9*cj10*htj8 + 1562501**(1/2)/1562501*sj9*sj10*htj8**3 - 1562501**(1/2)/1562501*sj9*sj10*htj8 - r00*cj12*cj13*htj8**3 - r00*cj12*cj13*htj8 + r01*cj12*sj13*htj8**3 + r01*cj12*sj13*htj8 - r02*sj12*htj8**3 - r02*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(1562501**(1/2)/1562501*cj9*cj10*htj8**2 - 1562501**(1/2)/1562501*cj9*cj10 + 1250*1562501**(1/2)/1562501*cj9*sj10*htj8**2 - 1250*1562501**(1/2)/1562501*cj9*sj10 + 1250*1562501**(1/2)/1562501*sj9*cj10*htj8**2 - 1250*1562501**(1/2)/1562501*sj9*cj10 - 1562501**(1/2)/1562501*sj9*sj10*htj8**2 + 1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r00*cj12*cj13*htj8**2 - r00*cj12*cj13 + r01*cj12*sj13*htj8**2 + r01*cj12*sj13 - r02*sj12*htj8**2 - r02*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r00,r01,r02]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(2*1562501**(1/2)/1562501*cj9*cj10*htj8**2 + 2500*1562501**(1/2)/1562501*cj9*sj10*htj8**2 + 2500*1562501**(1/2)/1562501*sj9*cj10*htj8**2 - 2*1562501**(1/2)/1562501*sj9*sj10*htj8**2 - r10*cj12*cj13*htj8**3 - r10*cj12*cj13*htj8 + r11*cj12*sj13*htj8**3 + r11*cj12*sj13*htj8 - r12*sj12*htj8**3 - r12*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-2*1562501**(1/2)/1562501*cj9*cj10*htj8 - 2500*1562501**(1/2)/1562501*cj9*sj10*htj8 - 2500*1562501**(1/2)/1562501*sj9*cj10*htj8 + 2*1562501**(1/2)/1562501*sj9*sj10*htj8, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r10*cj12*cj13*htj8**2 - r10*cj12*cj13 + r11*cj12*sj13*htj8**2 + r11*cj12*sj13 - r12*sj12*htj8**2 - r12*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r10,r11,r12]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-43*1562501**(1/2)/195312625*cj9*cj10*htj8**3 + 43*1562501**(1/2)/195312625*cj9*cj10*htj8 - 430*1562501**(1/2)/1562501*cj9*sj10*htj8**3 + 430*1562501**(1/2)/1562501*cj9*sj10*htj8 - 1991/5000000*cj9*htj8**3 + 1991/5000000*cj9*htj8 - 430*1562501**(1/2)/1562501*sj9*cj10*htj8**3 + 430*1562501**(1/2)/1562501*sj9*cj10*htj8 + 43*1562501**(1/2)/195312625*sj9*sj10*htj8**3 - 43*1562501**(1/2)/195312625*sj9*sj10*htj8 - 3124999/6250000*sj9*htj8**3 + 3124999/6250000*sj9*htj8 + 3*r00/100*cj13*htj8**3 + 3*r00/100*cj13*htj8 - 3*r01/100*sj13*htj8**3 - 3*r01/100*sj13*htj8 + (px - 51/200)*htj8**3 + 7/100*htj8**2 + (px + 51/200)*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(86*1562501**(1/2)/195312625*cj9*cj10*htj8**2 + 860*1562501**(1/2)/1562501*cj9*sj10*htj8**2 + 1991/2500000*cj9*htj8**2 + 860*1562501**(1/2)/1562501*sj9*cj10*htj8**2 - 86*1562501**(1/2)/195312625*sj9*sj10*htj8**2 + 3124999/3125000*sj9*htj8**2 + 3*r10/100*cj13*htj8**3 + 3*r10/100*cj13*htj8 - 3*r11/100*sj13*htj8**3 - 3*r11/100*sj13*htj8 + (py + 7/200)*htj8**3 + 51/100*htj8**2 + (py - 7/200)*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r20,r21,r22]')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(1250*1562501**(1/2)/1562501*cj9*cj10*htj8 - 1562501**(1/2)/1562501*cj9*sj10*htj8 - 1562501**(1/2)/1562501*sj9*cj10*htj8 - 1250*1562501**(1/2)/1562501*sj9*sj10*htj8 - r20*cj12*cj13*htj8 + r21*cj12*sj13*htj8 - r22*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj8, domain='ZZ[r20,r21,r22]'))
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-430*1562501**(1/2)/1562501*cj9*cj10 + 43*1562501**(1/2)/195312625*cj9*sj10 - 3124999/6250000*cj9 + 43*1562501**(1/2)/195312625*sj9*cj10 + 430*1562501**(1/2)/1562501*sj9*sj10 + 1991/5000000*sj9, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*r20/100*cj13 - 3*r21/100*sj13 + pz, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,pz]')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(430*1562501**(1/2)/1562501*cj9*cj10*htj8 - 43*1562501**(1/2)/195312625*cj9*sj10*htj8 + 3124999/6250000*cj9*htj8 - 43*1562501**(1/2)/195312625*sj9*cj10*htj8 - 430*1562501**(1/2)/1562501*sj9*sj10*htj8 - 1991/5000000*sj9*htj8 + 3*r20/100*cj13*htj8 - 3*r21/100*sj13*htj8 + pz*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-430*1562501**(1/2)/1562501*cj9*cj10 + 43*1562501**(1/2)/195312625*cj9*sj10 - 3124999/6250000*cj9 + 43*1562501**(1/2)/195312625*sj9*cj10 + 430*1562501**(1/2)/1562501*sj9*sj10 + 1991/5000000*sj9, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*r20/100*cj13 - 3*r21/100*sj13 + pz, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,pz]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(7*1562501**(1/2)/312500200*cj9*cj10*htj8 + 175*1562501**(1/2)/6250004*cj9*sj10*htj8 + 175*1562501**(1/2)/6250004*sj9*cj10*htj8 - 7*1562501**(1/2)/312500200*sj9*sj10*htj8 - rxp0_2*cj12*cj13*htj8 + rxp1_2*cj12*sj13*htj8 - 3*r21/100*sj12*cj13*htj8 - 3*r20/100*sj12*sj13*htj8 - rxp2_2*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-7*1562501**(1/2)/312500200*cj9*cj10 - 175*1562501**(1/2)/6250004*cj9*sj10 - 175*1562501**(1/2)/6250004*sj9*cj10 + 7*1562501**(1/2)/312500200*sj9*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-rxp0_2*cj12*cj13 + rxp1_2*cj12*sj13 - 3*r21/100*sj12*cj13 - 3*r20/100*sj12*sj13 - rxp2_2*sj12, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,rxp0_2,rxp1_2,rxp2_2]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-2193*1562501**(1/2)/19531262500*cj9*cj10*htj8 - 2193*1562501**(1/2)/15625010*cj9*sj10*htj8 - 101541/500000000*cj9*htj8 - 2193*1562501**(1/2)/15625010*sj9*cj10*htj8 + 2193*1562501**(1/2)/19531262500*sj9*sj10*htj8 - 159374949/625000000*sj9*htj8 - 134375042613*1562501**(1/2)/488281562500000*cj10*htj8 + 1209289*1562501**(1/2)/1220703906250000*sj10*htj8 + 3*npx/50*cj13*htj8 - 3*npy/50*sj13*htj8 + (pp - 271053749102041/625000000000000)*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(2193*1562501**(1/2)/19531262500*cj9*cj10 + 2193*1562501**(1/2)/15625010*cj9*sj10 + 101541/500000000*cj9 + 2193*1562501**(1/2)/15625010*sj9*cj10 - 2193*1562501**(1/2)/19531262500*sj9*sj10 + 159374949/625000000*sj9 + 134375042613*1562501**(1/2)/488281562500000*cj10 - 1209289*1562501**(1/2)/1220703906250000*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(3*npx/50*cj13 - 3*npy/50*sj13 + pp - 271053749102041/625000000000000, cj12, sj12, cj13, sj13, htj8, domain='QQ[npx,npy,pp]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-51*1562501**(1/2)/312500200*cj9*cj10*htj8 - 1275*1562501**(1/2)/6250004*cj9*sj10*htj8 - 1275*1562501**(1/2)/6250004*sj9*cj10*htj8 + 51*1562501**(1/2)/312500200*sj9*sj10*htj8 - 3125000991*1562501**(1/2)/7812505000000*cj10*htj8 + 28123*1562501**(1/2)/19531262500000*sj10*htj8 - npx*cj12*cj13*htj8 + npy*cj12*sj13*htj8 - 3/100*cj12*htj8 - npz*sj12*htj8 - 43/125*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(51*1562501**(1/2)/312500200*cj9*cj10 + 1275*1562501**(1/2)/6250004*cj9*sj10 + 1275*1562501**(1/2)/6250004*sj9*cj10 - 51*1562501**(1/2)/312500200*sj9*sj10 + 3125000991*1562501**(1/2)/7812505000000*cj10 - 28123*1562501**(1/2)/19531262500000*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly(-npx*cj12*cj13 + npy*cj12*sj13 - 3/100*cj12 - npz*sj12 - 43/125, cj12, sj12, cj13, sj13, htj8, domain='QQ[npx,npy,npz]'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj8 is non-zero, dividing by Poly(-23600468749740911*1562501**(1/2)/97656312500000000000*cj9*cj10*htj8 - 122290099552009*1562501**(1/2)/976563125000000000000*cj9*sj10*htj8 - 134374957/390625000*cj9*htj8 + 187397599552009*1562501**(1/2)/976563125000000000000*sj9*cj10*htj8 - 15462031249740911*1562501**(1/2)/97656312500000000000*sj9*sj10*htj8 + 85613/312500000*sj9*htj8 - 1434273*1562501**(1/2)/1953126250000000*cj10*htj8 - 159375050541*1562501**(1/2)/781250500000000*sj10*htj8 + (2*npx*pz - pp*r20 + 9*r20/10000)*cj12*cj13*htj8 + (-2*npy*pz + pp*r21 - 9*r21/10000)*cj12*sj13*htj8 + 3*pz/50*cj12*htj8 + 3*rxp1_2/50*sj12*cj13*htj8 + 3*rxp0_2/50*sj12*sj13*htj8 + (2*npz*pz - pp*r22 - 9*r22/10000)*sj12*htj8, cj9, sj9, cj10, sj10, cj12, sj12, cj13, sj13, htj8, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(23600468749740911*1562501**(1/2)/97656312500000000000*cj9*cj10 + 122290099552009*1562501**(1/2)/976563125000000000000*cj9*sj10 + 134374957/390625000*cj9 - 187397599552009*1562501**(1/2)/976563125000000000000*sj9*cj10 + 15462031249740911*1562501**(1/2)/97656312500000000000*sj9*sj10 - 85613/312500000*sj9 + 1434273*1562501**(1/2)/1953126250000000*cj10 + 159375050541*1562501**(1/2)/781250500000000*sj10, cj9, sj9, cj10, sj10, htj8, domain='EX'), Poly((2*npx*pz - pp*r20 + 9*r20/10000)*cj12*cj13 + (-2*npy*pz + pp*r21 - 9*r21/10000)*cj12*sj13 + 3*pz/50*cj12 + 3*rxp1_2/50*sj12*cj13 + 3*rxp0_2/50*sj12*sj13 + (2*npz*pz - pp*r22 - 9*r22/10000)*sj12, cj12, sj12, cj13, sj13, htj8, domain='QQ[r20,r21,r22,npx,npy,npz,pp,pz,rxp0_2,rxp1_2]'))
openravepy.ikfast: solveLiWoernleHiller, matrix has 0 symbols
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 2, numausymbols=3, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 4, numausymbols=0, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 5, numausymbols=0, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 7, numausymbols=3, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 12, numausymbols=26, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 14, numausymbols=35, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, skipping dependent index 17, numausymbols=51, numausymbols=0
openravepy.ikfast: solveLiWoernleHiller, computed non-singular AU matrix
openravepy.ikfast: solveLiWoernleHiller, reducing 10 equations
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1739), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1794), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1797), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1788), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (1847), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (2109), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (2172), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, failed with leftvar htj12: CannotSolveError: solveDialytically: more unknowns than equations 12>10
openravepy.ikfast: solveFullIK_6DGeneral, solving (cj8, sj8, cj9, sj9, cj10, sj10): CannotSolveError: failed to solve dialytically
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, allowed indices: [0]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-r20*cj12*cj13*htj11 + r21*cj12*sj13*htj11 - r22*sj12*htj11 + 1250*1562501**(1/2)/1562501*cj9*cj10*htj11 - 1562501**(1/2)/1562501*cj9*sj10*htj11 - 1562501**(1/2)/1562501*sj9*cj10*htj11 - 1250*1562501**(1/2)/1562501*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(r20*cj12*cj13 - r21*cj12*sj13 + r22*sj12, cj12, sj12, cj13, sj13, htj11, domain='ZZ[r20,r21,r22]'), Poly(1250*1562501**(1/2)/1562501*cj9*cj10 - 1562501**(1/2)/1562501*cj9*sj10 - 1562501**(1/2)/1562501*sj9*cj10 - 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(3*r20/100*cj13*htj11 - 3*r21/100*sj13*htj11 + 430*1562501**(1/2)/1562501*cj9*cj10*htj11 - 43*1562501**(1/2)/195312625*cj9*sj10*htj11 + 3124999/6250000*cj9*htj11 - 43*1562501**(1/2)/195312625*sj9*cj10*htj11 - 430*1562501**(1/2)/1562501*sj9*sj10*htj11 - 1991/5000000*sj9*htj11 + pz*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-3*r20/100*cj13 + 3*r21/100*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21]'), Poly(430*1562501**(1/2)/1562501*cj9*cj10 - 43*1562501**(1/2)/195312625*cj9*sj10 + 3124999/6250000*cj9 - 43*1562501**(1/2)/195312625*sj9*cj10 - 430*1562501**(1/2)/1562501*sj9*sj10 - 1991/5000000*sj9 + pz, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-1250*1562501**(1/2)*r22/1562501*cj12*htj11**3 + 2*1562501**(1/2)*r22/1562501*cj12*htj11**2 + 1250*1562501**(1/2)*r22/1562501*cj12*htj11 + 1250*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**3 - 2*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 - 1250*1562501**(1/2)*r20/1562501*sj12*cj13*htj11 - 1250*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**3 + 2*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 + 1250*1562501**(1/2)*r21/1562501*sj12*sj13*htj11 - 1562501**(1/2)*r21/1562501*cj13*htj11**3 - 2500*1562501**(1/2)*r21/1562501*cj13*htj11**2 + 1562501**(1/2)*r21/1562501*cj13*htj11 - 1562501**(1/2)*r20/1562501*sj13*htj11**3 - 2500*1562501**(1/2)*r20/1562501*sj13*htj11**2 + 1562501**(1/2)*r20/1562501*sj13*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(1250*1562501**(1/2)*r22/1562501*cj12*htj11**2 - 2*1562501**(1/2)*r22/1562501*cj12*htj11 - 1250*1562501**(1/2)*r22/1562501*cj12 - 1250*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 + 2*1562501**(1/2)*r20/1562501*sj12*cj13*htj11 + 1250*1562501**(1/2)*r20/1562501*sj12*cj13 + 1250*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 - 2*1562501**(1/2)*r21/1562501*sj12*sj13*htj11 - 1250*1562501**(1/2)*r21/1562501*sj12*sj13 + 1562501**(1/2)*r21/1562501*cj13*htj11**2 + 2500*1562501**(1/2)*r21/1562501*cj13*htj11 - 1562501**(1/2)*r21/1562501*cj13 + 1562501**(1/2)*r20/1562501*sj13*htj11**2 + 2500*1562501**(1/2)*r20/1562501*sj13*htj11 - 1562501**(1/2)*r20/1562501*sj13, cj12, sj12, cj13, sj13, htj11, domain='EX'), Poly(0, cj9, sj9, cj10, sj10, htj11, domain='ZZ'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-1562501**(1/2)*r22/1562501*cj12*htj11**3 - 2500*1562501**(1/2)*r22/1562501*cj12*htj11**2 + 1562501**(1/2)*r22/1562501*cj12*htj11 + 1562501**(1/2)*r20/1562501*sj12*cj13*htj11**3 + 2500*1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 - 1562501**(1/2)*r20/1562501*sj12*cj13*htj11 - 1562501**(1/2)*r21/1562501*sj12*sj13*htj11**3 - 2500*1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 + 1562501**(1/2)*r21/1562501*sj12*sj13*htj11 + 1250*1562501**(1/2)*r21/1562501*cj13*htj11**3 - 2*1562501**(1/2)*r21/1562501*cj13*htj11**2 - 1250*1562501**(1/2)*r21/1562501*cj13*htj11 + 1250*1562501**(1/2)*r20/1562501*sj13*htj11**3 - 2*1562501**(1/2)*r20/1562501*sj13*htj11**2 - 1250*1562501**(1/2)*r20/1562501*sj13*htj11 - 1562501**(1/2)/1562501*cj9*cj10*htj11**3 - 1562501**(1/2)/1562501*cj9*cj10*htj11 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj11**3 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj11 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj11**3 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj11 + 1562501**(1/2)/1562501*sj9*sj10*htj11**3 + 1562501**(1/2)/1562501*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(1562501**(1/2)*r22/1562501*cj12*htj11**2 + 2500*1562501**(1/2)*r22/1562501*cj12*htj11 - 1562501**(1/2)*r22/1562501*cj12 - 1562501**(1/2)*r20/1562501*sj12*cj13*htj11**2 - 2500*1562501**(1/2)*r20/1562501*sj12*cj13*htj11 + 1562501**(1/2)*r20/1562501*sj12*cj13 + 1562501**(1/2)*r21/1562501*sj12*sj13*htj11**2 + 2500*1562501**(1/2)*r21/1562501*sj12*sj13*htj11 - 1562501**(1/2)*r21/1562501*sj12*sj13 - 1250*1562501**(1/2)*r21/1562501*cj13*htj11**2 + 2*1562501**(1/2)*r21/1562501*cj13*htj11 + 1250*1562501**(1/2)*r21/1562501*cj13 - 1250*1562501**(1/2)*r20/1562501*sj13*htj11**2 + 2*1562501**(1/2)*r20/1562501*sj13*htj11 + 1250*1562501**(1/2)*r20/1562501*sj13, cj12, sj12, cj13, sj13, htj11, domain='EX'), Poly(-1562501**(1/2)/1562501*cj9*cj10*htj11**2 - 1562501**(1/2)/1562501*cj9*cj10 - 1250*1562501**(1/2)/1562501*cj9*sj10*htj11**2 - 1250*1562501**(1/2)/1562501*cj9*sj10 - 1250*1562501**(1/2)/1562501*sj9*cj10*htj11**2 - 1250*1562501**(1/2)/1562501*sj9*cj10 + 1562501**(1/2)/1562501*sj9*sj10*htj11**2 + 1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj11, domain='ZZ[r20,r21,r22]'), Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(r20*cj12*cj13*htj11 - r21*cj12*sj13*htj11 + r22*sj12*htj11 - 1250*1562501**(1/2)/1562501*cj9*cj10*htj11 + 1562501**(1/2)/1562501*cj9*sj10*htj11 + 1562501**(1/2)/1562501*sj9*cj10*htj11 + 1250*1562501**(1/2)/1562501*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-r20*cj12*cj13 + r21*cj12*sj13 - r22*sj12, cj12, sj12, cj13, sj13, htj11, domain='ZZ[r20,r21,r22]'), Poly(-1250*1562501**(1/2)/1562501*cj9*cj10 + 1562501**(1/2)/1562501*cj9*sj10 + 1562501**(1/2)/1562501*sj9*cj10 + 1250*1562501**(1/2)/1562501*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-npx*cj12*cj13*htj11 + npy*cj12*sj13*htj11 - 3/100*cj12*htj11 - npz*sj12*htj11 - 51*1562501**(1/2)/312500200*cj9*cj10*htj11 - 1275*1562501**(1/2)/6250004*cj9*sj10*htj11 - 1275*1562501**(1/2)/6250004*sj9*cj10*htj11 + 51*1562501**(1/2)/312500200*sj9*sj10*htj11 - 3125000991*1562501**(1/2)/7812505000000*cj10*htj11 + 28123*1562501**(1/2)/19531262500000*sj10*htj11 - 43/125*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(npx*cj12*cj13 - npy*cj12*sj13 + 3/100*cj12 + npz*sj12, cj12, sj12, cj13, sj13, htj11, domain='QQ[npx,npy,npz]'), Poly(-51*1562501**(1/2)/312500200*cj9*cj10 - 1275*1562501**(1/2)/6250004*cj9*sj10 - 1275*1562501**(1/2)/6250004*sj9*cj10 + 51*1562501**(1/2)/312500200*sj9*sj10 - 3125000991*1562501**(1/2)/7812505000000*cj10 + 28123*1562501**(1/2)/19531262500000*sj10 - 43/125, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(rxp0_2*cj12*cj13*htj11 - rxp1_2*cj12*sj13*htj11 + 3*r21/100*sj12*cj13*htj11 + 3*r20/100*sj12*sj13*htj11 + rxp2_2*sj12*htj11 - 7*1562501**(1/2)/312500200*cj9*cj10*htj11 - 175*1562501**(1/2)/6250004*cj9*sj10*htj11 - 175*1562501**(1/2)/6250004*sj9*cj10*htj11 + 7*1562501**(1/2)/312500200*sj9*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-rxp0_2*cj12*cj13 + rxp1_2*cj12*sj13 - 3*r21/100*sj12*cj13 - 3*r20/100*sj12*sj13 - rxp2_2*sj12, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21,rxp0_2,rxp1_2,rxp2_2]'), Poly(-7*1562501**(1/2)/312500200*cj9*cj10 - 175*1562501**(1/2)/6250004*cj9*sj10 - 175*1562501**(1/2)/6250004*sj9*cj10 + 7*1562501**(1/2)/312500200*sj9*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(-3*npx/50*cj13*htj11 + 3*npy/50*sj13*htj11 + 2193*1562501**(1/2)/19531262500*cj9*cj10*htj11 + 2193*1562501**(1/2)/15625010*cj9*sj10*htj11 + 101541/500000000*cj9*htj11 + 2193*1562501**(1/2)/15625010*sj9*cj10*htj11 - 2193*1562501**(1/2)/19531262500*sj9*sj10*htj11 + 159374949/625000000*sj9*htj11 + 134375042613*1562501**(1/2)/488281562500000*cj10*htj11 - 1209289*1562501**(1/2)/1220703906250000*sj10*htj11 + (-pp + 271053749102041/625000000000000)*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(3*npx/50*cj13 - 3*npy/50*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[npx,npy]'), Poly(2193*1562501**(1/2)/19531262500*cj9*cj10 + 2193*1562501**(1/2)/15625010*cj9*sj10 + 101541/500000000*cj9 + 2193*1562501**(1/2)/15625010*sj9*cj10 - 2193*1562501**(1/2)/19531262500*sj9*sj10 + 159374949/625000000*sj9 + 134375042613*1562501**(1/2)/488281562500000*cj10 - 1209289*1562501**(1/2)/1220703906250000*sj10 - pp + 271053749102041/625000000000000, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, not unique: [Poly(-3*r20/100*cj13 + 3*r21/100*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[pz,r20,r21]'), Poly(430*1562501**(1/2)/1562501*cj9*cj10 - 43*1562501**(1/2)/195312625*cj9*sj10 + 3124999/6250000*cj9 - 43*1562501**(1/2)/195312625*sj9*cj10 - 430*1562501**(1/2)/1562501*sj9*sj10 - 1991/5000000*sj9 + pz, cj9, sj9, cj10, sj10, htj11, domain='EX')]
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly(3*r20/100*cj13*htj11 - 3*r21/100*sj13*htj11 + 430*1562501**(1/2)/1562501*cj9*cj10*htj11 - 43*1562501**(1/2)/195312625*cj9*sj10*htj11 + 3124999/6250000*cj9*htj11 - 43*1562501**(1/2)/195312625*sj9*cj10*htj11 - 430*1562501**(1/2)/1562501*sj9*sj10*htj11 - 1991/5000000*sj9*htj11 + pz*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly(-3*r20/100*cj13 + 3*r21/100*sj13, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21]'), Poly(430*1562501**(1/2)/1562501*cj9*cj10 - 43*1562501**(1/2)/195312625*cj9*sj10 + 3124999/6250000*cj9 - 43*1562501**(1/2)/195312625*sj9*cj10 - 430*1562501**(1/2)/1562501*sj9*sj10 - 1991/5000000*sj9 + pz, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, assuming equation htj11 is non-zero, dividing by Poly((-2*npx*pz + pp*r20 - 9*r20/10000)*cj12*cj13*htj11 + (2*npy*pz - pp*r21 + 9*r21/10000)*cj12*sj13*htj11 - 3*pz/50*cj12*htj11 - 3*rxp1_2/50*sj12*cj13*htj11 - 3*rxp0_2/50*sj12*sj13*htj11 + (-2*npz*pz + pp*r22 + 9*r22/10000)*sj12*htj11 + 23600468749740911*1562501**(1/2)/97656312500000000000*cj9*cj10*htj11 + 122290099552009*1562501**(1/2)/976563125000000000000*cj9*sj10*htj11 + 134374957/390625000*cj9*htj11 - 187397599552009*1562501**(1/2)/976563125000000000000*sj9*cj10*htj11 + 15462031249740911*1562501**(1/2)/97656312500000000000*sj9*sj10*htj11 - 85613/312500000*sj9*htj11 + 1434273*1562501**(1/2)/1953126250000000*cj10*htj11 + 159375050541*1562501**(1/2)/781250500000000*sj10*htj11, cj12, sj12, cj13, sj13, cj9, sj9, cj10, sj10, htj11, domain='EX')
openravepy.ikfast: solveLiWoernleHiller, not unique: (Poly((2*npx*pz - pp*r20 + 9*r20/10000)*cj12*cj13 + (-2*npy*pz + pp*r21 - 9*r21/10000)*cj12*sj13 + 3*pz/50*cj12 + 3*rxp1_2/50*sj12*cj13 + 3*rxp0_2/50*sj12*sj13 + (2*npz*pz - pp*r22 - 9*r22/10000)*sj12, cj12, sj12, cj13, sj13, htj11, domain='QQ[r20,r21,r22,npx,npy,npz,pp,pz,rxp0_2,rxp1_2]'), Poly(23600468749740911*1562501**(1/2)/97656312500000000000*cj9*cj10 + 122290099552009*1562501**(1/2)/976563125000000000000*cj9*sj10 + 134374957/390625000*cj9 - 187397599552009*1562501**(1/2)/976563125000000000000*sj9*cj10 + 15462031249740911*1562501**(1/2)/97656312500000000000*sj9*sj10 - 85613/312500000*sj9 + 1434273*1562501**(1/2)/1953126250000000*cj10 + 159375050541*1562501**(1/2)/781250500000000*sj10, cj9, sj9, cj10, sj10, htj11, domain='EX'))
openravepy.ikfast: solveLiWoernleHiller, matrix has 82 symbols
at some point as you can see above, an error message is sent which says that the solver cannot solve it, but it keeps going (I copied it again bellow to find it easier):
...
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (2109), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, equation way too complex (2172), looking for another solution
openravepy.ikfast: solveLiWoernleHiller, failed with leftvar htj12: CannotSolveError: solveDialytically: more unknowns than equations 12>10
openravepy.ikfast: solveFullIK_6DGeneral, solving (cj8, sj8, cj9, sj9, cj10, sj10): CannotSolveError: failed to solve dialytically
...
even though i get this error, should I continue waiting, or should I just end the program? Right now its been more than 13 min that I have called the rosrun and am still waiting at the last line (openravepy.ikfast: solveLiWoernleHiller, matrix has 82 symbols). Is this natural? should I wait more?
Thank you very much @rhaschke
Looks like IKFast cannot solve your kinematics. This solver relies on some common geometric constraints, like
If that's not the case for your robot, IKFast won't find a solution.
oh i see, i do not have any of those. But I think I am able to make a little change to qualify for 3 consecutive axes intersecting. Does it need to be intersecting in n exact location, or approximate is fine?
Thank you very much @rhaschke
@rhaschke So I changed my .urdf file in order to have 3 conservative joints intersecting, and apparently now it can detect it. However, after about 1 min, it gives me the following error and it ends he program:
rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D --name wwrobot wwrobot_description/urdf/$MYROBOT_NAME.rounded.dae brush_arm base_body_link brush_hand_pitch_link
Building docker image
Sending build context to Docker daemon 24.57MB
Step 1/3 : FROM personalrobotics/ros-openrave
---> 3470beb7a703
Step 2/3 : RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && apt-get update && apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && apt-get clean && rm -rf /var/lib/apt/lists/*
---> Using cache
---> aa8eb41e926a
Step 3/3 : RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1
---> Using cache
---> 6100e2f191a6
Successfully built 6100e2f191a6
Successfully tagged fixed-openrave:latest
Successfully built docker image.
Running openrave0.9.py --database inversekinematics --robot=/input/wrapper.xml --iktype=Transform6D --iktests=1000
could not import scipy.optimize.leastsq
2022-02-09 01:33:20,448 openrave [WARN] [plugindatabase.h:577 InterfaceBasePtr OpenRAVE::RaveDatabase::Create] Failed to create name fcl_, interface collisionchecker
openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip wwrobot: Transform6D [8, 9, 10, 11, 12, 13], precision=8, maxcasedepth=3 (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /input/.openrave/kinematics.cdadf6fd2a6bb544f3b29208ad4a0ff3/ikfast0x10000049.Transform6D.8_9_10_11_12_13.cpp
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, -3/40] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 0, 97/500] to left
openravepy.ikfast: generateIkSolver, [[0, 0, 1, -3/8],[0, 1, 0, 0],[-1, 0, 0, -13/100]]
openravepy.ikfast: generateIkSolver, [[cos(j8), -sin(j8), 0, 0],[sin(j8), cos(j8), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 1, 0, -51/200],[0, 0, 1, 0],[1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j9), -sin(j9), 0, 0],[sin(j9), cos(j9), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1562501**(1/2)/1562501, 1250*1562501**(1/2)/1562501, 0, -3124999/6250000],[-1250*1562501**(1/2)/1562501, 1562501**(1/2)/1562501, 0, -1991/5000000],[0, 0, 1, -1/200]]
openravepy.ikfast: generateIkSolver, [[cos(j10), -sin(j10), 0, 0],[sin(j10), cos(j10), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, -1, -43/125],[0, 1, 0, 1/25]]
openravepy.ikfast: generateIkSolver, [[cos(j11), -sin(j11), 0, 0],[sin(j11), cos(j11), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1562501**(1/2)/1562501, 0, -1250*1562501**(1/2)/1562501, 0],[1250*1562501**(1/2)/1562501, 0, 1562501**(1/2)/1562501, 0],[0, -1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j12), -sin(j12), 0, 0],[sin(j12), cos(j12), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 0],[1, 0, 0, 0],[0, 1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j13), -sin(j13), 0, 0],[sin(j13), cos(j13), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
openravepy.ikfast: solveFullIK_6D, ikfast 6d: [j8, j9, j10, j11, j12, j13]
openravepy.ikfast: iterateThreeIntersectingAxes, found 3 consecutive intersecting axes links[6:11], rotvars=[j11, j12, j13], translationvars=[j8, j9, j10]
openravepy.ikfast: SolveAllEquations, depth=0 c=1, [] [j8, j9, j10]: cases=None
openravepy.ikfast: checkForDivideByZero, adding atan2(-py, px) = Abs(px) + Abs(py) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-py, px) = Abs(px) + Abs(py) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-py, px) = Abs(px) + Abs(py) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-py, px) = Abs(px) + Abs(py) all zeros check
openravepy.ikfast: SolveAllEquations, depth=0 c=3, [j8] [j9, j10]: cases=set([])
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000, -3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) = Abs(-3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) + Abs(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000, -3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) = Abs(-3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) + Abs(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000, -3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) = Abs(-3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) + Abs(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000, -3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) = Abs(-3125000991*cj8*px/3125002000 - 3125000991*py*sj8/3125002000 + 3125000991*pz/3906252500000 - 159375050541/625000400000) + Abs(-3125000991*cj8*px/3906252500000 - 3125000991*py*sj8/3906252500000 - 3125000991*pz/3125002000 - 159375050541/781250500000000) all zeros check
openravepy.ikfast: SolveAllEquations, depth=0 c=5, [j8, j10] [j9]: cases=set([])
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 - cj8*px - py*sj8 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, -3125000991*cj8*px*sin(j9)/3125002000 - 3125000991*cj8*px*cos(j9)/3906252500000 - 51*cj8*px/100 - pp - 3125000991*py*sj8*sin(j9)/3125002000 - 3125000991*py*sj8*cos(j9)/3906252500000 - 51*py*sj8/100 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1562501**(1/2)*cj8*px*sin(j9)/1562501 + 1250*1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1562501**(1/2)*py*sj8*sin(j9)/1562501 + 1250*1562501**(1/2)*py*sj8*cos(j9)/1562501 - 1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 - 1250*1562501**(1/2)*cj8*px*sin(j9)/1562501 - 1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1250*1562501**(1/2)*py*sj8*sin(j9)/1562501 - 1562501**(1/2)*py*sj8*cos(j9)/1562501 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 - cj8*px - py*sj8 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, -3125000991*cj8*px*sin(j9)/3125002000 - 3125000991*cj8*px*cos(j9)/3906252500000 - 51*cj8*px/100 - pp - 3125000991*py*sj8*sin(j9)/3125002000 - 3125000991*py*sj8*cos(j9)/3906252500000 - 51*py*sj8/100 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1562501**(1/2)*cj8*px*sin(j9)/1562501 + 1250*1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1562501**(1/2)*py*sj8*sin(j9)/1562501 + 1250*1562501**(1/2)*py*sj8*cos(j9)/1562501 - 1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 - 1250*1562501**(1/2)*cj8*px*sin(j9)/1562501 - 1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1250*1562501**(1/2)*py*sj8*sin(j9)/1562501 - 1562501**(1/2)*py*sj8*cos(j9)/1562501 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 - cj8*px - py*sj8 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, -3125000991*cj8*px*sin(j9)/3125002000 - 3125000991*cj8*px*cos(j9)/3906252500000 - 51*cj8*px/100 - pp - 3125000991*py*sj8*sin(j9)/3125002000 - 3125000991*py*sj8*cos(j9)/3906252500000 - 51*py*sj8/100 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1562501**(1/2)*cj8*px*sin(j9)/1562501 + 1250*1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1562501**(1/2)*py*sj8*sin(j9)/1562501 + 1250*1562501**(1/2)*py*sj8*cos(j9)/1562501 - 1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 - 1250*1562501**(1/2)*cj8*px*sin(j9)/1562501 - 1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1250*1562501**(1/2)*py*sj8*sin(j9)/1562501 - 1562501**(1/2)*py*sj8*cos(j9)/1562501 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 - cj8*px - py*sj8 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, -3125000991*cj8*px*sin(j9)/3125002000 - 3125000991*cj8*px*cos(j9)/3906252500000 - 51*cj8*px/100 - pp - 3125000991*py*sj8*sin(j9)/3125002000 - 3125000991*py*sj8*cos(j9)/3906252500000 - 51*py*sj8/100 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1562501**(1/2)*cj8*px*sin(j9)/1562501 + 1250*1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1562501**(1/2)*py*sj8*sin(j9)/1562501 + 1250*1562501**(1/2)*py*sj8*cos(j9)/1562501 - 1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 - 1250*1562501**(1/2)*cj8*px*sin(j9)/1562501 - 1562501**(1/2)*cj8*px*cos(j9)/1562501 - 1250*1562501**(1/2)*py*sj8*sin(j9)/1562501 - 1562501**(1/2)*py*sj8*cos(j9)/1562501 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000]
openravepy.ikfast: AddSolution, depth=0, c=2, iter=0/2, starting newcases: set([py, px])
openravepy.ikfast: SolveAllEquations, depth=2 c=7, [] [j8, j9, j10]: cases=set([py, px])
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: SolveAllEquations, depth=2 c=9, [j10] [j8, j9]: cases=set([py, px])
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: SolveAllEquations, depth=2 c=11, [j10, j9] [j8]: cases=set([py, px])
openravepy.ikfast: AddSolution, depth=0, c=2, iter=1/2, starting newcases: set([Abs(px) + Abs(py)])
openravepy.ikfast: SolveAllEquations, depth=1 c=12, [] [j8, j9, j10]: cases=set([Abs(px) + Abs(py)])
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(-3125000991*pz/3125002000 - 159375050541/781250500000000, 3125000991*pz/3906252500000 - 159375050541/625000400000) = 9765631193750982081*pz**2/9765631250000000000 + 25400406734946304392681/390625250000000000000000 all zeros check
openravepy.ikfast: SolveAllEquations, depth=1 c=14, [j10] [j8, j9]: cases=set([Abs(px) + Abs(py)])
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [-pz**2 + 3125000991*pz*sin(j9)/3906252500000 - 3125000991*pz*cos(j9)/3125002000 - 159375050541*sin(j9)/625000400000 - 159375050541*cos(j9)/781250500000000 - 7635317330350982081/39062525000000000000, -1250*1562501**(1/2)*pz*sin(j9)/1562501 - 1562501**(1/2)*pz*cos(j9)/1562501 + 43*sj10/125 - 51*1562501**(1/2)*sin(j9)/312500200 + 1275*1562501**(1/2)*cos(j9)/6250004, -43*cj10/125 + 1562501**(1/2)*pz*sin(j9)/1562501 - 1250*1562501**(1/2)*pz*cos(j9)/1562501 - 1275*1562501**(1/2)*sin(j9)/6250004 - 51*1562501**(1/2)*cos(j9)/312500200 - 3125000991*1562501**(1/2)/7812505000000, -430*1562501**(1/2)*cj10*sin(j9)/1562501 - 43*1562501**(1/2)*cj10*cos(j9)/195312625 + 43*1562501**(1/2)*sj10*sin(j9)/195312625 - 430*1562501**(1/2)*sj10*cos(j9)/1562501 - 3124999*sin(j9)/6250000 - 1991*cos(j9)/5000000 - 51/200, 43*1562501**(1/2)*cj10*sin(j9)/195312625 - 430*1562501**(1/2)*cj10*cos(j9)/1562501 - pz + 430*1562501**(1/2)*sj10*sin(j9)/1562501 + 43*1562501**(1/2)*sj10*cos(j9)/195312625 + 1991*sin(j9)/5000000 - 3124999*cos(j9)/6250000]
openravepy.ikfast: SolveAllEquations, depth=1 c=16, [j10, j9] [j8]: cases=set([Abs(px) + Abs(py)])
openravepy.ikfast: SolveAllEquations, depth=0 c=17, [j8, j9, j10] [j11, j12, j13]: cases=None
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(-1250*new_r02 + new_r12)/1562501, 1562501**(1/2)*(-new_r02 - 1250*new_r12)/1562501) = new_r02**2 + new_r12**2 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(-1250*new_r02 + new_r12)/1562501, 1562501**(1/2)*(-new_r02 - 1250*new_r12)/1562501) = new_r02**2 + new_r12**2 all zeros check
openravepy.ikfast: SolveAllEquations, depth=0 c=19, [j8, j9, j10, j12] [j11, j13]: cases=None
openravepy.ikfast: solveSingleVariable, j11 solution: equations used for atan2: [-1562501**(1/2)*cj12/1562501 + new_r02*cos(j11) + new_r12*sin(j11), -1250*1562501**(1/2)*cj12/1562501 - new_r02*sin(j11) + new_r12*cos(j11), 1250*1562501**(1/2)*cj12*sin(j11)/1562501 - 1562501**(1/2)*cj12*cos(j11)/1562501 + new_r02, -1562501**(1/2)*cj12*sin(j11)/1562501 - 1250*1562501**(1/2)*cj12*cos(j11)/1562501 + new_r12, -1562501**(1/2)*new_r02*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r02*cos(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sin(j11)/1562501 + 1562501**(1/2)*new_r12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r00*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r00*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r10*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r10*cos(j11)/1562501 - new_r20*sj12, -1250*1562501**(1/2)*cj12*new_r01*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r01*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r11*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r11*cos(j11)/1562501 - new_r21*sj12, -cj12*new_r22 + 1250*1562501**(1/2)*new_r02*sj12*sin(j11)/1562501 - 1562501**(1/2)*new_r02*sj12*cos(j11)/1562501 - 1562501**(1/2)*new_r12*sj12*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sj12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r02*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r02*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r12*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r12*cos(j11)/1562501 - new_r22*sj12 - 1]
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*cj12*(-1250*new_r02 + new_r12), 1562501**(1/2)*cj12*(new_r02 + 1250*new_r12)) = new_r02**2 + new_r12**2 all zeros check
openravepy.ikfast: solveSingleVariable, j11 solution: equations used for atan2: [-1562501**(1/2)*cj12/1562501 + new_r02*cos(j11) + new_r12*sin(j11), -1250*1562501**(1/2)*cj12/1562501 - new_r02*sin(j11) + new_r12*cos(j11), 1250*1562501**(1/2)*cj12*sin(j11)/1562501 - 1562501**(1/2)*cj12*cos(j11)/1562501 + new_r02, -1562501**(1/2)*cj12*sin(j11)/1562501 - 1250*1562501**(1/2)*cj12*cos(j11)/1562501 + new_r12, -1562501**(1/2)*new_r02*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r02*cos(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sin(j11)/1562501 + 1562501**(1/2)*new_r12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r00*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r00*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r10*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r10*cos(j11)/1562501 - new_r20*sj12, -1250*1562501**(1/2)*cj12*new_r01*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r01*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r11*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r11*cos(j11)/1562501 - new_r21*sj12, -cj12*new_r22 + 1250*1562501**(1/2)*new_r02*sj12*sin(j11)/1562501 - 1562501**(1/2)*new_r02*sj12*cos(j11)/1562501 - 1562501**(1/2)*new_r12*sj12*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sj12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r02*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r02*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r12*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r12*cos(j11)/1562501 - new_r22*sj12 - 1]
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(cj12**2 - 1562501*new_r02**2), 1562501**(1/2)*(1250*cj12**2 + 1562501*new_r02*new_r12)) = Abs(1562501**(1/2)*(cj12**2 - 1562501*new_r02**2)) + Abs(1562501**(1/2)*(1250*cj12**2 + 1562501*new_r02*new_r12)) all zeros check
openravepy.ikfast: solveSingleVariable, j11 solution: equations used for atan2: [-1562501**(1/2)*cj12/1562501 + new_r02*cos(j11) + new_r12*sin(j11), -1250*1562501**(1/2)*cj12/1562501 - new_r02*sin(j11) + new_r12*cos(j11), 1250*1562501**(1/2)*cj12*sin(j11)/1562501 - 1562501**(1/2)*cj12*cos(j11)/1562501 + new_r02, -1562501**(1/2)*cj12*sin(j11)/1562501 - 1250*1562501**(1/2)*cj12*cos(j11)/1562501 + new_r12, -1562501**(1/2)*new_r02*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r02*cos(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sin(j11)/1562501 + 1562501**(1/2)*new_r12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r00*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r00*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r10*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r10*cos(j11)/1562501 - new_r20*sj12, -1250*1562501**(1/2)*cj12*new_r01*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r01*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r11*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r11*cos(j11)/1562501 - new_r21*sj12, -cj12*new_r22 + 1250*1562501**(1/2)*new_r02*sj12*sin(j11)/1562501 - 1562501**(1/2)*new_r02*sj12*cos(j11)/1562501 - 1562501**(1/2)*new_r12*sj12*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sj12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r02*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r02*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r12*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r12*cos(j11)/1562501 - new_r22*sj12 - 1]
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12), 1562501**(1/2)*(cj12**2 - 1562501*new_r12**2)) = Abs(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12)) + Abs(1562501**(1/2)*(cj12**2 - 1562501*new_r12**2)) all zeros check
openravepy.ikfast: solveSingleVariable, j11 solution: equations used for atan2: [-1562501**(1/2)*cj12/1562501 + new_r02*cos(j11) + new_r12*sin(j11), -1250*1562501**(1/2)*cj12/1562501 - new_r02*sin(j11) + new_r12*cos(j11), 1250*1562501**(1/2)*cj12*sin(j11)/1562501 - 1562501**(1/2)*cj12*cos(j11)/1562501 + new_r02, -1562501**(1/2)*cj12*sin(j11)/1562501 - 1250*1562501**(1/2)*cj12*cos(j11)/1562501 + new_r12, -1562501**(1/2)*new_r02*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r02*cos(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sin(j11)/1562501 + 1562501**(1/2)*new_r12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r00*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r00*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r10*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r10*cos(j11)/1562501 - new_r20*sj12, -1250*1562501**(1/2)*cj12*new_r01*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r01*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r11*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r11*cos(j11)/1562501 - new_r21*sj12, -cj12*new_r22 + 1250*1562501**(1/2)*new_r02*sj12*sin(j11)/1562501 - 1562501**(1/2)*new_r02*sj12*cos(j11)/1562501 - 1562501**(1/2)*new_r12*sj12*sin(j11)/1562501 - 1250*1562501**(1/2)*new_r12*sj12*cos(j11)/1562501, -1250*1562501**(1/2)*cj12*new_r02*sin(j11)/1562501 + 1562501**(1/2)*cj12*new_r02*cos(j11)/1562501 + 1562501**(1/2)*cj12*new_r12*sin(j11)/1562501 + 1250*1562501**(1/2)*cj12*new_r12*cos(j11)/1562501 - new_r22*sj12 - 1]
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12), 1562501**(1/2)*(-1562500*cj12**2 + 1562501*new_r02**2)) = Abs(1562501**(1/2)*(-1562500*cj12**2 + 1562501*new_r02**2)) + Abs(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12)) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*cj12*(-1250*new_r02 + new_r12), 1562501**(1/2)*cj12*(new_r02 + 1250*new_r12)) = new_r02**2 + new_r12**2 all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(cj12**2 - 1562501*new_r02**2), 1562501**(1/2)*(1250*cj12**2 + 1562501*new_r02*new_r12)) = Abs(1562501**(1/2)*(cj12**2 - 1562501*new_r02**2)) + Abs(1562501**(1/2)*(1250*cj12**2 + 1562501*new_r02*new_r12)) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12), 1562501**(1/2)*(cj12**2 - 1562501*new_r12**2)) = Abs(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12)) + Abs(1562501**(1/2)*(cj12**2 - 1562501*new_r12**2)) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12), 1562501**(1/2)*(-1562500*cj12**2 + 1562501*new_r02**2)) = Abs(1562501**(1/2)*(-1562500*cj12**2 + 1562501*new_r02**2)) + Abs(1562501**(1/2)*(-1250*cj12**2 + 1562501*new_r02*new_r12)) all zeros check
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj12*cos(j13) + new_r20, -cj12*sin(j13) + new_r21]
openravepy.ikfast: checkForDivideByZero, adding atan2(new_r21, -new_r20) = Abs(new_r20) + Abs(new_r21) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(new_r21, -new_r20) = Abs(new_r20) + Abs(new_r21) all zeros check
openravepy.ikfast: checkFinalEquation, cannot evalute: [-1250.0*new_r02 - 1.0*new_r12, 0, 1250.0*new_r02 + 1.0*new_r12]
openravepy.ikfast: SolveAllEquations, depth=0 c=21, [j8, j9, j10, j12, j11] [j13]: cases=set([])
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj12*cos(j13) + new_r20, -cj12*sin(j13) + new_r21, cj11*new_r00 + new_r10*sj11 + 1562501**(1/2)*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sin(j13)/1562501, cj11*new_r01 + new_r11*sj11 - 1562501**(1/2)*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 + 1250*1562501**(1/2)*sj12*cos(j13)/1562501 - 1562501**(1/2)*sin(j13)/1562501, cj11*new_r11 - new_r01*sj11 - 1250*1562501**(1/2)*sj12*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), 1562501**(1/2)*cj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*cj11*sin(j13)/1562501 + new_r00 - 1250*1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1562501**(1/2)*sj11*sin(j13)/1562501, -1562501**(1/2)*cj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 + 1250*1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, 1250*1562501**(1/2)*cj11*sj12*cos(j13)/1562501 - 1562501**(1/2)*cj11*sin(j13)/1562501 + new_r10 + 1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501, -1250*1562501**(1/2)*cj11*sj12*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 - 1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*new_r01*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r11*sj12/1562501 - cj12*new_r21 + 1250*1562501**(1/2)*new_r01*sj11*sj12/1562501 - 1562501**(1/2)*new_r11*sj11*sj12/1562501 + sin(j13), -1562501**(1/2)*cj11*new_r00*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r10*sj12/1562501 - cj12*new_r20 + 1250*1562501**(1/2)*new_r00*sj11*sj12/1562501 - 1562501**(1/2)*new_r10*sj11*sj12/1562501 - cos(j13)]
openravepy.ikfast: checkForDivideByZero, adding atan2(new_r21, -new_r20) = Abs(new_r20) + Abs(new_r21) all zeros check
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj12*cos(j13) + new_r20, -cj12*sin(j13) + new_r21, cj11*new_r00 + new_r10*sj11 + 1562501**(1/2)*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sin(j13)/1562501, cj11*new_r01 + new_r11*sj11 - 1562501**(1/2)*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 + 1250*1562501**(1/2)*sj12*cos(j13)/1562501 - 1562501**(1/2)*sin(j13)/1562501, cj11*new_r11 - new_r01*sj11 - 1250*1562501**(1/2)*sj12*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), 1562501**(1/2)*cj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*cj11*sin(j13)/1562501 + new_r00 - 1250*1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1562501**(1/2)*sj11*sin(j13)/1562501, -1562501**(1/2)*cj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 + 1250*1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, 1250*1562501**(1/2)*cj11*sj12*cos(j13)/1562501 - 1562501**(1/2)*cj11*sin(j13)/1562501 + new_r10 + 1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501, -1250*1562501**(1/2)*cj11*sj12*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 - 1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*new_r01*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r11*sj12/1562501 - cj12*new_r21 + 1250*1562501**(1/2)*new_r01*sj11*sj12/1562501 - 1562501**(1/2)*new_r11*sj11*sj12/1562501 + sin(j13), -1562501**(1/2)*cj11*new_r00*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r10*sj12/1562501 - cj12*new_r20 + 1250*1562501**(1/2)*new_r00*sj11*sj12/1562501 - 1562501**(1/2)*new_r10*sj11*sj12/1562501 - cos(j13)]
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj12*cos(j13) + new_r20, -cj12*sin(j13) + new_r21, cj11*new_r00 + new_r10*sj11 + 1562501**(1/2)*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sin(j13)/1562501, cj11*new_r01 + new_r11*sj11 - 1562501**(1/2)*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 + 1250*1562501**(1/2)*sj12*cos(j13)/1562501 - 1562501**(1/2)*sin(j13)/1562501, cj11*new_r11 - new_r01*sj11 - 1250*1562501**(1/2)*sj12*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), 1562501**(1/2)*cj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*cj11*sin(j13)/1562501 + new_r00 - 1250*1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1562501**(1/2)*sj11*sin(j13)/1562501, -1562501**(1/2)*cj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 + 1250*1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, 1250*1562501**(1/2)*cj11*sj12*cos(j13)/1562501 - 1562501**(1/2)*cj11*sin(j13)/1562501 + new_r10 + 1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501, -1250*1562501**(1/2)*cj11*sj12*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 - 1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*new_r01*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r11*sj12/1562501 - cj12*new_r21 + 1250*1562501**(1/2)*new_r01*sj11*sj12/1562501 - 1562501**(1/2)*new_r11*sj11*sj12/1562501 + sin(j13), -1562501**(1/2)*cj11*new_r00*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r10*sj12/1562501 - cj12*new_r20 + 1250*1562501**(1/2)*new_r00*sj11*sj12/1562501 - 1562501**(1/2)*new_r10*sj11*sj12/1562501 - cos(j13)]
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj12*cos(j13) + new_r20, -cj12*sin(j13) + new_r21, cj11*new_r00 + new_r10*sj11 + 1562501**(1/2)*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sin(j13)/1562501, cj11*new_r01 + new_r11*sj11 - 1562501**(1/2)*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 + 1250*1562501**(1/2)*sj12*cos(j13)/1562501 - 1562501**(1/2)*sin(j13)/1562501, cj11*new_r11 - new_r01*sj11 - 1250*1562501**(1/2)*sj12*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), 1562501**(1/2)*cj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*cj11*sin(j13)/1562501 + new_r00 - 1250*1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1562501**(1/2)*sj11*sin(j13)/1562501, -1562501**(1/2)*cj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 + 1250*1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, 1250*1562501**(1/2)*cj11*sj12*cos(j13)/1562501 - 1562501**(1/2)*cj11*sin(j13)/1562501 + new_r10 + 1562501**(1/2)*sj11*sj12*cos(j13)/1562501 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501, -1250*1562501**(1/2)*cj11*sj12*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 - 1562501**(1/2)*sj11*sj12*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*new_r01*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r11*sj12/1562501 - cj12*new_r21 + 1250*1562501**(1/2)*new_r01*sj11*sj12/1562501 - 1562501**(1/2)*new_r11*sj11*sj12/1562501 + sin(j13), -1562501**(1/2)*cj11*new_r00*sj12/1562501 - 1250*1562501**(1/2)*cj11*new_r10*sj12/1562501 - cj12*new_r20 + 1250*1562501**(1/2)*new_r00*sj11*sj12/1562501 - 1562501**(1/2)*new_r10*sj11*sj12/1562501 - cos(j13)]
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*cj11*cj12*new_r10 - 1562501**(1/2)*cj12*new_r00*sj11 - 1250*new_r20*sj12, -new_r20) = Abs(new_r20) + Abs(1562501**(1/2)*cj11*cj12*new_r10 - 1562501**(1/2)*cj12*new_r00*sj11 - 1250*new_r20*sj12) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(new_r21, -new_r20) = Abs(new_r20) + Abs(new_r21) all zeros check
openravepy.ikfast: checkForDivideByZero, adding atan2(1562501**(1/2)*cj11*cj12*new_r10 - 1562501**(1/2)*cj12*new_r00*sj11 - 1250*new_r20*sj12, -new_r20) = Abs(new_r20) + Abs(1562501**(1/2)*cj11*cj12*new_r10 - 1562501**(1/2)*cj12*new_r00*sj11 - 1250*new_r20*sj12) all zeros check
openravepy.ikfast: AddSolution, dual constraint [(new_r21, 0), (new_r20, 0), (new_r02, 0), (new_r12, 0), (new_r00**2, -new_r01**2 + 1), (new_r00**3, -new_r00*new_r01**2 + new_r00), (new_r11**2, -new_r01**2 + 1)] in Abs(new_r20) + Abs(new_r21)
openravepy.ikfast: AddSolution, depth=0, c=22, iter=0/3, starting newcases: set([Abs(j12 - 1.57079632679489661923132169164)])
openravepy.ikfast: SolveAllEquations, depth=1 c=23, [j8, j9, j10, j12, j11] [j13]: cases=set([Abs(j12 - 1.57079632679489661923132169164)])
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj11*new_r00 + new_r10*sj11 + 1250*1562501**(1/2)*sin(j13)/1562501 + 1562501**(1/2)*cos(j13)/1562501, cj11*new_r01 + new_r11*sj11 - 1562501**(1/2)*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 - 1562501**(1/2)*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r11 - new_r01*sj11 - 1250*1562501**(1/2)*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, -1562501**(1/2)*cj11*new_r01/1562501 - 1250*1562501**(1/2)*cj11*new_r11/1562501 + 1250*1562501**(1/2)*new_r01*sj11/1562501 - 1562501**(1/2)*new_r11*sj11/1562501 + sin(j13), -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), -1562501**(1/2)*cj11*new_r00/1562501 - 1250*1562501**(1/2)*cj11*new_r10/1562501 + 1250*1562501**(1/2)*new_r00*sj11/1562501 - 1562501**(1/2)*new_r10*sj11/1562501 - cos(j13), 1250*1562501**(1/2)*cj11*sin(j13)/1562501 + 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r00 + 1562501**(1/2)*sj11*sin(j13)/1562501 - 1250*1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r10 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, -1250*1562501**(1/2)*cj11*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 - 1562501**(1/2)*sj11*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501]
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj11*new_r00 + new_r10*sj11 + 1250*1562501**(1/2)*sin(j13)/1562501 + 1562501**(1/2)*cos(j13)/1562501, cj11*new_r01 + new_r11*sj11 - 1562501**(1/2)*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 - 1562501**(1/2)*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r11 - new_r01*sj11 - 1250*1562501**(1/2)*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, -1562501**(1/2)*cj11*new_r01/1562501 - 1250*1562501**(1/2)*cj11*new_r11/1562501 + 1250*1562501**(1/2)*new_r01*sj11/1562501 - 1562501**(1/2)*new_r11*sj11/1562501 + sin(j13), -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), -1562501**(1/2)*cj11*new_r00/1562501 - 1250*1562501**(1/2)*cj11*new_r10/1562501 + 1250*1562501**(1/2)*new_r00*sj11/1562501 - 1562501**(1/2)*new_r10*sj11/1562501 - cos(j13), 1250*1562501**(1/2)*cj11*sin(j13)/1562501 + 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r00 + 1562501**(1/2)*sj11*sin(j13)/1562501 - 1250*1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r10 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, -1250*1562501**(1/2)*cj11*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 - 1562501**(1/2)*sj11*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501]
openravepy.ikfast: AddSolution, depth=0, c=22, iter=0/3, adding newcases: set([Abs(j12 - 1.57079632679489661923132169164)])
openravepy.ikfast: AddSolution, depth=0, c=22, iter=1/3, starting newcases: set([Abs(j12 + 1.57079632679489661923132169164)])
openravepy.ikfast: SolveAllEquations, depth=1 c=25, [j8, j9, j10, j12, j11] [j13]: cases=set([Abs(j12 + 1.57079632679489661923132169164)])
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj11*new_r00 + new_r10*sj11 + 1250*1562501**(1/2)*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, cj11*new_r01 + new_r11*sj11 + 1562501**(1/2)*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 - 1562501**(1/2)*sin(j13)/1562501 - 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r11 - new_r01*sj11 + 1250*1562501**(1/2)*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, 1562501**(1/2)*cj11*new_r01/1562501 + 1250*1562501**(1/2)*cj11*new_r11/1562501 - 1250*1562501**(1/2)*new_r01*sj11/1562501 + 1562501**(1/2)*new_r11*sj11/1562501 + sin(j13), -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), 1562501**(1/2)*cj11*new_r00/1562501 + 1250*1562501**(1/2)*cj11*new_r10/1562501 - 1250*1562501**(1/2)*new_r00*sj11/1562501 + 1562501**(1/2)*new_r10*sj11/1562501 - cos(j13), 1250*1562501**(1/2)*cj11*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r00 + 1562501**(1/2)*sj11*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501, 1562501**(1/2)*cj11*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 - 1250*1562501**(1/2)*sj11*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*sin(j13)/1562501 - 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r10 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501 - 1562501**(1/2)*sj11*cos(j13)/1562501, 1250*1562501**(1/2)*cj11*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 + 1562501**(1/2)*sj11*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501]
openravepy.ikfast: solveSingleVariable, j13 solution: equations used for atan2: [cj11*new_r00 + new_r10*sj11 + 1250*1562501**(1/2)*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, cj11*new_r01 + new_r11*sj11 + 1562501**(1/2)*sin(j13)/1562501 + 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r10 - new_r00*sj11 - 1562501**(1/2)*sin(j13)/1562501 - 1250*1562501**(1/2)*cos(j13)/1562501, cj11*new_r11 - new_r01*sj11 + 1250*1562501**(1/2)*sin(j13)/1562501 - 1562501**(1/2)*cos(j13)/1562501, 1562501**(1/2)*cj11*new_r01/1562501 + 1250*1562501**(1/2)*cj11*new_r11/1562501 - 1250*1562501**(1/2)*new_r01*sj11/1562501 + 1562501**(1/2)*new_r11*sj11/1562501 + sin(j13), -1250*1562501**(1/2)*cj11*new_r00/1562501 + 1562501**(1/2)*cj11*new_r10/1562501 - 1562501**(1/2)*new_r00*sj11/1562501 - 1250*1562501**(1/2)*new_r10*sj11/1562501 - sin(j13), -1250*1562501**(1/2)*cj11*new_r01/1562501 + 1562501**(1/2)*cj11*new_r11/1562501 - 1562501**(1/2)*new_r01*sj11/1562501 - 1250*1562501**(1/2)*new_r11*sj11/1562501 - cos(j13), 1562501**(1/2)*cj11*new_r00/1562501 + 1250*1562501**(1/2)*cj11*new_r10/1562501 - 1250*1562501**(1/2)*new_r00*sj11/1562501 + 1562501**(1/2)*new_r10*sj11/1562501 - cos(j13), 1250*1562501**(1/2)*cj11*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r00 + 1562501**(1/2)*sj11*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501, 1562501**(1/2)*cj11*sin(j13)/1562501 + 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r01 - 1250*1562501**(1/2)*sj11*sin(j13)/1562501 + 1562501**(1/2)*sj11*cos(j13)/1562501, -1562501**(1/2)*cj11*sin(j13)/1562501 - 1250*1562501**(1/2)*cj11*cos(j13)/1562501 + new_r10 + 1250*1562501**(1/2)*sj11*sin(j13)/1562501 - 1562501**(1/2)*sj11*cos(j13)/1562501, 1250*1562501**(1/2)*cj11*sin(j13)/1562501 - 1562501**(1/2)*cj11*cos(j13)/1562501 + new_r11 + 1562501**(1/2)*sj11*sin(j13)/1562501 + 1250*1562501**(1/2)*sj11*cos(j13)/1562501]
openravepy.ikfast: AddSolution, depth=0, c=22, iter=1/3, adding newcases: set([Abs(j12 + 1.57079632679489661923132169164)])
openravepy.ikfast: AddSolution, depth=0, c=22, iter=2/3, starting newcases: set([Abs(new_r20) + Abs(new_r21)])
openravepy.ikfast: SolveAllEquations, depth=1 c=27, [j8, j9, j10, j12, j11] [j13]: cases=set([Abs(new_r20) + Abs(new_r21)])
Traceback (most recent call last):
File "/usr/bin/openrave0.9.py", line 126, in <module>
database.run(args=args)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 1123, in run
InverseKinematicsModel.RunFromParser(*args,**kwargs)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 1088, in RunFromParser
model = DatabaseGenerator.RunFromParser(Model=Model,parser=parser,robotatts=robotatts,args=args,**kwargs)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/__init__.py", line 262, in RunFromParser
model.autogenerate(options=options)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 653, in autogenerate
self.generate(iktype=iktype,freejoints=freejoints,precision=precision,forceikbuild=forceikbuild,outputlang=outputlang,ipython=ipython,ikfastmaxcasedepth=ikfastmaxcasedepth)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 884, in generate
chaintree = solver.generateIkSolver(baselink=baselink,eelink=eelink,freeindices=self.freeindices,solvefn=solvefn)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 2281, in generateIkSolver
chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/inversekinematics.py", line 746, in solveFullIK_6D
return self.ikfast.IKFastSolver.solveFullIK_6D(*args,**kwargs)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 2876, in solveFullIK_6D
tree = self.TestIntersectingAxes(solvejointvars,Links, LinksInv,endbranchtree)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 2964, in TestIntersectingAxes
return self.solve6DIntersectingAxes(T0links,T1links,transvars,rotvars,solveRotationFirst=solveRotationFirst, endbranchtree=endbranchtree)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 3159, in solve6DIntersectingAxes
rottree += self.SolveAllEquations(AllEquations,curvars=currotvars,othersolvedvars=othersolvedvars,solsubs=self.freevarsubs[:],endbranchtree=storesolutiontree)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 6848, in SolveAllEquations
return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 6947, in AddSolution
return [solution[0].subs(solsubs)]+self.SolveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 6708, in SolveAllEquations
return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 7213, in AddSolution
nextsolutions[var] = self.SolveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 6708, in SolveAllEquations
return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 7511, in AddSolution
newtree = self.SolveAllEquations(NewEquationsClean,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=newcases, currentcasesubs=newcasesubs, unknownvars=unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 6691, in SolveAllEquations
rawsolutions=self.solveSingleVariable(self.sortComplexity(raweqns),curvar,othersolvedvars, unknownvars=curvars+unknownvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 8188, in solveSingleVariable
pc = Poly(eqnew,varsym.cvar)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/polytools.py", line 98, in __new__
return cls._from_expr(rep, opt)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/polytools.py", line 209, in _from_expr
return cls._from_dict(rep, opt)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/polytools.py", line 151, in _from_dict
domain, rep = construct_domain(rep, opt=opt)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/constructor.py", line 217, in construct_domain
result = _construct_composite(coeffs, opt)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/constructor.py", line 169, in _construct_composite
numer[monom] = ground.from_sympy(coeff)
File "/usr/local/lib/python2.7/dist-packages/sympy/polys/domains/pythonrationalfield.py", line 34, in from_sympy
raise CoercionFailed("expected `Rational` object, got %s" % a)
sympy.polys.polyerrors.CoercionFailed: expected `Rational` object, got -oo
docker run --rm --user 1002:1002 -v /tmp/ikfast.Z39TKc:/input --workdir /input -e HOME=/input fixed-openrave:latest openrave0.9.py --database inversekinematics --robot=/input/wrapper.xml --iktype=Transform6D --iktests=1000\nfailed with exec code 1:
cat: /tmp/ikfast.Z39TKc/ifast.log: No such file or directory
What is the problem now? (expected Rational object???). Thank you
Sorry, but I cannot help further. Note, that IKFast is an external package (provided by OpenRave, but not MoveIt). If IKFast fails for your kinematics, you should use a generic IK solver like KDL.
Actually I was using KDL but it takes a lot of time to solve. I was trying to find an analytical solver. But thanks anyways for your guidance @rhaschke
Hello,
Im trying to follow the steps on https://ros-planning.github.io/moveit_tutorials/doc/ikfast/ikfast_tutorial.html to use IKFast for my custom built 6DOF robotic arm. I have Ubuntu Noetic (Ubuntu 20.04.3 LTS). I followed all steps until "Source" (right after "Binary Install"). In the Source section, i did the first 2 lines with no problem ( git clone ... and rosdep install ...), but when i want to catkin_make the workspace it gives me the following error:
Appearently it does not recognize
srdf.getNoDefaultCollisionLinks()
andsrdf.getEnabledCollisionPairs()
fromsrdf::Model
. I have tried to follow all the steps as mentioned, am I doing something wrong?I would appreciate your kind help. Thank you,