ikfast Module¶
IKFast: The Robot Kinematics Compiler¶
IKFast analytically solves robot inverse kinematics equations and generates optimized C++ files.
The inverse kinematics equations arise from attemping to place the robot end effector coordinate system in the world while maintaining joint and userspecified constraints. Userspecified constraints make up many different IK Types, each of them having advantages depending on the task.
IKFast will work with any number of joints arranged in a chain; this is defined by the Robot.Manipulator. For chains containing more degrees of freedom (DOF) than the IK type requires, the user can set arbitrary values of a subset of the joints until the number of unknown joints matches the degrees of freedom of the IK type.
It is not trivial to create handoptimized inverse kinematics solutions for arms that can capture all degenerate cases, having closedform IK speeds up many tasks including planning algorithms, so it really is a must for most robotics researchers.
Closedform solutions are necessary for motion planning due to two reasons:
 Numerical inverse kinematics solvers will always be much slower than closed form solutions. Planners require being able to process thousands of configurations per second. The closedform code generated by ikfast can produce solutions on the order of ~4 microseconds! As a comparison, most numerical solutions are on the order of 10 milliseconds (assuming good convergence).
 The null space of the solution set can be explored because all solutions are computed.
Features¶
 Can handle robots with arbitrary joint complexity like nonintersecting axes.
 All possible discrete solutions calculated (can be up to 16).
 Generated C++ code independent of OpenRAVE or any other library.
 Automatically detects degenerate cases where 2 or more axes align and cause infinite solutions.
 Invalid solutions are detected by checking if square roots are given negative values or arc sines and arc cosines are given inputs exceeding the [1,1] range.
 All divide by zero conditions are automatically checked and handled.
IK Types¶
The following inverse kinematics types are supported:
 Transform6D  end effector reaches desired 6D transformation
 Rotation3D  end effector reaches desired 3D rotation
 Translation3D  end effector origin reaches desired 3D translation
 Direction3D  direction on end effector coordinate system reaches desired direction
 Ray4D  ray on end effector coordinate system reaches desired global ray
 Lookat3D  direction on end effector coordinate system points to desired 3D position
 TranslationDirection5D  end effector origin and direction reaches desired 3D translation and direction. Can be thought of as Ray IK where the origin of the ray must coincide.
 TranslationXY2D  end effector origin reaches desired XY translation position, Z is ignored. The coordinate system with relative to the base link.
 TranslationLocalGlobal6D  local point on end effector origin reaches desired 3D global point. Because both local point and global point can be specified, there are 6 values.
 TranslationXAxisAngle4D, TranslationYAxisAngle4D, TranslationZAxisAngle4D  end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with x/y/zaxis (defined in the manipulator base link’s coordinate system)
 TranslationXAxisAngleZNorm4D, TranslationYAxisAngleXNorm4D, TranslationZAxisAngleYNorm4D  end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z, x, or y axis and be rotated at a certain angle starting from the x, y, or z axis (defined in the manipulator base link’s coordinate system)
The possible solve methods are defined by ikfast.IKFastSolver.GetSolvers()
Usage¶
The main file ikfast.py can be used both as a library and as an executable program. For advanced users, it is also possible to use run ikfast.py as a standalone program, which makes it mostly independent of the OpenRAVE runtime.
However, the recommended way of using IKFast is through the OpenRAVE databases.inversekinematics database generator which directly loads the IK into OpenRAVE as an interface.
Standalone Executable¶
To get help and a description of the ikfast arguments type
python `openraveconfig pythondir`/openravepy/_openravepy_/ikfast.py help
A simple example to generate IK for setting the 3rd joint free of the Barrett WAM is
python `openraveconfig pythondir`/openravepy/_openravepy_/ikfast.py robot=robots/barrettwam.robot.xml baselink=0 eelink=7 savefile=ik.cpp freeindex=2
Through Python¶
IKFast can also be used as a library in python. Generating 6D IK for the Barrett WAM while setting the 3rd joint free can be achieved with:
env = Environment()
kinbody = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
env.Add(kinbody)
solver = ikfast.IKFastSolver(kinbody=kinbody)
chaintree = solver.generateIkSolver(baselink=0,eelink=7,freeindices=[2],solvefn=ikfast.IKFastSolver.solveFullIK_6D)
code = solver.writeIkSolver(chaintree)
open('ik.cpp','w').write(code)
Using Generated IK Files¶
The common usage is to generate a C++ file that can be compiled into a standalone shared object/DLL, an executable program, or linked in statically to a bigger project. For more complex kinematics, LAPACK is needed. Here is the header file, which can be found in share/openraveX.Y/python/ikfast.h.
Compiling with GCC¶
The most basic command is:
gcc lstdc++ o ik ik.cpp
This will generate a small program that outputs all solutions given the end effector with respect to the robot base.
Using gcc, this requires “llapack” to be added. For MSVC++, users will have to compile lapack and link it themselves.
Compiling with MSVC¶
LAPACK For Windows should be installed in order to get complex kinematics linking correctly.
Details¶
Terminology:
 solve joints  the joints to solve for using inverse kinematics
 free joints  the joints that are specified before the IK is run, these values are known at runtime, but not known at IK generation time.
The top level class is ikfast.IKFastSolver and generates an Abstract Syntax Tree (AST) using definitions from ikfast.AST. The AST is then passed to the languagespecific generators defined in ikfast.CodeGenerators.
Internal symbolic math uses sympy. Infinite precision fractions are used in order to keep track of linearly independent equations and when they evaluate to 0. The infinite precision fractions are converted to decimals in the generators.
Open Issues¶
 currently ikfast does not handle big decimal numbers well. for example defining the axes or anchors as 1.032513241 will produce very big fractions and make things slow.
 there are cases when axes align and there are infinite solutions. although ikfast can detect such cases, we need a lot more work in this area.
 for 6D ik, there are still mechanisms it cannot solve, please send the kinematics model if such a situation is encountered.
 there are 10 different types of IK, currently ray4d IK needs a lot of work.
FAQ¶
 ikfast has been running for more than an hour, will it ever finish?
 Most likely not, usually an iksolver finishes within 10 minutes.
 class openravepy.ikfast.AST[source]¶
Abstarct Syntax Tree class definitions specific for evaluating complex math equations.
 class SolverBranchConds(jointbranches)[source]¶

 jointbranches = None¶
 thresh = 9.9999999999999995e07¶
 class AST.SolverCheckZeros(jointname, jointcheckeqs, zerobranch, nonzerobranch, thresh=9.9999999999999995e07, anycondition=True)[source]¶
 anycondition = None¶
 dictequations = None¶
 equationsused = None¶
 jointcheckeqs = None¶
 jointname = None¶
 nonzerobranch = None¶
 thresh = None¶
 zerobranch = None¶
 class AST.SolverCoeffFunction(jointnames, jointeval=None, exportvar=None, exportcoeffeqs=None, exportfnname=None, isHinges=None, rootmaxdim=16, jointevalcos=None, jointevalsin=None)[source]¶
Evaluate a set of coefficients and pass them to a custom function which will then return all possible values of the specified variables in jointnames.
 FeasibleIsZeros = False¶
 checkforzeros = None¶
 dictequations = None¶
 equationsused = None¶
 exportcoeffeqs = None¶
 exportfnname = None¶
 exportvar = None¶
 isHinges = True¶
 jointeval = None¶
 jointevalcos = None¶
 jointevalsin = None¶
 jointnames = None¶
 presetcheckforzeros = None¶
 rootmaxdim = None¶
 score = None¶
 class AST.SolverConditionedSolution(solversolutions)[source]¶
 dictequations = None¶
 solversolutions = None¶
 thresh = 9.9999999999999995e07¶
 class AST.SolverIKChainAxisAngle(solvejointvars, freejointvars, Pee, angleee, jointtree, Pfk=None, anglefk=None, iktype=None)[source]¶
 Pee = None¶
 Pfk = None¶
 angleee = None¶
 anglefk = None¶
 dictequations = None¶
 freejointvars = None¶
 iktype = None¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverIKChainDirection3D(solvejointvars, freejointvars, Dee, jointtree, Dfk=None)[source]¶
 Dee = None¶
 Dfk = None¶
 dictequations = None¶
 freejointvars = None¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverIKChainLookat3D(solvejointvars, freejointvars, Pee, jointtree, Pfk=None, Dfk=None)[source]¶
 Dfk = None¶
 Pee = None¶
 Pfk = None¶
 dictequations = None¶
 freejointvars = None¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverIKChainRay(solvejointvars, freejointvars, Pee, Dee, jointtree, Pfk=None, Dfk=None, is5dray=False)[source]¶
 Dee = None¶
 Dfk = None¶
 Pee = None¶
 Pfk = None¶
 dictequations = None¶
 freejointvars = None¶
 is5dray = False¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverIKChainRotation3D(solvejointvars, freejointvars, Ree, jointtree, Rfk=None)[source]¶
 Ree = None¶
 Rfk = None¶
 dictequations = None¶
 freejointvars = None¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverIKChainTransform6D(solvejointvars, freejointvars, Tee, jointtree, Tfk=None)[source]¶
 Tee = None¶
 Tfk = None¶
 dictequations = None¶
 freejointvars = None¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverIKChainTranslation3D(solvejointvars, freejointvars, Pee, jointtree, Pfk=None)[source]¶
 Pee = None¶
 Pfk = None¶
 dictequations = None¶
 freejointvars = None¶
 jointtree = None¶
 solvejointvars = None¶
 uselocaltrans = False¶
 class AST.SolverIKChainTranslationXY2D(solvejointvars, freejointvars, Pee, jointtree, Pfk=None)[source]¶
 Pee = None¶
 Pfk = None¶
 dictequations = None¶
 freejointvars = None¶
 jointtree = None¶
 solvejointvars = None¶
 class AST.SolverMatrixInverse(A, Asymbols)[source]¶
Take the inverse of a large matirx and set the coefficients of the inverse to the symbols in Asymbols.
 A = None¶
 Asymbols = None¶
 checkforzeros = None¶
 class AST.SolverPolynomialRoots(jointname, poly=None, jointeval=None, isHinge=True)[source]¶
find all roots of the polynomial and plug it into jointeval. poly should be Poly
 AddHalfTanValue = False¶
 FeasibleIsZeros = False¶
 checkforzeros = None¶
 dictequations = None¶
 equationsused = None¶
 isHinge = True¶
 jointeval = None¶
 jointevalcos = None¶
 jointevalsin = None¶
 jointname = None¶
 poly = None¶
 postcheckfornonzeros = None¶
 postcheckforrange = None¶
 postcheckforzeros = None¶
 score = None¶
 thresh = 9.9999999999999995e08¶
 class AST.SolverSolution(jointname, jointeval=None, jointevalcos=None, jointevalsin=None, AddPiIfNegativeEq=None, isHinge=True, thresh=9.9999999999999995e07)[source]¶
Contains equations for evaluating one unknown variable. The variable can have multiple solutions, and the solution is only valid if every equation in checkforzeros is nonzero
 AddHalfTanValue = False¶
 AddPiIfNegativeEq = None¶
 FeasibleIsZeros = False¶
 checkforzeros = None¶
 dictequations = None¶
 equationsused = None¶
Meaning of FeasibleIsZeros: If set to false, then solution is feasible only if all of these equations evalute to nonzero. If set to true, solution is feasible only if all these equations evaluate to zero.
 isHinge = True¶
 jointeval = None¶
 jointevalcos = None¶
 jointevalsin = None¶
 jointname = None¶
 presetcheckforzeros = None¶
 score = None¶
 thresh = None¶
 class openravepy.ikfast.IKFastSolver(kinbody=None, kinematicshash='', precision=None)[source]¶
Bases: openravepy.metaclass.AutoReloader
Solves the analytical inverse kinematics equations. The symbol naming conventions are as follows:
cjX  cos joint angle constX  temporary constant used to simplify computations dummyX  dummy intermediate variables to solve for gconstX  global constant that is also used during ik generation phase htjX  half tan of joint angle jX  joint angle pX  end effector position information rX  end effector rotation information sjX  sin joint angle tconstX  secondlevel temporary constant tjX  tan of joint angle
 exception CannotSolveError(value)[source]¶
Bases: exceptions.Exception
thrown when ikfast fails to solve a particular set of equations with the given knowns and unknowns
 static IKFastSolver.GetSolvers()[source]¶
Returns a dictionary of all the supported solvers and their official identifier names
 exception IKFastSolver.IKFeasibilityError(equations, checkvars)[source]¶
Bases: exceptions.Exception
thrown when it is not possible to solve the IK due to robot not having enough degrees of freedom. For example, a robot with 5 joints does not have 6D IK
 IKFastSolver.MatchSimilarFraction(num, numbersubs, matchlimit=40)[source]¶
returns None if no appropriate match found
 IKFastSolver.addSolution(solutions, AllEquations, curvars, othersolvedvars, solsubs, endbranchtree, currentcases=None)[source]¶
Take the least complex solution of a set of solutions and resume solving
 IKFastSolver.buildEquationsFromPositions(T1links, T1linksinv, transvars, othersolvedvars, uselength=True, removesmallnumbers=True)[source]¶
 IKFastSolver.buildEquationsFromRotation(T0links, Ree, rotvars, othersolvedvars)[source]¶
Ree is a 3x3 matrix
 IKFastSolver.buildRaghavanRothEquationsFromMatrix(T0, T1, solvejointvars)[source]¶
Builds the 14 equations using only 5 unknowns. Method explained in [Raghavan1993]. Basically take the position and one column/row so that the least number of variables are used.
[Raghavan1993] M Raghavan and B Roth, “Inverse Kinematics of the General 6R Manipulator and related Linkages”, Journal of Mechanical Design, Volume 115, Issue 3, 1993.
 IKFastSolver.checkFinalEquation(pfinal, subs=None)[source]¶
check an equation in one variable for validity
 IKFastSolver.checkSolvabilityReal(AllEquations, checkvars, othervars)[source]¶
returns true if there are enough equations to solve for checkvars
 static IKFastSolver.det_bareis(M, *vars, **kwargs)[source]¶
Function from sympy with a couple of improvements. Compute matrix determinant using Bareis’ fractionfree algorithm which is an extension of the well known Gaussian elimination method. This approach is best suited for dense symbolic matrices and will result in a determinant with minimal number of fractions. It means that less term rewriting is needed on resulting formulae.
TODO: Implement algorithm for sparse matrices (SFF).
Function from sympy/matrices/matrices.py
 IKFastSolver.forwardKinematicsChain(chainlinks, chainjoints)[source]¶
The first and last matrices returned are always nonsymbolic
 static IKFastSolver.groupTerms(expr, vars, symbolgen=None)[source]¶
Separates all terms that do have var in them
 static IKFastSolver.isValidSolution(expr)[source]¶
return true if solution does not contain any nan or inf terms
 IKFastSolver.iterateThreeIntersectingAxes(solvejointvars, Links, LinksInv)[source]¶
Search for 3 consectuive intersecting axes. If a robot has this condition, it makes a lot of IK computations simpler.
 IKFastSolver.iterateThreeNonIntersectingAxes(solvejointvars, Links, LinksInv)[source]¶
check for three consecutive nonintersecting axes. if several points exist, so have to choose one that is least complex?
 IKFastSolver.normalizeRotation(M)[source]¶
error from openrave can be on the order of 1e6 (especially if they are defined diagonal to some axis)
 IKFastSolver.reduceBothSides(polyeqs)[source]¶
Reduces a set of equations in 5 unknowns to a set of equations with 3 unknowns by solving for one side with respect to another. The input is usually the output of buildRaghavanRothEquations.
 IKFastSolver.reduceBothSidesInverseMatrix(leftsideeqs, rightsideeqs)[source]¶
solve a linear system inside the program since the matrix cannot be reduced so easily
 IKFastSolver.reduceBothSidesSymbolicallyDelayed(leftsideeqs, rightsideeqs, maxsymbols=10, usesymbols=True)[source]¶
the left and right side of the equations need to have different variables
 static IKFastSolver.removecommonexprs(eq, returncommon=False, onlygcd=False, onlynumbers=True)[source]¶
removes common expressions from a sum. Assumes all the coefficients are rationals. For example: a*c_0 + a*c_1 + a*c_2 = 0 will return in c_0 + c_1 + c_2 = 0
 static IKFastSolver.replaceNumbers(expr, symbolgen=None)[source]¶
Replaces all numbers with symbols, this is to make gcd faster when fractions get too big
 static IKFastSolver.sequence_cross_product(*sequences)[source]¶
iterates through the cross product of all items in the sequences
 IKFastSolver.simplifyTransform(eq, othervars=None)[source]¶
Attemps to simplify an equation given that variables from a rotation matrix have been used. There are 12 constraints that are tested:  lengths of rows and colums are 1  dot products of combinations of rows/columns are 0  cross products of combinations of rows/columns yield the left over row/column
 IKFastSolver.simplifyTransformPoly(peq)[source]¶
simplifies the coefficients of the polynomial with simplifyTransform and returns the new polynomial
 IKFastSolver.solve6DIntersectingAxes(T0links, T1links, transvars, rotvars, solveRotationFirst, endbranchtree)[source]¶
Solve 6D equations using fact that 3 axes are intersecting. The 3 intersecting axes are all part of T0links and will be used to compute the rotation of the robot. The other 3 axes are part of T1links and will be used to first compute the position.
 IKFastSolver.solveAllEquations(AllEquations, curvars, othersolvedvars, solsubs, endbranchtree, currentcases=None, unknownvars=None)[source]¶
 IKFastSolver.solveDialytically(newreducedeqs, ileftvar, returnmatrix=False, getsubs=None)[source]¶
Return the coefficients to solve equations dialytically (Salmon 1885) leaving out variable index ileftvar.
Extract the coefficients of 1, leftvar**1, leftvar**2, ... of every equation every len(newreducedeqs)*len(monoms) coefficients specify one degree of all the equations (order of monoms is specified in exportmonomorder there should be len(newreducedeqs)*len(monoms)*maxdegree coefficients
Method also checks if the equations are linearly dependent
 IKFastSolver.solveFullIK_6D(LinksRaw, jointvars, isolvejointvars, Tgripperraw=[1, 0, 0, 0]
 [0, 1, 0, 0]
 [0, 0, 1, 0]
 [0, 0, 0, 1])
Solves the full 6D translatio + rotation IK
 IKFastSolver.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree)[source]¶
Solve 6D equations of a general kinematics structure. This method only works if there exists 3 consecutive joints in that do not always intersect!
 IKFastSolver.solveFullIK_Direction3D(LinksRaw, jointvars, isolvejointvars, rawbasedir=[0]
 [0]
 [1])
basedir needs to be filled with a 3elemtn vector of the initial direction to control
 IKFastSolver.solveFullIK_Lookat3D(LinksRaw, jointvars, isolvejointvars, rawbasedir=[0]
 [0]
 [1], rawbasepos=[0]
 [0]
 [0])
basedir,basepos needs to be filled with a direction and position of the ray to control the lookat
 IKFastSolver.solveFullIK_Ray4D(LinksRaw, jointvars, isolvejointvars, rawbasedir=[0]
 [0]
 [1], rawbasepos=[0]
 [0]
 [0])
basedir,basepos needs to be filled with a direction and position of the ray to control
 IKFastSolver.solveFullIK_Rotation3D(LinksRaw, jointvars, isolvejointvars, Rbaseraw=[1, 0, 0]
 [0, 1, 0]
 [0, 0, 1])
 IKFastSolver.solveFullIK_Translation3D(LinksRaw, jointvars, isolvejointvars, rawbasepos=[0]
 [0]
 [0])
 IKFastSolver.solveFullIK_TranslationAxisAngle4D(LinksRaw, jointvars, isolvejointvars, rawbasedir=[1]
 [0]
 [0], rawbasepos=[0]
 [0]
 [0], rawglobaldir=[0]
 [0]
 [1], rawnormaldir=None)
Solves 3D translation + Angle with respect to Xaxis
 IKFastSolver.solveFullIK_TranslationDirection5D(LinksRaw, jointvars, isolvejointvars, rawbasedir=[0]
 [0]
 [1], rawbasepos=[0]
 [0]
 [0])
Solves 3D translation + 3D direction
 IKFastSolver.solveFullIK_TranslationLocalGlobal6D(LinksRaw, jointvars, isolvejointvars, Tgripperraw=[1, 0, 0, 0]
 [0, 1, 0, 0]
 [0, 0, 1, 0]
 [0, 0, 0, 1])
 IKFastSolver.solveFullIK_TranslationXY2D(LinksRaw, jointvars, isolvejointvars, rawbasepos=[0]
 [0])
 IKFastSolver.solveFullIK_TranslationXYOrientation3D(LinksRaw, jointvars, isolvejointvars, rawbasepos=[0]
 [0], rawangle=0)
 IKFastSolver.solveHighDegreeEquationsHalfAngle(lineareqs, varsym, subs=None)[source]¶
solve a set of equations in one variable with halfangle substitution
 IKFastSolver.solveKohliOsvatic(rawpolyeqs, solvejointvars, endbranchtree)[source]¶
Find a 16x16 matrix where the entries are linear with respect to the tan halfangle of one of the variables [Kohli1993]. Takes in the 14 raghavan/roth equations.
[Kohli1993] Dilip Kohli and M. Osvatic, “Inverse Kinematics of General 6R and 5R,P Serial Manipulators”, Journal of Mechanical Design, Volume 115, Issue 4, Dec 1993.
 IKFastSolver.solveLiWoernleHiller(rawpolyeqs, solvejointvars, endbranchtree)[source]¶
LiWoernleHiller procedure covered in Jorge Angeles, “Fundamentals of Robotics Mechanical Systems”, Springer, 2007.
 IKFastSolver.solveManochaCanny(rawpolyeqs, solvejointvars, endbranchtree)[source]¶
Solves the IK equations using eigenvalues/eigenvectors of a 12x12 quadratic eigenvalue problem. Method explained in
Dinesh Manocha and J.F. Canny. “Efficient inverse kinematics for general 6R manipulators”, IEEE Transactions on Robotics and Automation, Volume 10, Issue 5, Oct 1994.
 IKFastSolver.solvePairVariables(raweqns, var0, var1, othersolvedvars, maxcomplexity=50, unknownvars=None)[source]¶
 IKFastSolver.solvePairVariablesHalfAngle(raweqns, var0, var1, othersolvedvars, subs=None)[source]¶
solves equations of two variables in sin and cos
 IKFastSolver.solveSingleVariable(raweqns, var, othersolvedvars, maxsolutions=4, maxdegree=2, subs=None, unknownvars=None)[source]¶
 class openravepy.ikfast.atan2check[source]¶
Bases: sympy.functions.elementary.trigonometric.atan2
defines floatingpoint mod
 default_assumptions = {'real': True, 'imaginary': False, 'complex': True, 'commutative': True}¶
 is_Function = True¶
 is_commutative = True¶
 is_complex = True¶
 is_imaginary = False¶
 is_real = True¶
 nargs = 2¶
 openravepy.ikfast.axisAngleFromRotationMatrix((object)rotation) → object :¶
RaveVector < T > axisAngleFromMatrix(const RaveTransformMatrix < T > & rotation)
Converts the rotation of a matrix into axisangle representation.
 Parameters
 rotation 
 3x3 rotation matrix
 class openravepy.ikfast.fmod[source]¶
Bases: sympy.core.function.Function
defines floatingpoint mod
 default_assumptions = {'real': True, 'imaginary': False, 'complex': True, 'commutative': True}¶
 is_Function = True¶
 is_commutative = True¶
 is_complex = True¶
 is_imaginary = False¶
 is_real = True¶
 nargs = 2¶
Questions/Feedback
Having problems with OpenRAVE?
 Search for information in the archives of the openraveusers mailing list, or post a question.
 If you notice errors , please open a ticket and let us know!