Unknown reason for failure to get solutions
kracon7 opened this issue · 4 comments
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
[]
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
@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