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 user-specified constraints. User-specified 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 hand-optimized inverse kinematics solutions for arms that can capture all degenerate cases, having closed-form IK speeds up many tasks including planning algorithms, so it really is a must for most robotics researchers.
Closed-form solutions are necessary for motion planning due to two reasons:
The following inverse kinematics types are supported:
The possible solve methods are defined by ikfast.IKFastSolver.GetSolvers()
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 stand-alone program, which makes it mostly independent of the OpenRAVE run-time.
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.
To get help and a description of the ikfast arguments type
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --help
A simple example to generate IK for setting the 3rd joint free of the Barrett WAM is
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=robots/barrettwam.robot.xml --baselink=0 --eelink=7 --savefile=ik.cpp --freeindex=2
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)
The common usage is to generate a C++ file that can be compiled into a stand-alone 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/openrave-X.Y/python/ikfast.h.
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.
LAPACK For Windows should be installed in order to get complex kinematics linking correctly.
Terminology:
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 language-specific 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.
Abstarct Syntax Tree class definitions specific for evaluating complex math equations.
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.
Take the inverse of a large matirx and set the coefficients of the inverse to the symbols in Asymbols.
find all roots of the polynomial and plug it into jointeval. poly should be Poly
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 non-zero
Meaning of FeasibleIsZeros: If set to false, then solution is feasible only if all of these equations evalute to non-zero. If set to true, solution is feasible only if all these equations evaluate to zero.
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 - second-level temporary constant tjX - tan of joint angle
Bases: exceptions.Exception
thrown when ikfast fails to solve a particular set of equations with the given knowns and unknowns
Returns a dictionary of all the supported solvers and their official identifier names
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
returns None if no appropriate match found
Take the least complex solution of a set of solutions and resume solving
Ree is a 3x3 matrix
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. |
check an equation in one variable for validity
returns true if there are enough equations to solve for checkvars
Function from sympy with a couple of improvements. Compute matrix determinant using Bareis’ fraction-free 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
The first and last matrices returned are always non-symbolic
Separates all terms that do have var in them
return true if solution does not contain any nan or inf terms
Search for 3 consectuive intersecting axes. If a robot has this condition, it makes a lot of IK computations simpler.
check for three consecutive non-intersecting axes. if several points exist, so have to choose one that is least complex?
error from openrave can be on the order of 1e-6 (especially if they are defined diagonal to some axis)
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.
solve a linear system inside the program since the matrix cannot be reduced so easily
the left and right side of the equations need to have different variables
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
Replaces all numbers with symbols, this is to make gcd faster when fractions get too big
iterates through the cross product of all items in the sequences
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
simplifies the coefficients of the polynomial with simplifyTransform and returns the new polynomial
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.
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
Solves the full 6D translatio + rotation IK
Solve 6D equations of a general kinematics structure. This method only works if there exists 3 consecutive joints in that do not always intersect!
basedir needs to be filled with a 3elemtn vector of the initial direction to control
basedir,basepos needs to be filled with a direction and position of the ray to control the lookat
basedir,basepos needs to be filled with a direction and position of the ray to control
Solves 3D translation + Angle with respect to X-axis
Solves 3D translation + 3D direction
solve a set of equations in one variable with half-angle substitution
Find a 16x16 matrix where the entries are linear with respect to the tan half-angle 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. |
Li-Woernle-Hiller procedure covered in Jorge Angeles, “Fundamentals of Robotics Mechanical Systems”, Springer, 2007.
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.
solves equations of two variables in sin and cos
Bases: sympy.functions.elementary.trigonometric.atan2
defines floating-point mod
RaveVector < T > axisAngleFromMatrix(const RaveTransformMatrix < T > & rotation)
Converts the rotation of a matrix into axis-angle representation.
- Parameters
- rotation -
- 3x3 rotation matrix
Bases: sympy.core.function.Function
defines floating-point mod