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++

``` 1#include <rw/kinematics/MovableFrame.hpp>
2#include <rw/kinematics/Kinematics.hpp>
3#include <rw/kinematics/State.hpp>
4#include <rw/math/Transform3D.hpp>
5#include <rw/models/SerialDevice.hpp>
6
7using namespace rw::kinematics;
8using rw::math::Transform3D;
9using rw::models::SerialDevice;
10
11void fwdKinematics(SerialDevice::Ptr sdevice,
12    Frame* frame, MovableFrame* mframe, State& state)
13{
14    // calculate the transform from one frame to another
15    Transform3D<> fTmf = Kinematics::frameTframe(frame, mframe, state);
16    // calculate the transform from world to frame
17    Transform3D<> wTmf = Kinematics::worldTframe( mframe, state );
18    // we can find the world to frame transform by a little jogling
19    Transform3D<> wTf = wTmf * inverse(fTmf);
20    // test if frame is a dynamic attachable frame
21    if( Kinematics::isDAF( mframe ) ){
22       // attach mframe to end of serial device
23       Kinematics::gripFrame(mframe, sdevice->getEnd(), state);
24    }
25}
```

Python

``` 1from sdurw import *
2
3def fwdKinematics(device, frame, mframe, state):
4    # calculate the transform from one frame to another
5    fTmf = Kinematics.frameTframe(frame, mframe, state);
6    # calculate the transform from world to frame
7    wTmf = Kinematics.worldTframe( mframe, state );
8    # we can find the world to frame transform by a little jogling
9    wTf = wTmf * inverse(fTmf);
10    # test if frame is a dynamic attachable frame
11    if Kinematics.isDAF( mframe ):
12       # attach mframe to end of serial device
13       Kinematics.gripFrame(mframe, sdevice.getEnd(), state);
```

Java

``` 1import org.robwork.sdurw.*;
2import static org.robwork.sdurw.sdurw.inverse;
3
4public class ExFwdKinematics {
5    public static void fwdKinematics(SerialDevicePtr sdevice,
6            Frame frame, MovableFrame mframe, State state)
7    {
8        // calculate the transform from one frame to another
9        Transform3Dd fTmf = Kinematics.frameTframe(frame, mframe, state);
10        // calculate the transform from world to frame
11        Transform3Dd wTmf = Kinematics.worldTframe( mframe, state );
12        // we can find the world to frame transform by a little jogling
13        Transform3Dd wTf = wTmf.multiply(inverse(fTmf));
14        // test if frame is a dynamic attachable frame
15        if( Kinematics.isDAF( mframe ) ){
16           // attach mframe to end of serial device
17           Kinematics.gripFrame(mframe, sdevice.getEnd(), state);
18        }
19    }
20}
```

LUA

``` 1function fwdKinematics(device, frame, mframe, state)
2    -- calculate the transform from one frame to another
3    fTmf = Kinematics.frameTframe(frame, mframe, state);
4    -- calculate the transform from world to frame
5    wTmf = Kinematics.worldTframe( mframe, state );
6    -- we can find the world to frame transform by a little jogling
7    wTf = wTmf * inverse(fTmf);
8    -- test if frame is a dynamic attachable frame
9    if Kinematics.isDAF( mframe ) then
10       -- attach mframe to end of serial device
11       Kinematics.gripFrame(mframe, sdevice:getEnd(), state);
12    end
13end
```

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++

``` 1#include <rw/kinematics/MovableFrame.hpp>
2#include <rw/kinematics/State.hpp>
3#include <rw/math/Q.hpp>
4#include <rw/math/Transform3D.hpp>
5#include <rw/models/SerialDevice.hpp>
6
7using namespace rw::kinematics;
8using namespace rw::math;
9using rw::models::SerialDevice;
10
11void fwdKinematicsDevice(SerialDevice::Ptr sdevice, MovableFrame* mframe, State& state)
12{
13    // get device base frame
14    Frame *base = sdevice->getBase();
15    // get device end effector
16    Frame *end = sdevice->getEnd();
17    // calculate base to end transform
18    Transform3D<> bTe = sdevice->baseTend(state);
19    // or just base to any frame
20    Transform3D<> bTmf = sdevice->baseTframe(mframe, state);
21    // get device name
22    std::string sdevicename = sdevice->getName();
23    // the degrees of freedom of this device
24    int dof = sdevice->getDOF();
25    // set the configuration of the device to zero
26    sdevice->setQ( Q::zero(dof) , state );
27}
```

Python

``` 1from sdurw import *
2
3def fwdKinematicsDevice(sdevice, mframe, state):
4    # get device base frame
5    base = sdevice.getBase();
6    # get device end effector
7    end = sdevice.getEnd();
8    # calculate base to end transform
9    bTe = sdevice.baseTend(state);
10    # or just base to any frame
11    bTmf = sdevice.baseTframe(mframe, state);
12    # get device name
13    sdevicename = sdevice.getName();
14    # the degrees of freedom of this device
15    dof = sdevice.getDOF();
16    # set the configuration of the device to zero
17    sdevice.setQ( Q(dof,0.0) , state );
```

Java

``` 1import org.robwork.sdurw.*;
2import org.robwork.sdurw.Q;
3import static org.robwork.sdurw.sdurw.inverse;
4
5public class ExFwdKinematicsDevice {
6    public static void fwdKinematicsDevice(SerialDevicePtr sdevice, MovableFrame mframe, State state)
7    {
8        // get device base frame
9        Frame base = sdevice.getBase();
10        // get device end effector
11        Frame end = sdevice.getEnd();
12        // calculate base to end transform
13        Transform3Dd bTe = sdevice.baseTend(state);
14        // or just base to any frame
15        Transform3Dd bTmf = sdevice.baseTframe(mframe, state);
16        // get device name
17        String sdevicename = sdevice.getName();
18        // the degrees of freedom of this device
19        long dof = sdevice.getDOF();
20        // set the configuration of the device to zero
21        sdevice.setQ( new Q((int)dof,0.0) , state );
22    }
23}
```

LUA

``` 1function fwdKinematicsDevice(device, mframe, state)
2    -- get device base frame
3    base = sdevice:getBase();
4    -- get device end effector
5    endFrame = sdevice:getEnd();
6    -- calculate base to end transform
7    bTe = sdevice:baseTend(state);
8    -- or just base to any frame
9    bTmf = sdevice:baseTframe(mframe, state);
10    -- get device name
11    sdevicename = sdevice:getName();
12    -- the degrees of freedom of this device
13    dof = sdevice:getDOF();
14    -- set the configuration of the device to zero
15    sdevice:setQ( Q(dof,0.0) , state );
16end
```

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++

``` 1#include <rw/kinematics/Frame.hpp>
2#include <rw/models/WorkCell.hpp>
3
4
5#include <string>
6
7using namespace rw::kinematics;
8using rw::math::Transform3D;
9using rw::models::WorkCell;
10
11void printKinematicTree (Frame& frame, const State& state, const Transform3D<>& parentTransform,
12                         int level)
13{
14    const Transform3D<> transform = parentTransform * frame.getTransform (state);
15
16    std::cout << std::string (level, ' ') << frame.getName () << " at " << transform.P ()
17              << std::endl;
18
19    for (Frame::Ptr& child : frame.getChildrenList (state)) {
20        printKinematicTree (*child, state, transform, level + 1);
21    }
22}
23
24void printDefaultWorkCellStructure (const WorkCell& workcell)
25{
26    std::cout << workcell << std::endl;
27    printKinematicTree (
28        *workcell.getWorldFrame (), workcell.getDefaultState (), Transform3D<>::identity (), 0);
29}
```

Python

``` 1from sdurw import *
2
3def printKinematicTree(frame, state, parentTransform, level):
4    transform = parentTransform * frame.getTransform(state);
5
6    for i in range(level):
7        print(' ', end = '')
8
9    print(frame.getName() + " at " + str(transform.P()))
10
11    for child in frame.getChildren(state):
12        printKinematicTree(child, state, transform, level + 1);
13
14def printDefaultWorkCellStructure(workcell):
15    printKinematicTree(
16        workcell.getWorldFrame(),
17        workcell.getDefaultState(),
18        Transform3D.identity(),
19        0);
```

Java

``` 1import org.robwork.sdurw.*;
2
3public class ExPrintKinematicTree {
4    public static void printKinematicTree(
5            Frame frame,
6            State state,
7            Transform3Dd parentTransform,
8            int level)
9    {
10        final Transform3Dd transform = parentTransform.multiply(frame.getTransform(state));
11
12        for (int i = 0; i < level; i++) {
13            System.out.print(" ");
14        }
15        System.out.println(frame.getName() + " at " + transform.P());
16
17        FrameVector children = frame.getChildren(state);
18        for (int i = 0; i < children.size(); i++) {
19            printKinematicTree(children.get(i), state, transform, level + 1);
20        }
21    }
22
23    public static void printDefaultWorkCellStructure(WorkCellPtr workcell)
24    {
25        printKinematicTree(
26                workcell.getWorldFrame(),
27                workcell.getDefaultState(),
28                Transform3Dd.identity(),
29                0);
30    }
31
32}
```

LUA

``` 1function printKinematicTree(frame, state, parentTransform, level)
2    transform = parentTransform * frame:getTransform(state);
3
4    indent = ""
5    for i=1,level do
6        indent = indent .. " "
7    end
8
9    print(indent .. frame:getName() .. " at " .. tostring(transform:P()))
10
11    local children = frame:getChildren(state)
12    if not children:empty() then
13        for i = 0,children:size()-1 do
14            child = children[i]
15            printKinematicTree(child, state, transform, level + 1);
16        end
17    end
18end
19
20function printDefaultWorkCellStructure(workcell)
21    printKinematicTree(
22        workcell:getWorldFrame(),
23        workcell:getDefaultState(),
24        Transform3Dd.identity(),
25        0);
26end
```

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++

``` 1#include <rw/kinematics/FKTable.hpp>
2
3#include <vector>
4
5using rw::math::Transform3D;
6using namespace rw::kinematics;
7
8std::vector<Transform3D<> > worldTransforms(
9    const std::vector<const Frame*>& frames, const State& state)
10{
11    FKTable fk(state);
12
13    std::vector<Transform3D<> > result;
14    for(const Frame* const f : frames) {
15        result.push_back(fk.get(*f));
16    }
17    return result;
18}
```

API Reference: rw::kinematics::FKTable

Python

```1from sdurw import *
2
3def worldTransforms(frames, state):
4    fk = FKTable(state);
5
6    result = Transform3DdVector();
7    for frame in frames:
8        result.push_back(fk.get(frame));
9    return result;
```

API Reference:

• `rw.FKTable`

Java

``` 1import java.util.Iterator;
2
3import org.robwork.sdurw.*;
4
5public class ExWorldTransforms {
6    public static Transform3DdVector worldTransforms(
7            FrameVector frames, State state)
8    {
9        FKTable fk = new FKTable(state);
10
11        Transform3DdVector result = new Transform3DdVector();
12        Iterator<Frame> iterator = frames.iterator();
13        while(iterator.hasNext()) {
14            Frame frame = iterator.next();
16        }
17        return result;
18    }
19}
```

API Reference:

LUA

```1function worldTransforms(frames, state)
2    fk = FKTable(state);
3
4    result = Transform3DdVector()
5    for i = 0,frames:size()-1 do
6        result:push_back(fk:get(frames[i]))
7    end
8    return result
9end
```

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++

``` 1#include <rw/kinematics/FKRange.hpp>
2
3using rw::math::Transform3D;
4using namespace rw::kinematics;
5
6Transform3D<> frameToFrameTransform(
7    const Frame& a, const Frame& b, const State& state)
8{
9    FKRange fk(&a, &b, state);
10    return fk.get(state);
11}
```

API Reference: rw::kinematics::FKRange

Python

```1from sdurw import *
2
3def frameToFrameTransform(a, b, state):
4    fk = FKRange(a, b, state);
5    return fk.get(state);
```

API Reference:

• `rw.FKRange`

Java

``` 1import org.robwork.sdurw.*;
2
3public class ExFrameToFrameTransform {
4    public static Transform3Dd frameToFrameTransform(
5            Frame a, Frame b, State state)
6    {
7        FKRange fk = new FKRange(a, b, state);
8        return fk.get(state);
9    }
10}
```

API Reference:

LUA

```1function frameToFrameTransform(a, b, state)
2    fk = FKRange(a, b, state)
3    return fk:get(state)
4end
```

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++

``` 1#include <rw/kinematics/FKRange.hpp>
2#include <rw/kinematics/State.hpp>
3
4#include <vector>
5
6using rw::math::Transform3D;
7using namespace rw::kinematics;
8
9std::vector<Transform3D<> > frameToFrameTransforms(
10    const Frame& a,
11    const Frame& b,
12    const State& tree_structure,
13    const std::vector<State>& states)
14{
15    FKRange fk(&a, &b, tree_structure);
16
17    std::vector<Transform3D<> > result;
18    for(const State& state : states) {
19        result.push_back(fk.get(state));
20    }
21    return result;
22}
```

Python

```1from sdurw import *
2
3def frameToFrameTransforms(a, b, tree_structure, states):
4    fk = FKRange(a, b, tree_structure);
5
6    result = Transform3DdVector();
7    for state in states:
8        result.push_back(fk.get(state));
9    return result;
```

Java

``` 1import java.util.Iterator;
2
3import org.robwork.sdurw.*;
4
5public class ExFrameToFrameTransforms {
6    public static Transform3DdVector frameToFrameTransforms(
7            Frame a, Frame b, State tree_structure, StateVector states)
8    {
9        FKRange fk = new FKRange(a, b, tree_structure);
10
11        Transform3DdVector result = new Transform3DdVector();
12        Iterator<State> iterator = states.iterator();
13        while(iterator.hasNext()) {
14            State state = iterator.next();
16        }
17        return result;
18    }
19}
```

LUA

```1function frameToFrameTransforms(a, b, tree_structure, states)
2    fk = FKRange(a, b, tree_structure);
3
4    result = Transform3DdVector()
5    for i = 0,states:size()-1 do
6        result:push_back(fk:get(states[i]))
7    end
8    return result
9end
```

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:

```1#include <rw/kinematics/Frame.hpp>
2
3using namespace rw::kinematics;
4
5bool isDaf(const Frame& frame)
6{
7    return frame.getParent() == NULL;
8}
```

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:

``` 1#include <rw/kinematics/MovableFrame.hpp>
2#include <rw/kinematics/FKRange.hpp>
3#include <rw/math/Transform3D.hpp>
4
5using rw::math::Transform3D;
6using namespace rw::kinematics;
7
8void gripMovableFrame(
9    MovableFrame& item, Frame& gripper, State& state)
10{
11    FKRange fk(&gripper, &item, state);
12    const Transform3D<> transform = fk.get(state);
13    item.setTransform(transform, state);
14    item.attachTo(&gripper, state);
15}
```

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.