# 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 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 #include #include #include #include #include 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); } } 

Python

  1 2 3 4 5 6 7 8 9 10 11 12 13 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); 

Java

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 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); } } } 

LUA

  1 2 3 4 5 6 7 8 9 10 11 12 13 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 

## 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 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 #include #include #include #include #include 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 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++

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 #include #include #include 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 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(), Transform3Dd.identity(), 0); 

Java

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 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++

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

API Reference: rw::kinematics::FKTable

Python

 1 2 3 4 5 6 7 8 9 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 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 iterator = frames.iterator(); while(iterator.hasNext()) { Frame frame = iterator.next(); result.add(fk.get(frame)); } return result; } } 

API Reference:

LUA

 1 2 3 4 5 6 7 8 9 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++

  1 2 3 4 5 6 7 8 9 10 11 #include 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

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

API Reference:

• rw.FKRange

Java

  1 2 3 4 5 6 7 8 9 10 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:

LUA

 1 2 3 4 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++

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

Python

 1 2 3 4 5 6 7 8 9 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

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 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 iterator = states.iterator(); while(iterator.hasNext()) { State state = iterator.next(); result.add(fk.get(state)); } return result; } } 

LUA

 1 2 3 4 5 6 7 8 9 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:

 1 2 3 4 5 6 7 8 #include 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:

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 #include #include #include 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.