search

andyzeng / ikfastpy

Python wrapper over OpenRave's IKFast inverse kinematics solver for a UR5 robot arm.
221 stars 65 forks source link

Unknown reason for failure to get solutions #4

Open kracon7 opened 4 years ago

kracon7 commented 4 years ago

Hi, I found that the wrapper was doing something wierd. My regenerated robot model is pretty much the same as it is in the repo, expect for the last link translation. I changed that from 0.0823 to 0.3

The issue is: For tatget rotation configuration that's aligned with the initial frame, the solver seems to fail no matter how. I tried a feasible configuration close to that and the inverse could work. Please check the code below. 

Python 3.6.8 (default, Dec 24 2018, 19:24:27) 
[GCC 5.4.0 20160609] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> from ikfastpy import ikfastpy
>>> import numpy as np
>>> kin = ikfastpy.PyKinematics()
>>> p = np.array([0,-2.2, 1.38, -0.723, 1.45, 1.51])
>>> ee_pose = np.array(kin.forward(p)).reshape(3,4)
>>> ee_pose
array([[ 0.9979704 ,  0.05739251, -0.02759018, -0.12037876],
       [-0.06031614,  0.99087894, -0.12050273, -0.14530082],
       [ 0.02042257,  0.12192228,  0.99232954,  1.0146302 ]])
>>> kin.inverse(ee_pose.reshape(-1).tolist())
[-1.597476601600647, -2.515026330947876, 1.3043287992477417, -0.4816295802593231, 1.5464282035827637, 3.110640048980713, -1.597476601600647, -2.240016460418701, 1.3455054759979248, 2.3437767028808594, -1.5464282035827637, -0.030952485278248787, 2.2614331740555826e-08, -2.200000047683716, 1.3799999952316284, -0.7229999899864197, 1.4500000476837158, 1.5099999904632568, 2.261408127424147e-08, -1.8565380573272705, 1.2691690921783447, 2.1859614849090576, -1.4500000476837158, -1.6315926313400269, -1.597476601600647, -1.2718806266784668, -1.3043287992477417, 0.8838823437690735, 1.5464282035827637, 3.110640048980713, 2.2614297989775878e-08, -0.6463527083396912, -1.2691690921783447, -2.769070863723755, -1.4500000476837158, -1.6315926313400269, -1.597476601600647, -0.9583459496498108, -1.3455054759979248, -2.5300683975219727, -1.5464282035827637, -0.030952485278248787, 2.26142358172865e-08, -0.8861240148544312, -1.3799999952316284, 0.7231239676475525, 1.4500000476837158, 1.5099999904632568]

However if I change the rotation frame to identity, the inverse fails. I double checked that this configuration is not at singularity.

>>> ee_pose[:,:3] = np.eye(3)
>>> ee_pose
array([[ 1.        ,  0.        ,  0.        , -0.12037876],
       [ 0.        ,  1.        ,  0.        , -0.14530082],
       [ 0.        ,  0.        ,  1.        ,  1.0146302 ]])
>>> kin.inverse(ee_pose.reshape(-1).tolist())
Error: (inverse kinematics) failed to get ik solution
[]
capaulson commented 4 years ago

Try removing mpmath and use sympy 0.7.1, worked for me: http://openrave-users-list.185357.n3.nabble.com/mpmath-not-needed-td4027687.html

kracon7 commented 4 years ago

@capaulson Actually I didn't have mpmath. The mpmath was already removed when I tried to setup OpenRave for regenerating the .cpp code. mpmath did cause problem in that step as I mentioned earlier in issue #2

ZIkun-Yu commented 4 years ago

Hi @kracon7 , did you solve this issue, I had the same issue that ikfast was failed to get ik solution.

kracon7 commented 4 years ago

Hi @kracon7 , did you solve this issue, I had the same issue that ikfast was failed to get ik solution.

No, I wasn't able to solve it.