Rock

the Robot Construction Kit

Frame Tree

## Frame Tree

The frame tree stores information about cartesian reference frames that are available in the environment. Every environment has a root frame, which can be retrieved from an environment object using the `getRootNode()` method.

Additional frames can be added to the frame tree using one of the two methods

``````FrameNode *childFrame = new FrameNode(
Eigen::Transform3d( Eigen::Translation3d( 0, 0, 1.0 ) ) );

env->addChild( env->getRootNode(), childFrame )
``````

or alternatively

``````env->getRootNode()->addChild( childFrame );
``````

The second method obviously only works if the parent framenode is already attached to the environment.

## Transformation

The transformation given in the constructor of the FrameNode denotes the transformation from the given frame to the parent frame. For the above example, a point with (0, 0, 0) in childFrame would be (0, 0, 1.0) in the parentFrame (in this case the root Frame).

The transformation can be accessed using the `getTransform` and `setTransform` methods of the FrameNode.

In order to obtain the transformation between two arbitrary frames (both of which need to be connected to the same frame tree) the `relativeTransform` method can be used.

``````Transform3d C_a2b = env->relativeTransform( frameA, frameB );
``````

Will return the transform C_a2b, which given a vector v in frame A would provide v’ = C_a2b * v, the same vector in frame B.

## Uncertainty

The frame tree also supports uncertainty, but special methods have to be used for that. Using the `setTransformWithUncertainty` method of the frame allows you to provide a gaussian uncertainty of the transformation. The class for that is `TransformWithUncertainty`, which additionaly to the transformation stores the covariance of the position and rotation (as a scaled axis rotation) as a 6x6 matrix.

Chaining of transformations with uncertainty is also available, so you can see how the uncertainty propages through the frames. A simple example would be the following

``````// create a point with uncertainty.
// we provide sigma values of 0.01 m, which have to be squared,
// since the class is expecting a covariance
PointWithUncertainty p(
Eigen::Vector3d( 0.0, 0, 0 ),
Eigen::Vector3d( 0.01, 0.01, 0.01 )
.array().square().matrix().asDiagonal() );

// create a 6x6 covariance matrix
Eigen::Matrix<double,6,1> lt1;
lt1 <<
0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

// create a transform with uncertainty from the given
// diagonal of sigma values
TransformWithUncertainty t1(
Eigen::Affine3d(
Eigen::Translation3d(Eigen::Vector3d(r,0,0))),
lt1.array().square().matrix().asDiagonal() );

// and a second one
Eigen::Matrix<double,6,1> lt2;
lt2 <<
0.0, 0.0, 0.1, 0.0, 0.0, 0.0;

TransformWithUncertainty t2(
Eigen::Affine3d(
Eigen::Affine3d::Identity()),
lt2.array().square().matrix().asDiagonal );

// The point transformation is the same as without uncertainty,
// but the uncertainty information is also propagated
PointWithUncertainty pp = t2 * t1 * p;
``````

The same works when storing the `TransformationWithUncertainty` in the frame tree. Instead of `relativeTransform`, the `relativeTransformWithUncertainty` can be used.