Robotics

List of quick references for Robotics:

- Frame transformations
- Quaternions

Frame transformations

Given a point $p$ in frame "source"

p^{source} = \begin{bmatrix} x_s \\ y_s \\ z_s \\ 1 \end{bmatrix}

you can convert it to frame "target" by means of pre-multiplication:

p^{target} = T_{s}^{t} \cdot p^{source}

How to obtain $T_{s}^{t}$

In TF2 this is equal to:

ROS2 CLI

ros2 run tf2_ros tf2_echo source target

TF2.cpp

std::string toFrameRel = "target";
std::string fromFrameRel = "source";

geometry_msgs::msg::TransformStamped t_from_source_to_target = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel, tf2::TimePointZero);

Frame composition

If you have an ordered sequence of frames A, B, C, etc… with only the transformations between subsequent frames you can compose them by multiplication:

T_A^D = T_A^B \cdot T_B^C \cdot T_C^D

TF2.cpp

Suppose you’re building the transformation manually

geometry_msgs::msg::TransformStamped t_A_B;  // naming convention t_{from}_{destination}
t_A_B.header.frame_id = "A";
t_A_B.child_frame_id = "B";
// t_A_B.transform.translation or rotation of frame B relative to frame A 
// coming from somewhere else
// e.g. a message received from a sensor

geometry_msgs::msg::TransformStamped t_B_C;  
t_B_C.header.frame_id = "B";
t_B_C.child_frame_id = "C";TF2_A_B
// translation or rotation coming from somewhere else

tf2::Transform TF2_A_B, TF2_B_C;
// convert transformation msgs to TF2 objects
// uses tf2::fromMsg(const geometry_msgs::msg::Transform&, tf2::Transform&)
// docs: https://docs.ros.org/en/ros2_packages/jazzy/api/tf2_geometry_msgs/generated/index.html#tf2-geometry-msgs
tf2::fromMsg(t_A_B.transform, TF2_A_B);
tf2::fromMsg(t_B_C.transform, TF2_B_C);

// Transformations can also be built at runtime directly without converting from msgs
tf2::Transform  TF2_C_D; 	// Identity 
// D_frame is shifted by [x,y,z]=(0.1, 0.2, 0.3) compared to the C_frame
TF2_C_D.setOrigin(tf2::Vector3(0.1, 0.2, 0.3));
// create Quaternion for rotation
tf2::Quaternion q_C_D;
// set Yaw = PI/10 = 18°, pitch & roll = 0
q_C_D.setRPY(0.0, 0.0, M_PI/10);
// Apply rotation to frame:
TF2_C_D.setRotation(q_C_D);

// compose transformations
tf2::Transform TF2_A_D = TF2_A_B * TF2_B_C * TF2_C_D;

Frame Inversions

Suppose there is a sensor that doesn’t output the pose in sequential form. Eg. instead of $T_B^C$ you have $T_C^B$ .

You can recreate the transformation chain by inverting it:

T_A^D = T_A^B \cdot (T_C^B)^{-1} \cdot T_C^D \cdot T_B^C \cdot T_C^D

tf2::Transform TF2_A_D =  TF2_A_B *  TF2_C_B.inverse() * TF2_C_D;