andyzeng/ikfastpy

Python wrapper over OpenRave's IKFast inverse kinematics solver for a UR5 robot arm.

ikfastpy's Issues

an example for visual servoing

Hi @andyzeng, many thanks for open-sourcing this great repo for UR5.

In the README, you mentioned that IKFast can be used with speedj command for real-time visual servoing. I am wondering do you have a example code for that? How do you get the Jacobian of the UR5?

Thank you very much.

Get Poses of all joints

How can I use ikfastpy to get the poses of all joints?

ee_pose = np.asarray(ee_pose).reshape(3,4) -> gives me just the endefector ...

Can we make same wrapper for C++ ?

Hi all,
I want to find FK and IK of given pose data since my project is in C++ language, thus I want to make cpp wrapper on ikfast61.cpp. I'm just not sure which file should i use to extend in my wrapper to make it able to work like python wrapper of this repository.
I tried to include ikfast61.cpp file in my cpp wrapper but it gives this error:

/tmp/ccZNJDSH.o: In function `IKSolver::solvedialyticpoly8qep(double const*, double*, int&)':
mywrapper.cpp:(.text._ZN8IKSolver21solvedialyticpoly8qepEPKdPdRi[_ZN8IKSolver21solvedialyticpoly8qepEPKdPdRi]+0x492): undefined reference to `dgetrf_'

Any kind of help will be appreciated. Thank you

Unknown reason for failure to get solutions

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
[]

Goal tolerance setting

Hello, is there any way to change the IK accuracy in this setup?

I have created Translation3D solver only for the wrist part of the UR5e. I can successfully compute forward and inverse kinematics. But the joint (or goal) tolerances are too small that most of the time I cannot get any valid solutions.

For instance ee_pose_test returns a valid solution whereas ee_pose_test2 does not.

ee_pose_test = np.array([[ 1., 0., 0., 0.1000000015],
[ 0., 1., 0., 0.22650042],
[ 0., 1., 0., 0.00998334]])

ee_pose_test2 = np.array([[ 1., 0., 0., 0.1000000],
[ 0., 1., 0., 0.22650],
[ 0., 1., 0., 0.00998]])

I was wondering if is there any chance that I can set an ε or something that I can reduce the required significant numbers of EE_pose which I use in the IK solver? Something like arm_group.set_goal_joint_tolerance(0.001) in Moveit.

In this link I saw that --accuracy parameter which is used during creating ik.cpp can do something similar. However, it is also stated that to increase this tolerance value may cause bigger problems in the solver.

Any suggestions?

The ikfastpy can not get the correct ur5 rotation matrix in the world frame.

Thank you for your work.
we want to use your code to creat the solutions for ur5. But we get different solutions when we calculate the rotation matrixs between from ROS and your code.
for examp,we get the rotation matrixs from quaternion in ros:
[ 9.44e-05 1 9.762e-05 -1.0915e-01
-1.56e-04 9.76e-05 -9.999e-01 6.04687e-01
-1 9.440e-05 1.5637e-04 4.318e-01 ]

but your code from jiont angle is :
rotation matrixs:
[ 1 9.746e-05 -9.446e-5 -1.0915e-01
9.76e-05 -9.999e-01 1.56e-04 6.04687e-01

• 9.440e-05 -1.56e-04 -1 4.318e-01]
  We can see the two rotation matrixs have same position information，but the rotation part seem to be in other coordinate system

NoConvergence: Didn't converge in maxsteps=50 steps, when regenerating ikfast61.cpp

Hi @andyzeng,
Thanks for sharing this awesome work!
I was trying to modify the robot configuration and regenerate the ikfast61.cpp file following the instructions you gave. I got a NoConvergence error and it remains even when I test it without modifying the ur.robot.xml. Any suggestions on how to debug this issue? Appreciate it!

Traceback (most recent call last):
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 9521, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2281, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2936, in solveFullIK_6D
    tree = self.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree, usesolvers=1)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 3271, in solveFullIK_6DGeneral
    leftovertree = self.SolveAllEquations(AllEquationsExtra,curvars=curvars,othersolvedvars = self.freejointvars+usedvars,solsubs = solsubs,endbranchtree=origendbranchtree)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6848, in SolveAllEquations
    return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 7511, in AddSolution
    newtree = self.SolveAllEquations(NewEquationsClean,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=newcases, currentcasesubs=newcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6848, in SolveAllEquations
    return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/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/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6828, in SolveAllEquations
    rawsolutions=self.SolvePairVariables(raweqns,var0,var1,othersolvedvars,unknownvars=curvars+unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 8955, in SolvePairVariables
    return self.SolvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 7740, in SolvePairVariablesHalfAngle
    possiblefinaleq = self.checkFinalEquation(Poly(Malldet,leftvar),subs)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 8154, in checkFinalEquation
    roots = mpmath.polyroots(numpy.array(numpy.array(coeffs),numpy.float64))
  File "/usr/lib/python2.7/dist-packages/mpmath/calculus/polynomials.py", line 189, in polyroots
    % maxsteps)
mpmath.libmp.libhyper.NoConvergence: Didn't converge in maxsteps=50 steps.

error: command 'd:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30037\\bin\\HostX86\\x64\\cl.exe' failed with exit status 2

error: command 'd:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Tools\MSVC\14.29.30037\bin\HostX86\x64\cl.exe' failed with exit status 2