How to transform a three-dimensional vector based on the A coordinate system to the B coordinate system in ROS?

Abstract

In ROS, the rotation quaternion rot from the A coordinate system to the B coordinate system is obtained through the tf.TransformListener.lookupTransform method, and the quaternion coordinate representation in the new coordinate system is obtained through the quaternion_multiply function. Based on this, this blog provides standard code templates and application examples for transforming a three-dimensional vector based on the A coordinate system into a B coordinate system for easy subsequent reference.

Keywords

ROS, TF, coordinate system transformation, quaternion, vector representation

Foreword

When constructing a marine robot dynamics model, the external force terms usually include the forces of incoming wind and ocean currents. The vector representation of these forces in the global geodetic coordinate system (static coordinate system) usually has a fixed direction and size. constants, but if these effects are to be introduced into the dynamic model, they are generally based on the hull coordinate system. As the position and orientation of the hull change, the vector representation of wind and sea currents in the hull coordinate system, together with its three axes The weight must change all the time. After obtaining the vector representation of wind and sea currents in the geodetic coordinate system, how to quickly obtain its components in the hull coordinate system?

When the robot performs a target tracking task, in the robot’s path planning algorithm, the coordinate representation of the target is generally based on the coordinate system with the robot center as the origin, while the distance and declination angle obtained by the visual algorithm of target detection are The relative positional relationship between the target and the airborne camera; if you also want to know the coordinates of the target in the global map, you need to further obtain the position representation in the geodetic coordinate system.

Behind these questions is the same question: What are the coordinates of a three-dimensional vector based on the A coordinate system in another coordinate system, and how to quickly find it? This blog provides standard code templates and application examples for quickly transforming a three-dimensional vector based on the A coordinate system into a B coordinate system in ROS for easy subsequent reference.

Method

Basic idea

After abstracting it into a mathematical problem, it can be expressed as: Assume a three-dimensional real number space vector

v

v

v is expressed in coordinate system A as

v

=

[

x

,

y

,

z

]

∈

R

3

v=[x,y,z] \in \mathbb {R^3}

v=[x,y,z]∈R3, the rotation from coordinate system A to coordinate system B can be expressed by a quaternion vector, let this vector be

q

q

q, find the space vector

v

v

The coordinate representation of v in coordinate system B.

The method is: first convert the vector

v

v

v uses quaternion

p

p

Expressed by p, a real vector expressed with a quaternion must be an imaginary quaternion, that is

p

=

[

0

,

x

,

y

,

z

]

=

[

0

,

v

]

p=[0,x,y,z]=[0,v]

p=[0,x,y,z]=[0,v], then, vector

v

v

The quaternion representation of v in the B system (let this quaternion be

p

‘

p’

p′) is

p

‘

=

q

p

q

?

1

p’ = qpq^{-1}

p′=qpq?1

p

‘

p’

p′ is also an imaginary quaternion.

See the picture below for the source:

(Source link: https://www.cnblogs.com/gaoxiang12/p/5120175.html)

Therefore, in order to transform a three-dimensional vector based on the A coordinate system into the B coordinate system in ROS, you need to know:

1. The coordinates of the vector in the A system;
2. Quaternion representation of the coordinates of the vector in the A system;
3. Rotation quaternion for A-system to B-system transformation

Finally, implementing matrix multiplication in ROS

p

‘

=

q

p

q

?

1

p’ = qpq^{-1}

p′=qpq?1

and take

p

‘

p’

The imaginary part in the p′ quaternion is transformed into a coordinate representation based on the B coordinate system.

Basic template

# Neccessary library
import rospy
importtf
import tf2_ros

x,y,z = 1,2,3
vector = [x,y,z]
v_base_link= [vector[0], vector[1],vector[2], 0]
listener = tf.TransformListener()
try:
(trans,rot) = listener.lookupTransform('parent_frame', 'child_frame', rospy.Time(0))
    v_map = tf.transformations.quaternion_multiply(tf.transformations.quaternion_multiply(rot, v_base_link), tf.transformations.quaternion_conjugate(rot))
    v_new = v_map[0:3]
except Exception as e:
print(f"Exception:{<!-- -->e}")
#...

Part of the code explanation

# Assume the coordinates of vector v in the A system and the corresponding quaternion representation
x,y,z = 1,2,3
vector = [x,y,z]
v_base_link= [vector[0], vector[1],vector[2], 0]

# Instantiate the TransformListener class in the tf library, and use the lookupTransform method of this class to obtain the rotation quaternion rot from the A coordinate system to the B coordinate system.
# Note that in this problem, the coordinate system translation transformation vector trans does not need to be used.
listener = tf.TransformListener()
try:
(trans,rot) = listener.lookupTransform('parent_frame', 'child_frame', rospy.Time(0))

# Use the quaternion_multiply function to implement matrix multiplication in p' = qpq^{-1}
v_map = tf.transformations.quaternion_multiply(tf.transformations.quaternion_multiply(rot, v_base_link), tf.transformations.quaternion_conjugate(rot))

Details

Q: Quaternion is a vector, why does it have an inverse?

The inversion of a quaternion is actually taking the conjugate.

See the picture below for the source:

(Source link: https://zhuanlan.zhihu.com/p/52565002)

The function of tf.transformations.quaternion_conjugate(quaternion) is to return the conjugate of quaternion.

See the picture below for the source:
(Source link: https:// blog.csdn.net/weixin_44682965/article/details/107818474)

Q: Which of the first two parameters of listener.lookupTransform is the parent system (source coordinate system) and which is the child system (target coordinate system)?

The first parameter is the source coordinate system, and the second parameter is the target coordinate system.

Q: What is the role of rospy.Time(0)?

Get the rot variable closest to the time when the listener.lookupTransform function was called.

See the picture below for the source:

(Source link: http://wiki.ros.org/tf/Tutorials/Writing a tf listener (Python))

This also means that at the beginning of program execution, there may be no rot available for acquisition, which is why a try-except statement is needed.

Q: What is the arrangement of quaternions in ROS?

• Mathematically, if the expression of a quaternion is set to [w,x,y,z], then w is the real part and x,y,z is the imaginary part.

See the picture below for the source:

(Source link: https://www.cnblogs.com/gaoxiang12/p/5120175.html)

• In ROS, such quaternions are arranged in the following order: [x, y, z, w], that is, the real part is placed at the end.

See the picture below for the source:

(Source link: http://wiki.ros.org/tf2/Tutorials/Quaternions).

Application

Goals

How to implement transformation between two fixed coordinate systems in ROS? Questions in theApplication part. In this problem, the new static coordinate system is NED (Northeast coordinate system), and the vector

v

v

The coordinate representation of v in the world coordinate system is

v

=

[

1

,

2

,

3

]

v=[1,2,3]

v=[1,2,3], find the coordinate representation of this vector in the NED coordinate system.

Operation process

1. How to implement transformation between two fixed coordinate systems in ROS? Scripts in the Application section
2. Modify the code. The script code is as follows (the code has been tested):

#!/usr/bin/env python3
# coding: utf-8

# Neccessary library
import rospy
importtf
import tf2_ros
import geometry_msgs.msg
import numpy as np

rospy.init_node("static_transformation")

static_transformStamped = geometry_msgs.msg.TransformStamped()

broadcaster = tf2_ros.StaticTransformBroadcaster()
static_transformStamped.header.stamp = rospy.Time.now()
static_transformStamped.header.frame_id = "world"
static_transformStamped.child_frame_id = "NED"
# ...omitted here
broadcaster.sendTransform(static_transformStamped)

#The focus is on the following parts
x,y,z = 1,2,3
vector = [x,y,z]
v_base_link= [vector[0], vector[1],vector[2], 0]
listener = tf.TransformListener()
loop_counter = 0
r = rospy.Rate(100)
while not rospy.is_shutdown():
    try:
        (trans,rot) = listener.lookupTransform('world', 'NED', rospy.Time(0))
        v_map = tf.transformations.quaternion_multiply(tf.transformations.quaternion_multiply(rot, v_base_link), tf.transformations.quaternion_conjugate(rot))
        v_new = v_map[0:3]
        if loop_counter > 100: #Control printing frequency
            print(f"v_new:{<!-- -->v_new}")
            loop_counter = 0
        loop_counter + = 1
    except Exception as e:
        print(f"Exception:{<!-- -->e}")
    r.sleep()

3. Start roscore, source the Python file, and rosrun corresponds to the Python node.
   The specific process is abbreviated

Results

[1,2,3] in the northeast sky coordinate system is represented by [2,1,-3] in the northeast ground coordinate system.

The print result is as shown below:

Extended reading

What are the advantages of quaternions compared to Euler angles?

References

slightly