Generating IKFast solution failed for 4DOF crane like robot
Hi there,
I have been working on making an IKFast plugin for a self-made crane-like robot. It has 4DOF consists of two prismatic joints and two revolute joints(yaw). Here's the urdf and the collada file. The urdf was converted from Solidworks assembly.
I followed the Moveit tutorial at first. And the first issue was it constantly complains about not able to import scipy and failed to create name fcl_:
rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype TranslationXAxisAngle4D --name boxbot boxbot.urdf boxbot_arm base_link gripper_link
...
could not import scipy.optimize.leastsq
2020-12-19 07:31:50,917 openrave [WARN] [plugindatabase.h:577 InterfaceBasePtr OpenRAVE::RaveDatabase::Create] Failed to create name fcl_, interface collisionchecker
Although the cpp file was generated, the tests all failed:
...
2020-12-19 07:43:59,940 openrave [ERROR] [ikfastmodule.cpp:1093 DebugIK] FindIKSolution: Incorrect IK, i = 999 error: 1.920686003946152 originaljointvalues=[2.426242807598114, 0.6240073221831322, -0.7463420090236663, 1.375526472077847, ] returnedjointvalues=[0, 0.6240073221831319, -0.6280926721172346, 0.458355279916288, ] ikparamin="1140850699 1.570796326794897 -0.8053369741735978 0.2775487118424944 1.008221322183132 " ikparamout="1140850699 1.570796326794897 1.042773042542307 -0.2454465958505842 1.008221322183132 " raw ik command: 1.570796326794897 0 0 -0.8053369741735978 0 0 0 0.2775487118424944 0 0 0 1.008221322183132
openravepy._openravepy_0_9.databases.inversekinematics.InverseKinematicsError: Inverse Kinematics Error wrong rate 1.000000 > 0!
I figured I might have used the wrong iktype since I'm quite new to Moveit and inverse kinematic, so I also tried with other 4D iktypes, and they all failed in the same way.
After this, I tried what gvdhoorn mentioned in this ros answer, and the result was the same.
Upon desperation, I installed Openrave and all dependencies in a ubuntu 16.04 VM. I verified that the openrave-robot.py script can read the link info. Also, the openrave.py can read the collada file correctly.
openrave-robot.py boxbot.dae --info links
name index parents
-----------------------------
base_link 0
tower_link 1 base_link
arm_link 2 tower_link
wrist_link 3 arm_link
hand_link 4 wrist_link
gripper_link 5 hand_link
-----------------------------
But it failed in the same way as before.
openrave.py --database inversekinematics --robot=wrapper.xml --iktype=TranslationXAxisAngle4D --manipname=manipulator --iktests=100
2020-12-19 16:10:24,009 openrave [WARN] [fclspace.h:249 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link boxbot_description_solidworks/base_link with 0 geometries (env 1) (userdatakey fclcollision0x1479700)
...
2020-12-19 16:10:24,009 openrave [WARN] [fclspace.h:249 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link boxbot_description_solidworks/gripper_link with 0 geometries (env 1) (userdatakey fclcollision0x1479700)
openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip boxbot: TranslationXAxisAngle4D [0, 1, 2, 3], precision=8, maxcasedepth=3 (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /home/vm-openrave/.openrave/kinematics.47d9de6c824b7a118e7f3d7c36df94a7/ikfast0x10000049.TranslationXAxisAngle4D.0_1_2_3.cpp
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 653/5000] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 6941/50000, 23751/250000] to left
openravepy.ikfast: generateIkSolver, [[1, 0, 0, -20947/100000000],[0, 1, 0, -24613/2500000],[0, 0, 1, 40951/100000]]
openravepy.ikfast: generateIkSolver, [[cos ...
