Kinematics

The basic building blocks of a WorkCell are Frames. These are ordered in a tree-structure where the root node always is the WORLD frame. All frames has a parent which their position and orientation is relative to. Descending in this tree accumulating frame transformations is basically forward kinematics. The Kinematics class is a utility class for calculating forward kinematics.

Forward Kinematics

The examples below show how to calculate the transformation between two frames in the WorkCell. The final lines shows how a DAF frame can be attached to another frame. In this case the mframe is attached to the end frame of a serial device.

C++

#include <rw/kinematics/MovableFrame.hpp>
#include <rw/kinematics/Kinematics.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/models/SerialDevice.hpp>

using namespace rw::kinematics;
using rw::math::Transform3D;
using rw::models::SerialDevice;

void fwdKinematics(SerialDevice::Ptr sdevice,
    Frame* frame, MovableFrame* mframe, State& state)
{
    // calculate the transform from one frame to another
    Transform3D<> fTmf = Kinematics::frameTframe(frame, mframe, state);
    // calculate the transform from world to frame
    Transform3D<> wTmf = Kinematics::worldTframe( mframe, state );
    // we can find the world to frame transform by a little jogling
    Transform3D<> wTf = wTmf * inverse(fTmf);
    // test if frame is a dynamic attachable frame
    if( Kinematics::isDAF( mframe ) ){
       // attach mframe to end of serial device
       Kinematics::gripFrame(mframe, sdevice->getEnd(), state);
    }
}

See C++ Interface for more information about compilation and execution.

Python

from sdurw import *

def fwdKinematics(device, frame, mframe, state):
    # calculate the transform from one frame to another
    fTmf = Kinematics.frameTframe(frame, mframe, state);
    # calculate the transform from world to frame
    wTmf = Kinematics.worldTframe( mframe, state );
    # we can find the world to frame transform by a little jogling
    wTf = wTmf * inverse(fTmf);
    # test if frame is a dynamic attachable frame
    if Kinematics.isDAF( mframe ):
       # attach mframe to end of serial device
       Kinematics.gripFrame(mframe, sdevice.getEnd(), state);

See Python interface for more information about execution.

Java

import org.robwork.sdurw.*;
import static org.robwork.sdurw.sdurw.inverse;

public class ExFwdKinematics {
    public static void fwdKinematics(SerialDevicePtr sdevice,
            Frame frame, MovableFrame mframe, State state)
    {
        // calculate the transform from one frame to another
        Transform3Dd fTmf = Kinematics.frameTframe(frame, mframe, state);
        // calculate the transform from world to frame
        Transform3Dd wTmf = Kinematics.worldTframe( mframe, state );
        // we can find the world to frame transform by a little jogling
        Transform3Dd wTf = wTmf.multiply(inverse(fTmf));
        // test if frame is a dynamic attachable frame
        if( Kinematics.isDAF( mframe ) ){
           // attach mframe to end of serial device
           Kinematics.gripFrame(mframe, sdevice.getEnd(), state);
        }
    }
}

See Java Interface for more information about compilation and execution.

LUA

function fwdKinematics(device, frame, mframe, state)
    -- calculate the transform from one frame to another
    fTmf = Kinematics.frameTframe(frame, mframe, state);
    -- calculate the transform from world to frame
    wTmf = Kinematics.worldTframe( mframe, state );
    -- we can find the world to frame transform by a little jogling
    wTf = wTmf * inverse(fTmf);
    -- test if frame is a dynamic attachable frame
    if Kinematics.isDAF( mframe ) then
       -- attach mframe to end of serial device
       Kinematics.gripFrame(mframe, sdevice:getEnd(), state);
    end
end

See Lua Interface for more information about execution of the script.

Device Kinematics

The device class also define utility functions for calculating forward kinematics, at least those that relate to the device. Additionally the Device has functionality to compute the Device Jacobian, setting and getting the joint configurations and getting the joint limits.

C++

#include <rw/kinematics/MovableFrame.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/math/Q.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/models/SerialDevice.hpp>

using namespace rw::kinematics;
using namespace rw::math;
using rw::models::SerialDevice;

void fwdKinematicsDevice(SerialDevice::Ptr sdevice, MovableFrame* mframe, State& state)
{
    // get device base frame
    Frame *base = sdevice->getBase();
    // get device end effector
    Frame *end = sdevice->getEnd();
    // calculate base to end transform
    Transform3D<> bTe = sdevice->baseTend(state);
    // or just base to any frame
    Transform3D<> bTmf = sdevice->baseTframe(mframe, state);
    // get device name
    std::string sdevicename = sdevice->getName();
    // the degrees of freedom of this device
    int dof = sdevice->getDOF();
    // set the configuration of the device to zero
    sdevice->setQ( Q::zero(dof) , state );
}

Python

from sdurw import *

def fwdKinematicsDevice(sdevice, mframe, state):
    # get device base frame
    base = sdevice.getBase();
    # get device end effector
    end = sdevice.getEnd();
    # calculate base to end transform
    bTe = sdevice.baseTend(state);
    # or just base to any frame
    bTmf = sdevice.baseTframe(mframe, state);
    # get device name
    sdevicename = sdevice.getName();
    # the degrees of freedom of this device
    dof = sdevice.getDOF();
    # set the configuration of the device to zero
    sdevice.setQ( Q(dof,0.0) , state );

Java

import org.robwork.sdurw.*;
import org.robwork.sdurw.Q;
import static org.robwork.sdurw.sdurw.inverse;

public class ExFwdKinematicsDevice {
    public static void fwdKinematicsDevice(SerialDevicePtr sdevice, MovableFrame mframe, State state)
    {
        // get device base frame
        Frame base = sdevice.getBase();
        // get device end effector
        Frame end = sdevice.getEnd();
        // calculate base to end transform
        Transform3Dd bTe = sdevice.baseTend(state);
        // or just base to any frame
        Transform3Dd bTmf = sdevice.baseTframe(mframe, state);
        // get device name
        String sdevicename = sdevice.getName();
        // the degrees of freedom of this device
        long dof = sdevice.getDOF();
        // set the configuration of the device to zero
        sdevice.setQ( new Q((int)dof,0.0) , state );
    }
}

LUA

function fwdKinematicsDevice(device, mframe, state)
    -- get device base frame
    base = sdevice:getBase();
    -- get device end effector
    endFrame = sdevice:getEnd();
    -- calculate base to end transform
    bTe = sdevice:baseTend(state);
    -- or just base to any frame
    bTmf = sdevice:baseTframe(mframe, state);
    -- get device name
    sdevicename = sdevice:getName();
    -- the degrees of freedom of this device
    dof = sdevice:getDOF();
    -- set the configuration of the device to zero
    sdevice:setQ( Q(dof,0.0) , state );
end

Kinematics trees and states

The kinematic structure of the work cell is represented by a tree of frames (see rw::kinematics::Frame). The root of the kinematic tree is called the world frame (rw::models::WorkCell::getWorldFrame()). Each frame has a transformation (see rw::math::Transform3D) relative to its parent frame and this transformation may change in response to values assigned for the frame. A revolute joint of a device (see rw::models::RevoluteJoint) is for example implemented as a frame that has a single value that rotates the frame relative to its parent. Besides revolute joints RobWork also supports prismatic joints and dependent joints for which the value may depend on other joints.

It is important in RobWork to note that the values for the frames are not stored within the frames, but are instead stored explicitly in a value of type rw::kinematics::State. Given a state for the workcell, the transform of a frame relative to its parent can be calculated with rw::kinematics::Frame::getTransform().

The frames of the workcell are always organized in a tree, but for certain frames the parent that they are connected to can dynamically be changed. These frames are called dynamically attachable frames or DAFs for short. The parent that a DAF is attached to is not stored within the DAF itself, but is instead stored externally in a State value. Different state values can thus correspond to different structures of the tree. Given a state for the workcell the parent and children of a frame can be retrieved with rw::kinematics::Frame::getParent() and rw::kinematics::Frame::getChildren().

Because the values of the frames and the attachments of DAFs are stored outside of the workcell, we say that the workcell is stateless. This enables a workcell and the associated data to be used concurrently in multiple threads as well as to easily communicate the entire state of a workcell.

To illustrate these important ideas, this example shows how to print the structure of the kinematic tree of the workcell and for each frame print also the position of the frame in space:

C++

#include <rw/kinematics/Frame.hpp>
#include <rw/models/WorkCell.hpp>


#include <string>

using namespace rw::kinematics;
using rw::math::Transform3D;
using rw::models::WorkCell;

void printKinematicTree (Frame& frame, const State& state, const Transform3D<>& parentTransform,
                         int level)
{
    const Transform3D<> transform = parentTransform * frame.getTransform (state);

    std::cout << std::string (level, ' ') << frame.getName () << " at " << transform.P ()
              << std::endl;

    for (Frame::Ptr& child : frame.getChildrenList (state)) {
        printKinematicTree (*child, state, transform, level + 1);
    }
}

void printDefaultWorkCellStructure (const WorkCell& workcell)
{
    std::cout << workcell << std::endl;
    printKinematicTree (
        *workcell.getWorldFrame (), workcell.getDefaultState (), Transform3D<>::identity (), 0);
}

Python

from sdurw import *

def printKinematicTree(frame, state, parentTransform, level):
    transform = parentTransform * frame.getTransform(state);

    for i in range(level): 
        print(' ', end = '')

    print(frame.getName() + " at " + str(transform.P()))

    for child in frame.getChildren(state):
        printKinematicTree(child, state, transform, level + 1);

def printDefaultWorkCellStructure(workcell):
    printKinematicTree(
        workcell.getWorldFrame(),
        workcell.getDefaultState(),
        Transform3D.identity(),
        0);

Java

import org.robwork.sdurw.*;

public class ExPrintKinematicTree {
    public static void printKinematicTree(
            Frame frame,
            State state,
            Transform3Dd parentTransform,
            int level)
    {
        final Transform3Dd transform = parentTransform.multiply(frame.getTransform(state));

        for (int i = 0; i < level; i++) {
            System.out.print(" ");
        }
        System.out.println(frame.getName() + " at " + transform.P());

        FrameVector children = frame.getChildren(state);
        for (int i = 0; i < children.size(); i++) {
            printKinematicTree(children.get(i), state, transform, level + 1);
        }
    }

    public static void printDefaultWorkCellStructure(WorkCellPtr workcell)
    {
        printKinematicTree(
                workcell.getWorldFrame(),
                workcell.getDefaultState(),
                Transform3Dd.identity(),
                0);
    }

}

LUA

function printKinematicTree(frame, state, parentTransform, level)
    transform = parentTransform * frame:getTransform(state);

    indent = ""
    for i=1,level do
        indent = indent .. " "
    end

    print(indent .. frame:getName() .. " at " .. tostring(transform:P()))

    local children = frame:getChildren(state)
    if not children:empty() then
        for i = 0,children:size()-1 do
            child = children[i]
            printKinematicTree(child, state, transform, level + 1);
        end
    end
end

function printDefaultWorkCellStructure(workcell)
    printKinematicTree(
        workcell:getWorldFrame(),
        workcell:getDefaultState(),
        Transform3Dd.identity(),
        0);
end

We see from this example that given a state, it is straight-forward to compute the transform of every single frame in the workcell. RobWork has some utilities to make calculation of forward kinematics convenient in the day to day work, such a rw::kinematics::FKTable and rw::kinematics::FKRange described below.

World transforms for a set of frames

rw::kinematics::FKTable computes the forward kinematics for a number of frames for a common state. The results of the forward kinematics are stored in the FKTable object so that the transform for a frame is not computed over and over again. This example shows how the transform for a sequence of frames can be efficiently computed:

C++

#include <rw/kinematics/FKTable.hpp>

#include <vector>

using rw::math::Transform3D;
using namespace rw::kinematics;

std::vector<Transform3D<> > worldTransforms(
    const std::vector<const Frame*>& frames, const State& state)
{
    FKTable fk(state);

    std::vector<Transform3D<> > result;
    for(const Frame* const f : frames) {
        result.push_back(fk.get(*f));
    }
    return result;
}

API Reference: rw::kinematics::FKTable

Python

from sdurw import *

def worldTransforms(frames, state):
    fk = FKTable(state);
    
    result = Transform3DdVector();
    for frame in frames:
        result.push_back(fk.get(frame));
    return result;

API Reference:

• rw.FKTable

Java

import java.util.Iterator;

import org.robwork.sdurw.*;

public class ExWorldTransforms {
    public static Transform3DdVector worldTransforms(
            FrameVector frames, State state)
    {
        FKTable fk = new FKTable(state);

        Transform3DdVector result = new Transform3DdVector();
        Iterator<Frame> iterator = frames.iterator();
        while(iterator.hasNext()) {
            Frame frame = iterator.next();
            result.add(fk.get(frame));
        }
        return result;
    }
}

API Reference:

• org.robwork.rw.FKTable (Javadoc)

LUA

function worldTransforms(frames, state)
    fk = FKTable(state);

    result = Transform3DdVector()
    for i = 0,frames:size()-1 do
        result:push_back(fk:get(frames[i]))
    end
    return result
end

Relative transforms for a pair of frames

rw::kinematics::FKRange computes the relative transform for a pair of frames. To efficiently compute the relative transform for a pair of frames the path in the kinematic tree that connects the frames must be computed. Knowing the path and the relative transform between adjacent frames of the path (rw::kinematics::Frame::getTransform()) the full transform from start to end of the path can be computed. This example shows the use of rw::kinematics::FKRange:

C++

#include <rw/kinematics/FKRange.hpp>

using rw::math::Transform3D;
using namespace rw::kinematics;

Transform3D<> frameToFrameTransform(
    const Frame& a, const Frame& b, const State& state)
{
    FKRange fk(&a, &b, state);
    return fk.get(state);
}

API Reference: rw::kinematics::FKRange

Python

from sdurw import *

def frameToFrameTransform(a, b, state):
    fk = FKRange(a, b, state);
    return fk.get(state);

API Reference:

• rw.FKRange

Java

import org.robwork.sdurw.*;

public class ExFrameToFrameTransform {
    public static Transform3Dd frameToFrameTransform(
            Frame a, Frame b, State state)
    {
        FKRange fk = new FKRange(a, b, state);
        return fk.get(state);
    }
}

API Reference:

• org.robwork.rw.FKRange (Javadoc)

LUA

function frameToFrameTransform(a, b, state)
    fk = FKRange(a, b, state)
    return fk:get(state)
end

If you repeatedly compute the forward kinematics for the same pair of frames and the same parent-child structure of the tree, you can reuse the rw::kinematics::FKRange object so that e.g. the path connecting the frames need not be recomputed. For example, given a pair of frames and a set of states the relative transforms that relate the frames can be computed efficiently as follows:

C++

#include <rw/kinematics/FKRange.hpp>
#include <rw/kinematics/State.hpp>

#include <vector>

using rw::math::Transform3D;
using namespace rw::kinematics;

std::vector<Transform3D<> > frameToFrameTransforms(
    const Frame& a,
    const Frame& b,
    const State& tree_structure,
    const std::vector<State>& states)
{
    FKRange fk(&a, &b, tree_structure);

    std::vector<Transform3D<> > result;
    for(const State& state : states) {
        result.push_back(fk.get(state));
    }
    return result;
}

Python

from sdurw import *

def frameToFrameTransforms(a, b, tree_structure, states):
    fk = FKRange(a, b, tree_structure);
    
    result = Transform3DdVector();
    for state in states:
        result.push_back(fk.get(state));
    return result;

Java

import java.util.Iterator;

import org.robwork.sdurw.*;

public class ExFrameToFrameTransforms {
    public static Transform3DdVector frameToFrameTransforms(
            Frame a, Frame b, State tree_structure, StateVector states)
    {
        FKRange fk = new FKRange(a, b, tree_structure);

        Transform3DdVector result = new Transform3DdVector();
        Iterator<State> iterator = states.iterator();
        while(iterator.hasNext()) {
            State state = iterator.next();
            result.add(fk.get(state));
        }
        return result;
    }
}

LUA

function frameToFrameTransforms(a, b, tree_structure, states)
    fk = FKRange(a, b, tree_structure);

    result = Transform3DdVector()
    for i = 0,states:size()-1 do
        result:push_back(fk:get(states[i]))
    end
    return result
end

The frameToFrameTransform() utility function is available as rw::kinematics::Kinematics::frameTframe().

Dynamically attachable frames and movable frames

A dynamically attachable frame (DAF) is a frame for which the parent frame can be changed. We say that the frame is attached to a new parent frame (rw::kinematics::Frame::attachFrame()). A DAF can be attached to any frame of the workcell except itself. You should avoid attaching a DAF to a child of its subtree as this will create a cycle in the kinematic structure. Frames of any type can be a DAF. You can check if a frame is a DAF like this:

#include <rw/kinematics/Frame.hpp>

using namespace rw::kinematics;

bool isDaf(const Frame& frame)
{
    return frame.getParent() == NULL;
}

DAFs are used for example to simulate the picking up of an item by a gripper. The item is represented by a DAF and is initially attached to some frame of the workcell. When the gripper is closed, the picking up of the item is simulated by attaching the item frame to the gripper frame.

If the parent frame of a DAF is changed, the world transform of the DAF will generally change also. When simulating the picking up of an item, you do not want the item to instantly change position in space. Therefore a DAF is often a movable frame (rw::kinematics::MovableFrame) also. A movable frame is a frame for which an arbitrary transform can be set for the transform of the frame relative to its parent (rw::kinematics::MovableFrame::setTransform()). To simulate the gripping of the item, the parent of the frame is set to the frame of the gripper, and at the same time the relative transform of the item is assigned a value that equals the transform from gripper to item. This procedure is carried out as follows:

#include <rw/kinematics/MovableFrame.hpp>
#include <rw/kinematics/FKRange.hpp>
#include <rw/math/Transform3D.hpp>

using rw::math::Transform3D;
using namespace rw::kinematics;

void gripMovableFrame(
    MovableFrame& item, Frame& gripper, State& state)
{
    FKRange fk(&gripper, &item, state);
    const Transform3D<> transform = fk.get(state);
    item.setTransform(transform, state);
    item.attachTo(&gripper, state);
}

The function receives the current state of the workcell as input and updates this state to reflect the gripping of the item. Recall that the frames themselves are stateless: The attachment of the DAF and its change of relative transform is stored entirely within the state.

RobWork provides utilities for the above in the form of the rw::kinematics::Kinematics::gripFrame() and rw::kinematics::Kinematics::gripMovableFrame() collection of functions.