Frame Transformations

Frame Transformations#

Warning

This topic is under construction and this might not even be its final form. Please feel free to open an issue if you spot any typos or other problems.

Unless made of flexible materials, the robots we use and the objects they interact with can be well simplified as rigid bodies.

Terminology dictates that a position and an orientation are inherent elements of rigid bodies, related to information about their instantaneous reference frames. In this sense, a rotation and a translation would represent the position and the orientation of a rigid body with respect to a reference frame and that implies dislocation.

However, mathematically, positions and orientations are always represented with respect to a reference frame. I believe that, because of this, beyond this tutorial, in robotics and computer science, you will often see position/translation and orientation/rotation used interchangeably.

Please allow me to use only the terms translation and rotation henceforth to keep the discussion consistent with how it is utilised in tf2, namely, as a geometry_msgs/msg/TransformStamped.

We start by looking at how it is done in ROS2, go through the related mathematics, then create an example to show how to create these transformations.

Transformations in ROS2#

Let us take a look at message most commonly used in ROS2 to represent translation and rotation, TransformStamped.msg, part of the package geometry_msgs.

We can see its contents with the following command.

ros2 interface show geometry_msgs/msg/TransformStamped

It results in the slightly intricate message below.

To simplify the discussion, these are the contents of TransformStamped.msg.

# This expresses a transform from coordinate frame header.frame_id
# to the coordinate frame child_frame_id at the time of header.stamp
#
# This message is mostly used by the
# <a href="https://docs.ros.org/en/rolling/p/tf2/">tf2</a> package.
# See its documentation for more information.
#
# The child_frame_id is necessary in addition to the frame_id
# in the Header to communicate the full reference for the transform
# in a self contained message.

# The frame id in the header is used as the reference frame of this transform.
std_msgs/Header header

# The frame id of the child frame to which this transform points.
string child_frame_id

# Translation and rotation in 3-dimensions of child_frame_id from header.frame_id.
Transform transform

Note that it is a message composed of two other messages, and a built-in.

The Header is an essential timestamp used throughout ROS2. We can see its contents with the following command.

ros2 interface show std_msgs/msg/Header

The command results in the following output.

# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.

# Two-integer timestamp that is expressed as seconds and nanoseconds.
builtin_interfaces/Time stamp
        int32 sec
        uint32 nanosec

# Transform frame with which this data is associated.
string frame_id

The elements stamp and frame_id are used by many popular packages ROS2 to display and relate translational and rotational data of robots and objects.

Then, to the most important part of this section, we have the Transform.msg, whose contents can be obtained with the following command.

ros2 interface show geometry_msgs/msg/Transform

The command outputs the following.

# This represents the transform between two coordinate frames in free space.

Vector3 translation
        float64 x
        float64 y
        float64 z
Quaternion rotation
        float64 x 0
        float64 y 0
        float64 z 0
        float64 w 1

In this message type, the translation and rotation can be clearly seen.

Translations#

Translations \(\boldsymbol{t} \in \mathbb{R}^3\) can be easily represented using imaginary numbers, such as

(1)#\[\boldsymbol{t} \triangleq t_x\hat{\imath} + t_y\hat{\jmath} + t_z\hat{k},\]

where \(\hat{\imath}^2 = \hat{\jmath}^2 = \hat{k}^2 = \hat{\imath} \hat{\jmath} \hat{k} = -1\).

If you thought this would never be useful, think again. It’s about to be!

Rotations: Quaternions#

Note

I’m not explaining this (in a failed attempt) to show off. ROS2 represents rotations as quaternions, so this knowledge is needed in the short term as well.

Whereas translations are intuitive and can be easily processed with A-levels mathematics, rotations demand extra thought. However, the complexity is exaggerated in multiple sources to some extent.

In ROS2, rotations are represented as quaternions. They have several benefits over other representations, which you can see in related literature on quaternions. For robotics, it is easier to think of rotations directly in quaternions instead of relying on intermediary ways, such as Euler angles. The reason is that the particularities of quaternions will eventually catch up to you, no matter how long and how far one might try to run away from them.

Let’s rip the trademarked plaster brand out. The following equation represents the formation of a rotation quaternion.

(2)#\[\boldsymbol{r} \triangleq \cos\left(\frac{\phi}{2}\right) + \boldsymbol{v}\sin\left(\frac{\phi}{2}\right),\]

where \(\boldsymbol{v}^2=-1\). This means that the rotation axis \(\boldsymbol{v}\) can be any imaginary number such that \(||\boldsymbol{v}||=1\).

The easiest way to think about a rotation using quaternions is to think about the axis of rotation \(\boldsymbol{v}\) and the angle of rotation \(\phi\). Then, you construct the quaternion with the rotation quaternion formation law.

Examples

We can choose \(\phi=0\) and see how a quaternion represents no rotation. Regardless of rotation axis, this results in

\[\begin{split}\boldsymbol{r}_0 &\triangleq \cos\left(\frac{0}{2}\right) + \boldsymbol{v}\sin\left(\frac{0}{2}\right) \\ &= 1.\end{split}\]

Now, suppose that we have a rotation of \(\phi=\pi\) radians (angles always in radians, don’t forget!).

If we want such a rotation about the x-axis, we choose \(\boldsymbol{v}_1=\hat{\imath}\). Therefore, this rotation would be correctly represented by

\[\begin{split}\boldsymbol{r}_1 &\triangleq \cos\left(\frac{\pi}{2}\right) + \hat{\imath}\sin\left(\frac{\pi}{2}\right) \\ &= \hat{\imath}.\end{split}\]

If we want such a rotation about the z-axis, we choose \(\boldsymbol{v}_2=\hat{k}\), correctly represented by

\[\begin{split}\boldsymbol{r}_2 &\triangleq \cos\left(\frac{\pi}{2}\right) + \hat{k}\sin\left(\frac{\pi}{2}\right) \\ &= \hat{k}.\end{split}\]

Any \(\phi \in \mathbb{R}\) is acceptable and, more importantly, any \(\boldsymbol{v}\) is acceptable as long as it is imaginary and has norm one. For instance, \(\boldsymbol{v}_3=-\frac{\sqrt{2}}{2}\hat{\imath} + \frac{\sqrt{2}}{2}\hat{k}\) leads to the valid rotation quaternion

\[\begin{split}\boldsymbol{r}_3 &\triangleq \cos\left(\frac{\pi}{2}\right) + \left(-\frac{\sqrt{2}}{2}\hat{\imath} + \frac{\sqrt{2}}{2}\hat{k}\right)\sin\left(\frac{\pi}{2}\right) \\ &= \left(-\frac{\sqrt{2}}{2}\hat{\imath} + \frac{\sqrt{2}}{2}\hat{k}\right).\end{split}\]

Error

There are also common pitfalls.

Using a rotation axis that is not unit norm does not represent a rotation quaternion.

Using \(\boldsymbol{v}_4=-\hat{\imath} + \hat{k}\) is not a valid rotation quaternion because its norm is not one, \(||\boldsymbol{v}_4||=\sqrt{2}\).
Misunderstanding the rotation angle.

Note that, inside the quaternion, we have \(\frac{\phi}{2}\). For instance, the quaternion

\[\boldsymbol{r}_5 \triangleq \cos\left(\frac{\pi}{4}\right) + \hat{\jmath}\sin\left(\frac{\pi}{4}\right),\]

represents a rotation of \(\phi_{5} = \frac{\pi}{2}\) about the y-axis, not \(\frac{\pi}{4}\).

Sequential rotations#

Sequential rotations can be obtained via quaternion multiplication. For instance, to obtain the result of a rotation of \(\phi=\pi\) about the x-axis followed by a rotation of the same angle about the z-axis, we do

\[\boldsymbol{r}_{1,2} \triangleq \boldsymbol{r}_1\boldsymbol{r}_2,\]

which results, in this case,

\[\begin{split}\boldsymbol{r}_{1,2} &= \left[\cos\left(\frac{\pi}{2}\right) + \hat{\imath}\sin\left(\frac{\pi}{2}\right)\right]\left[\cos\left(\frac{\pi}{2}\right) + \hat{k}\sin\left(\frac{\pi}{2}\right)\right] \\ &= \hat{\imath}\hat{k} \\ &= -\hat{\jmath}.\end{split}\]

The product of any two quaternions can always be done algebraically, by respecting the multiplication between the basis elements. The result can be generalised by the quaternion multiplication. This generalization will not be repeated here as we will do it programmatically later.

Inverse rotations#

The inverse rotation can be obtained by flipping the axis of rotation. This is equivalent to obtaining the so-called quaternion conjugate of unit-norm quaternions.

(3)#\[\boldsymbol{r}^{*} \triangleq \cos\left(\frac{\phi}{2}\right) - \boldsymbol{v}\sin\left(\frac{\phi}{2}\right).\]

We can see that this is indeed the inverse rotation by noticing that the rotation multiplied by its conjugate results in \(1\), that is, the non rotation.

\[\begin{split}\boldsymbol{r}\boldsymbol{r}^{*} = \boldsymbol{r}^{*}\boldsymbol{r} &= \left[\cos\left(\frac{\phi}{2}\right) - \boldsymbol{v}\sin\left(\frac{\phi}{2}\right)\right]\left[\cos\left(\frac{\phi}{2}\right) + \boldsymbol{v}\sin\left(\frac{\phi}{2}\right)\right] \\ &= \cos^2\left(\frac{\phi}{2}\right) - \boldsymbol{v}\boldsymbol{v}\sin^2\left(\frac{\phi}{2}\right) \\ &= \cos^2\left(\frac{\phi}{2}\right) - (-1)\sin^2\left(\frac{\phi}{2}\right) \\ &= \cos^2\left(\frac{\phi}{2}\right) + \sin^2\left(\frac{\phi}{2}\right) \\ &= 1.\end{split}\]

Connecting these ideas#

For the purposes of this section, we want to connect a translation (in the code) to a translation (from the equation) and a rotation (in the code) to a rotation (from the equation).

For a translation, we have the following.

x will store the value of the term related to \(\hat{\imath}\) in \(\boldsymbol{t}\), that is \(t_x\).
y will store the value of the term is related to \(\hat{\jmath}\) in \(\boldsymbol{t}\), that is \(t_y\).
z will store the value of the term is related to \(\hat{k}\) in \(\boldsymbol{t}\), that is \(t_z\).

Example

To represent the following translation,

\[\boldsymbol{t}_1 \triangleq 1\hat{\imath} + 2\hat{\jmath} + 3\hat{k},\]

we would assign translation.x = 1, translation.y = 2, and translation.z = 3 in our program.

Similarly, for a rotation, we have the following.

x will store the value of the term related to \(\hat{\imath}\) in \(\boldsymbol{r}\).
y will store the value of the term is related to \(\hat{\jmath}\) in \(\boldsymbol{r}\).
z will store the value of the term is related to \(\hat{k}\) in \(\boldsymbol{r}\).

Lastly, we have, for a rotation,

w will store the value of the term not related to any imaginary unit.

Note

You might be wondering why w, when the other dimensions might be already natural to you. We need to give it a name in our programs so that we can store its data. In alphabetical order, w comes before the letters we usually use for the x, y, and z dimensions.

Example

To represent the following elementary rotation about the x-axis

\[\boldsymbol{r}_1 \triangleq \cos\left(\frac{\pi}{2}\right) + \hat{\imath}\sin\left(\frac{\pi}{2}\right),\]

we would assign rotation.w = cos(pi/2), rotation.x = sin(pi/2), rotation.y = 0, and rotation.z = 0 in our program.

Create the package#

We will create a package to showcase the transformations from the previous section. We use TransformedStamped as it will be useful right away, when we talk about tf2.

To see how this would work, programmatically, we start by creating the python_package_that_uses_geometry_msgs package. Note that it must depend on geometry_msgs.

cd ~/ros2_tutorial_workspace/src
ros2 pkg create python_package_that_uses_geometry_msgs \
--build-type ament_python \
--dependencies geometry_msgs

Package structure#

Highlighted below are the files that we will create or modify.

python_package_that_uses_geometry_msgs/
|-- package.xml
|-- python_package_that_uses_geometry_msgs
|   |-- __init__.py
|   `-- create_stamped_transforms_node.py
|-- resource
|   `-- python_package_that_uses_geometry_msgs
|-- setup.cfg
|-- setup.py
`-- test
    |-- test_copyright.py
    |-- test_flake8.py
    `-- test_pep257.py

Add sample code#

Create the following sample Python script. It will serve to show the operations we did earlier mathematically.

create_stamped_transforms_node.py

from math import cos, sin, pi
from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

class CreateStampedTransformsNode(Node):
    """A ROS2 Node that creates TransformStamped objects."""

    def __init__(self):
        super().__init__('create_stamped_transforms_node')

    def create_and_print_stamped_transform(self):
        """Basic method showing the creation of TransformStamped objects."""

        tfs = TransformStamped()

        # Initialize header with
        ## Timestamp equal to the current clock time
        tfs.header.stamp = self.get_clock().now().to_msg()
        ## Frame of reference equals `world`
        tfs.header.frame_id = 'world'
        ## The transform will be of the frame will be of this tag
        tfs.child_frame_id = "object_1"

        # Set the translation of the transform as a circle in the x-y plane
        tfs.transform.translation.x = 1.0
        tfs.transform.translation.y = 2.0
        tfs.transform.translation.z = 3.0

        # Set the rotation (Quaternion) of the transform as a rotation about the x-axis
        tfs.transform.rotation.w = cos(pi / 2.0)
        tfs.transform.rotation.x = sin(pi / 2.0)
        tfs.transform.rotation.y = 0.0
        tfs.transform.rotation.z = 0.0

        self.get_logger().info(f"This transform has translation:"
                               f" {tfs.transform.translation} "
                               f"and rotation:"
                               f" {tfs.transform.rotation}.")

def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = CreateStampedTransformsNode()
        node.create_and_print_stamped_transform()

    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)



if __name__ == '__main__':
    main()

Note that the only novelty will be the excerpt below. We create an instance of TransformStamped and start by adding information related to the header. Each transform has a frame of reference, called header.frame_id. Then, the actual frame this transform represents is held by child_frame_id.

The values corresponding to example translation \(\boldsymbol{t}_1 \triangleq 1\hat{\imath} + 2\hat{\jmath} + 3\hat{k}\) and the example rotation \(\boldsymbol{r}_1 \triangleq \cos\left(\frac{\pi}{2}\right) + \hat{\imath}\sin\left(\frac{\pi}{2}\right)\) are assigned in the highlight lines below.

    def create_and_print_stamped_transform(self):
        """Basic method showing the creation of TransformStamped objects."""

        tfs = TransformStamped()

        # Initialize header with
        ## Timestamp equal to the current clock time
        tfs.header.stamp = self.get_clock().now().to_msg()
        ## Frame of reference equals `world`
        tfs.header.frame_id = 'world'
        ## The transform will be of the frame will be of this tag
        tfs.child_frame_id = "object_1"

        # Set the translation of the transform as a circle in the x-y plane
        tfs.transform.translation.x = 1.0
        tfs.transform.translation.y = 2.0
        tfs.transform.translation.z = 3.0

        # Set the rotation (Quaternion) of the transform as a rotation about the x-axis
        tfs.transform.rotation.w = cos(pi / 2.0)
        tfs.transform.rotation.x = sin(pi / 2.0)
        tfs.transform.rotation.y = 0.0
        tfs.transform.rotation.z = 0.0

        self.get_logger().info(f"This transform has translation:"
                               f" {tfs.transform.translation} "
                               f"and rotation:"
                               f" {tfs.transform.rotation}.")

Update the `setup.py`#

As usual, we add the necessary entry point in setup.py.

setup.py

from setuptools import find_packages, setup

package_name = 'python_package_that_uses_geometry_msgs'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='murilo.marinho@manchester.ac.uk',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "create_stamped_transforms_node = python_package_that_uses_geometry_msgs.create_stamped_transforms_node:main",
        ],
    },
)

Build and source#

Before we proceed, let us build and source once.

cd ~/ros2_tutorial_workspace
colcon build
source install/setup.bash

Note

For additional explanation and troubleshooting tips, see Always source after you build.

Warning

colcon will not work properly if your terminal has an active venv.

Run Example#

We run our newly created program as follows.

ros2 run python_package_that_uses_geometry_msgs create_stamped_transforms_node

The result will be as follows.

This transform has translation: geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0) and rotation: geometry_msgs.msg.Quaternion(x=1.0, y=0.0, z=0.0, w=6.123233995736766e-17).

Note that the results are reasonably close to the ones we calculated mathematically. However, given the limitations on current computers related to floating point accuracy, you can always expect a level of inaccuracy. This is not limited or affected by the use of quaternions, this is an inherent limitation of our computers.

Frame Transformations

Contents

Frame Transformations#

Transformations in ROS2#

Translations#

Rotations: Quaternions#

Sequential rotations#

Inverse rotations#

Connecting these ideas#

Create the package#

Package structure#

Add sample code#

Update the setup.py#

Build and source#

Run Example#

Update the `setup.py`#