Inverse Kinematics

Inverse kinematics is the problem of calculating the configuration of a device, \(\mathbf{q}\), such that the transformation from base to TCP frame becomes the desired, \(\mathbf{T}_{base}^{TCP}(\mathbf{q}) = \mathbf{T}_{desired}\).

RobWork provides the InvKinSolver interface for solvers that are able to do this calculation. The solvers can be grouped into two overall types:

• ClosedFormIK: solvers that can solve the inverse kinematics by analytical expressions.

• IterativeIK: solvers that uses multiple steps to iteratively find a solution. Typically these are using the device Jacobian to do so.

Both types of IK solvers take a transform (rw::math::Transform3D<>) as input and return configurations for a device for which the transform from the base to the end of the device (rw::models::Device) equals the given transform.

In general the ClosedFormIK solvers can be expected to be computationally efficient, but these solvers will typically only work for specific robots. The iterative solvers can be expected to be less efficient, but are also applicable for more generic robots.

Closed Form Solvers

Specific closed form solvers are available for both Universal Robots and the Kuka IIWA robots. The more generic PieperSolver is also available.

Universal Robots

For Universal Robots, the closed form solver rw::invkin::ClosedFormIKSolverUR an be used. This robot has 6 degrees of freedom, and the solver will provide up to 8 solutions. Notice that for every joint there is a solution where the joint is rotated 360 degrees. Than means that each of the 8 solutions can typically be achieved in 64 different ways. It is up to the user to add or subtract 360 degrees such that it suits the application best.

Kuka IIWA

For the Kuka LBR IIWA 7 R800 robot, the closed form solver rw::invkin::ClosedFormIKSolverKukaIIWA an be used. This robot has 7 degrees of freedom. It can have an infinite number of solutions, as the middle joint can be placed anywhere on a circle. It is used very similarly to the Universal Robots solver, but the solve() function can optionally take an extra parameter. The extra parameter is the direction of where to place the middle joint. The solutions returned is only deterministic when providing this extra argument. Otherwise, a random direction is chosen. Up to 8 solutions is returned.

Pieper solver

The rw::invkin::PieperSolver is a closed form solver that works for serial devices with 6 degrees of freedom revolute joints. It requires that the last three axes intersect.

Iterative Solvers

An iterative IK solver needs a start configuration from which to start the iterative search. Depending on the start configuration and other constraints, the IK solver may fail or succeed in finding a valid configuration.

Jacobian Solver

The rw::invkin::JacobianIKSolver is the basic iterative solver. The method uses an Newton-Raphson iterative approach and is based on using the inverse of the device Jacobian to compute each local solution in each iteration. Solving with the JacobianIKSolver is very similar to the closed form solvers, but remember that the result depend on the initial configuration of the robot. The device configuration is stored in the state variable, and can be set by calling setQ on the device:

device.setQ(q, state);

The JacobianIKSolver has methods for adjusting the step size and using different solver types. Please consult the API documentation for further information.

Meta Solvers

The solution of an iterative solver may depend on the starting configuration, provided in the state given in the solve method. To search for all solution an iterative ik solver may be wrapped in the rw::invkin::IKMetaSolver which calls it a specified number of times with random start configurations. Many robots have ambiguities which can be resolved using the rw::invkin::AmbiguityResolver.

Parallel Robots

For parallel robots, the rw::invkin::ParallelIKSolver can be used. It is an iterative solver somewhat similar to the JacobianIKSolver. Where the JacobianIKSolver solves for only one objective, namely that the end-effector should be in a certain location, the ParallelIKSolver simultaneously solves for the objective that the defined junctions must remain connected. A stacked Jacobian is used to enforce these objectives simultaneously, and a Singular Value Decomposition (SVD) is used iteratively to find a solution.

Code Examples

In the following you find code examples on how to do inverse kinematics using the closed form solver for Universal Robots. Other types of solvers are used in a very similar way.

C++

#include <rw/invkin/ClosedFormIKSolverUR.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/loaders/WorkCellLoader.hpp>
#include <rw/math/EAA.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/math/Q.hpp>
#include <rw/models/SerialDevice.hpp>
#include <rw/models/WorkCell.hpp>

#include <iostream>

using rw::invkin::ClosedFormIKSolverUR;
using rw::kinematics::State;
using rw::loaders::WorkCellLoader;
using namespace rw::math;
using namespace rw::models;

#define WC_FILE "/devices/serialdev/UR10e_2018/UR10e.xml"

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cout << "Usage: " << argv[0] << " <path/to/RobWorkData>" << std::endl;
        exit(1);
    }
    const std::string path = argv[1];

    const WorkCell::Ptr wc = WorkCellLoader::Factory::load(path + WC_FILE);
    if (wc.isNull())
        RW_THROW("WorkCell could not be loaded.");
    const SerialDevice::Ptr device = wc->findDevice<SerialDevice>("UR10e");
    if (device.isNull())
        RW_THROW("UR10e device could not be found.");

    const State state = wc->getDefaultState();
    const ClosedFormIKSolverUR solver(device, state);

    const Transform3D<> Tdesired(Vector3D<>(0.2, -0.2, 0.5),
            EAA<>(0, Pi, 0).toRotation3D());
    const std::vector<Q> solutions = solver.solve(Tdesired, state);

    std::cout << "Inverse Kinematics for " << device->getName() << "." << std::endl;
    std::cout << " Base frame: " << device->getBase()->getName() << std::endl;
    std::cout << " End/TCP frame: " << solver.getTCP()->getName() << std::endl;
    std::cout << " Target Transform: " << Tdesired << std::endl;
    std::cout << "Found " << solutions.size() << " solutions." << std::endl;
    for(std::size_t i = 0; i < solutions.size(); i++) {
        std::cout << " " << solutions[i] << std::endl;
    }

    return 0;
}

In lines 22-30, the WorkCell is loaded. A SerialDevice is retrieved from the WorkCell in lines 31-33. Always remember to check for null pointers.

In lines 35-36 the default state is retrieved from the WorkCell, and the closed form solver is constructed.

The desired transformation is defined in lines 38-39 and in line 40 the solutions are calculated by the solver. The result is a vector of configurations, Q.

The result is printed in lines 42 to 49, along with some relevant information.

Python

# To get the serial devices you must copy RobWorkData from gitlab.com like this:
# git clone https://gitlab.com/sdurobotics/robworkdata.git
#

from sdurw import *
import sys


WC_FILE = "/devices/serialdev/UR10e_2018/UR10e.xml"

if __name__ == '__main__':
    if len(sys.argv) != 2:
        print("Usage: python3 " + sys.argv[0] + " <path/to/RobWorkData>")
        sys.exit(1)

    wc = WorkCellLoaderFactory.load(sys.argv[1] + WC_FILE)
    if wc.isNull():
        raise Exception("WorkCell could not be loaded")
    device = wc.findSerialDevice("UR10e")
    if device.isNull():
        raise Exception("UR10e device could not be found.")

    defState = wc.getDefaultState()
    solver = ClosedFormIKSolverUR(device.cptr(), defState)

    Tdesired = Transform3D(Vector3D(0.2, -0.2, 0.5), EAA(0, Pi, 0).toRotation3D())
    solutions = solver.solve(Tdesired, defState)

    print("Inverse Kinematics for " + device.getName() + ".")
    print(" Base frame: " + device.getBase().getName())
    print(" End/TCP frame: " + solver.getTCP().getName())
    print(" Target Transform: " + str(Tdesired))
    print("Found " + str(len(solutions)) + " solutions.")
    for solution in solutions:
        print(" " + str(solution))

In lines 7-13, the WorkCell is loaded. A SerialDevice is retrieved from the WorkCell in lines 14-16. Always remember to check for null pointers.

In lines 18-19 the default state is retrieved from the WorkCell, and the closed form solver is constructed.

The desired transformation is defined in line 21 and in line 22 the solutions are calculated by the solver. The result is a vector of configurations, Q.

The result is printed in lines 24 to 30, along with some relevant information.

Note

Before Python can find the rw module, you must add the the RobWork/libs/BUILD_TYPE directory to the PYTHONPATH environment variable.

Alternatively, you can import the sys module and call sys.path.append(‘RobWork/libs/BUILD_TYPE’) before importing the rw module.

Java

import java.lang.String;
import org.robwork.LoaderRW;
import org.robwork.sdurw.*;
import static org.robwork.sdurw.sdurwConstants.Pi;


public class ExInvKin {
    public static final String WC_FILE =
            "/devices/serialdev/UR10e_2018/UR10e.xml";

    public static void main(String[] args) throws Exception {
        LoaderRW.load("sdurw");

        if (args.length != 1) {
            System.out.print("Usage: java " + ExInvKin.class.getSimpleName());
            System.out.println(" <path/to/RobWorkData>");
            System.exit(1);
        }

        WorkCellPtr wc = WorkCellLoaderFactory.load(args[0] + WC_FILE);
        if (wc.isNull())
            throw new Exception("WorkCell could not be loaded.");
        SerialDevicePtr device = wc.findSerialDevice("UR10e");
        if (device.isNull())
            throw new Exception("UR10e device could not be found.");

        State state = wc.getDefaultState();
        ClosedFormIKSolverUR solver = new ClosedFormIKSolverUR(
                device.asSerialDeviceCPtr(), state);

        final Transform3Dd Tdesired = new Transform3Dd(new Vector3Dd(0.2, -0.2, 0.5),
                (new EAAd(0, Pi, 0)).toRotation3D());
        final QVector solutions = solver.solve(Tdesired, state);

        System.out.println("Inverse Kinematics for " + device.getName() + ".");
        System.out.println(" Base frame: " + device.getBase().getName());
        System.out.println(" End/TCP frame: " + solver.getTCP().getName());
        System.out.println(" Target Transform: " + Tdesired.toString());
        System.out.println("Found " + solutions.size() + " solutions.");
        for(int i = 0; i < solutions.size(); i++) {
            System.out.println(" " + solutions.get(i).toString());
        }
    }
}

In lines 13-21, the WorkCell is loaded. A SerialDevice is retrieved from the WorkCell in lines 22-24. Always remember to check for null pointers.

In lines 26-28 the default state is retrieved from the WorkCell, and the closed form solver is constructed.

The desired transformation is defined in lines 30-31 and in line 32 the solutions are calculated by the solver. The result is a vector of configurations, Q.

The result is printed in lines 34 to 41, along with some relevant information.

Note

Before you can compile or run the program, you must add the RobWork/libs/BUILD_TYPE/rw_java.jar file to the classpath. This can be done by setting the CLASSPATH environment variable or calling java and javac with the -cp option.

Java must be able to load the native JNI (Java Native Interface) library. This is what happens in line 11. Set the path with the option -Djava.library.path=”RobWork/libs/BUILD_TYPE/” when calling java. In Linux, the path can also be added to the LD_LIBRARY_PATH environment variable.

LUA

require("sdurw_core")
require("sdurw_math")
require("sdurw_kinematics")
require("sdurw_models")
require("sdurw_invkin")
require("sdurw_loaders")
require("sdurw")

using("sdurw_math")
using("sdurw_kinematics")
using("sdurw_models")
using("sdurw_invkin")
using("sdurw_loaders")
using("sdurw")

if #arg ~= 1 then
    print("Usage: lua ex-invkin.lua <path/to/RobWorkData>")
    return 1
end

local WC_FILE = "/devices/serialdev/UR10e_2018/UR10e.xml"

local wc = WorkCellLoaderFactory.load(arg[1] .. WC_FILE)
if wc:isNull() then
    error("WorkCell could not be loaded")
end
local device = wc:findSerialDevice("UR10e")
if device:isNull() then
    error("UR10e device could not be found.")
end

local state = wc:getDefaultState()
local solver = ClosedFormIKSolverUR(device:cptr(), state)

local Tdesired = Transform3D(Vector3D(0.2, -0.2, 0.5), EAA(0, Pi, 0):toRotation3D())
local solutions = solver:solve(Tdesired, state)

print("Inverse Kinematics for " .. device:getName() .. ".")
print(" Base frame: " .. device:getBase():getName())
print(" End/TCP frame: " .. solver:getTCP():getName())
print(" Target Transform: " .. tostring(Tdesired))
print("Found " .. solutions:size() .. " solutions.")
for i= 0,solutions:size()-1,1
do
    print(" " .. tostring(solutions[i]))
end

The rw module is imported in lines 1 and 2. Normally, all classes and functions must be called with the module as a prefix, such as rw.WorkCellLoaderFactory. To import all the names into the global namespace, the code in lines 2 can be used. Otherwise, you will have to use the “rw.” prefix.

In lines 4-14, the WorkCell is loaded. A SerialDevice is retrieved from the WorkCell in lines 15-18. Always remember to check for null pointers.

In lines 20-21 the default state is retrieved from the WorkCell, and the closed form solver is constructed.

The desired transformation is defined in line 18 and in line 19 the solutions are calculated by the solver. The result is a vector of configurations, Q.

The result is printed in lines 26 to 34, along with some relevant information.

Note

Before you can run the script, you must add the RobWork lua modules to the LUA_CPATH environment variable.

In Linux this could for instance look like this (imports all rw modules):

export LUA_CPATH=”/path/to/RobWork/libs/release/Lua/?.so”

Output

The output from any of the programs above will be the following:

Inverse Kinematics for UR10e.
 Base frame: UR10e.Base
 End/TCP frame: UR10e.Flange
 Target Transform: Transform3D(Vector3D(0.2, -0.2, 0.5), Rotation3D(-1, 0, 1.22465e-16, 0, 1, 0, -1.22465e-16, 0, -1))
Found 8 solutions.
 Q[6]{-0.12278, 3.02845, 2.16872, -0.484776, -1.5708, -1.69358}
 Q[6]{-0.12278, -1.21998, -2.16872, 1.8179, -1.5708, -1.69358}
 Q[6]{-0.12278, -2.90011, 2.36859, 2.10231, 1.5708, 1.44802}
 Q[6]{-0.12278, -0.705404, -2.36859, -1.63839, 1.5708, 1.44802}
 Q[6]{1.69358, -2.43619, 2.36859, -1.5032, -1.5708, 0.12278}
 Q[6]{1.69358, -0.241482, -2.36859, 1.03928, -1.5708, 0.12278}
 Q[6]{1.69358, -1.92161, 2.16872, 1.3237, 1.5708, -3.01881}
 Q[6]{1.69358, 0.113143, -2.16872, -2.65682, 1.5708, -3.01881}