Getting Started with ROS for Jetson Nano - Robot Coordinate Transformation

---

foreword

ROS provides developers with many highly integrated development tools, such as rviz and gazebo. rviz is a three-dimensional visualization tool that can display information such as images, models, paths, etc., but the premise is that these data have been released in the form of topics and parameters. What rviz does is to subscribe to these data and complete the visual rendering. This blogger Mainly rely on rviz's 3D platform to learn coordinate transformation and robot 3D modeling

---

1. The principle of space coordinate transformation

1. Pose description

Before learning the pose description, we must first understand the nature of the matrix. If we associate the matrix with the function and understand it as a mapping change, then the essence of the matrix is ​​to describe a space transformation of the basis vector, and the determinant expresses exactly the area enclosed by the transformation of the basis vector

$i$ and$j$ is the simplest pair of basis for forming a two-dimensional plane, and the matrix focuses on the linear transformation of these two pairs of basis vectors, a function similar to a function a ⃗ = f ( b ⃗ )
$$\vec{a}=f(\vec{b})$$

比如，$\left[\begin{array}{c}i\\ j\end{array}\right]$ This basis passes through$\left[\begin{array}{cc}3& 2\\ -2& 1\end{array}\right]$ mapping,$i$ becomes$(3,-2)$ This new basis vector,$j$ becomes$(2,1)$ With this new basis vector, the entire two-dimensional plane changes as follows:

We put the changed two basis vectors into the matrix as follows, so that this matrix$\left[\begin{array}{cc}3& 2\\ -2& 1\end{array}\right]$ represents a transformation mapping to a two-dimensional space, and the two columns are the transformed$i$ and$\mathbf{j}$。

Let's talk about pose, which stands for position and posture. Any rigid body in the space coordinate system $In\; OXYZ$ , position and attitude can be used to accurately and uniquely represent its position status. For the Cartesian coordinate system {A}, any point$The\; position\; of\; p$ can be used as a 3x1 column vector${}^{A}P$array
$${}^{A}P=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$$

Suppose the main coordinate system is $A$，for us$X_B、Y_B、Z_B$to represent another coordinate system $The\; unit\; vectors\; of\; the\; three\; axes\; of\; B$ , then the coordinate system$B$ is relative to the coordinate system$A$ boundary cycle with a defined$${}_B^{A}R={(}^A{X_B}^A{Y_B}^A{Z}_B)=\left[\begin{array}{ccc}{X}_B\bullet X_A& Y_B\bullet X_A& Z_B\bullet X_A\\ X_B\bullet Y_A& Y_B\bullet Y_A& Y_B\bullet Y_A\\ X_B\bullet Z_A& Y_B\bullet Z_A& Z_B\bullet Z_A\end{array}\right]$$

According to the essence of the matrix we just understood, $X_A$After being mapped by the matrix, it becomes the unit vector $X_B$在$X_A、Y_A、Z_A$The projection on the three base vectors is exactly $X_B$；$Y_A$After being mapped by the matrix, it becomes a unit vector $Y_B$在$X_A、Y_A、Z_A$The projection on the three base vectors is exactly $Y_B$；$Z_A$After being mapped by the matrix, it becomes a unit vector $Z_B$在$X_A、Y_A、Z_A$The projection on the three base vectors is exactly $Z_B$, this rotation matrix is ​​actually the coordinate system $The\; space\; transformation\; of\; A$ forms the coordinate system$B$

With $The\; attitude\; of\; the\; B$ coordinate system, now we need a position vector${}^A{P}_{BORG}$to determine $B$ coordinate system relative to$The\; relative\; position\; of\; the\; A$ coordinate system can finally be$The\; attitude\; expression\; of\; the\; B$ coordinate system is
$$B={}_B^{A}R{,}^A{P}_{BORG}$$

After learning the pose expressions of the two coordinate systems in three-dimensional space, let's discuss the coordinate system A {A}How does a vector in$A$ lie in the coordinate system B{B}Expressed in$B$ , we first discuss the case where the origins of the two coordinate systems coincide, assuming the coordinate system A {A}There is a vectorPP$in\; A$$P$ , according to the mapping principle of the matrix, the position vector$P$ is in the coordinate systemA{A}The expression in$A$$P$ in coordinate systemB{B}The expression in$B$$B$ maps to the coordinate system$A$
$${}^{A}P{=}^B{P}_B^{A}R$$

If the origins of the two coordinate systems do not coincide, we also need to consider the position vector ${}^A{P}_{BORG}$,from ${}^A{P}_{BORG}$is in the coordinate system $A$ is given as a reference, so there is no need for coordinate system mapping, just add vectors directly

$${}^{A}P{=}^B{P}_B^{A}R{+}^A{P}_{BORG}$$

Next we introduce a 4x4homogeneous transformation matrixTo replace the above formula, this expression is more clear and concise
$$\left[\begin{array}{c}{}^{A}P\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^{A}R& {}^A{P}_{BORG}\\ \left[\begin{array}{ccc}0& 0& 0\end{array}\right]& 1\end{array}\right]\left[\begin{array}{c}{}^{B}P\\ 1\end{array}\right]$$

2. Euler angles and quaternions

Try to imagine an airplane with a coordinate system B{B} at the center of mass of the airplane$B$ , the coordinate system$B$ around its$Z-$ axis rotation$\alpha$ angle, and then YYaround it$Y-$ axis rotation$\beta$ angle, and finally XXaround it$X-$ axis rotation$\gamma$ angle, this set of rotation three angles is called Euler angle. There are three Euler angles, namely yaw (yaw angle), pitch (pitch angle), and roll (roll angle), which represent the angle of rotation around the Z/Y/X axis

According to the process of forming Euler angles above, we can give the mathematical expression of each rotation
$$R_Z\left(a\right)=\left[\begin{array}{ccc}cos\left(\alpha \right)& -sin\left(a\right)& 0\\ sin\left(a\right)& cos\left(\alpha \right)& 0\\ 0& 0& 1\end{array}\right]$$
$$R_Y\left(b\right)=\left[\begin{array}{ccc}cos\left(b\right)& 0& sin\left(b\right)\\ 0& 1& 0\\ -sin(b& 0& cos(b\end{array}\right]$$
$$R_X\left(c\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left(c\right)& -sin\left(c\right)\\ 0& sin(c& cos(c\end{array}\right]$$
Finally, we integrate the product of the three rotation matrices to get the coordinate system B {B}The new coordinate system B ′ {B^{'}}$formed\; by\; B$ after a set of rotations$B^{{}^{\prime }}$ , we use a rotation matrix${}_{B^{{}^{\prime }}}^{B}R$ to express the coordinate system$B$ to coordinate system$B^{{}^{\text{'}}}$ Map relationship
$${}_{B^{{}^{\prime }}}^B{R}_{Z-Y-X}(a,b,c)=\left[\begin{array}{ccc}cos\left(a\right)cos\left(b\right)& cos\left(a\right)sin\left(b\right)sin\left(c\right)-sin\left(a\right)cos\left(c\right)& cos\left(a\right)sin\left(b\right)cos\left(c\right)+sin\left(a\right)sin\left(c\right)\\ sin\left(a\right)cos\left(b\right)& sin\left(a\right)sin\left(b\right)sin\left(c\right)+cos\left(a\right)cos\left(c\right)& sin\left(a\right)sin\left(b\right)cos\left(c\right)-cos\left(a\right)sin\left(c\right)\\ -sin(b& cos\left(b\right)sin(c& cos\left(b\right)cos(c\end{array}\right]$$

Quaternions are similar to complex numbers. Quaternions are actually for bases $(a,\vec{i},\vec{j},\vec{k})$ is a linear combination of a scalar and a 3x1 vector. We use complex numbers as a quaternion analogy. The scalar of the quaternion is the real part of the complex number, and the vector of the quaternion is the imaginary part of the complex number. Multiplication of

quaternions Follow the following properties, multiplication by itself is -1, and the same as other multiplication and cross multiplication properties, follow the right-hand rule

We define $q$ is a unit quaternion, which can be expressed as$\mathbf{q}=w+xi+yj+zk$ , give$q$ is a unit quaternion

Quaternion q \mathbf{q}The rotation matrix R of$q$${\mathbf{R}}_{\mathbf{q}}$can be expressed as

2. Python in ROS implements coordinate transformation

1. Coordinate msg message carrier

The data carrier msg in the subscription publishing model is an important implementation link. The commonly used coordinate data message carrier msg in the realization of coordinate transformation includes geometry_msgs/TransformStamped and geometry_msgs/PointStamped

The following is some data returned by the geometry_msgs/TransformStamped message, and we can get the pose relationship between the two coordinate systems. Generally, we choose the world coordinate system "world" for the parent coordinate system here.

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

The following is some data returned by the geometry_msgs/PointStamped message, we can determine the position of the point in the coordinate system to which it belongs

std_msgs/Header header                    #头信息
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

With these message data, we can implement some fun and powerful functions, such as the turtle following in the simulation environment

2. Implementation of the program that the turtle follows

The core of the turtle following implementation is that both turtles A and B will publish coordinate information relative to the world coordinate system, and then subscribe to this information and need to convert and obtain the information of A relative to the coordinate system of B, and finally generate linear velocity and angular velocity information, and control the turtle BSports.

First, in the src directory of the workspace, select the Create Catkin Package option to create a new function package, addtf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgsWait for the dependent package, then create a new scripts folder to store python files, and create a launch folder to store launch files

First, we call the service of ROS to generate the second little turtle. To create the second turtle, we need to use rosservice, and the topic is /spawn

#! /usr/bin/env python

#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("turtle_spawn_p")
    # 3.创建服务客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    client.wait_for_service()
    # 5.创建请求数据
    req = SpawnRequest()
    req.x = 1.0
    req.y = 1.0
    req.theta = 0
    req.name = "turtle2"
    # 6.发送请求并处理响应
    try:
        response = client.call(req)
        rospy.loginfo("乌龟创建成功，名字是:%s",response.name)
    except Exception as e:
        rospy.loginfo("服务调用失败....")

Then we need to write coordinate message topic publishing node, we pass topic $/turtle\; name/pose$ to obtain the pose of the turtle, and solve it into a coordinate message relative to the world coordinate system, and then publish$it$

#! /usr/bin/env python

# 1.导包
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

turtle_name = ""

def doPose(pose):
    # rospy.loginfo("x = %.2f",pose.x)
    #1.创建坐标系广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #2.将 pose 信息转换成 TransFormStamped
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0

    qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    #3.广播器发布 tfs
    broadcaster.sendTransform(tfs)


if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.解析传入的命名空间
    rospy.loginfo("-------------------------------%d",len(sys.argv))
    if len(sys.argv) < 2:
        rospy.loginfo("请传入参数:乌龟的命名空间")
    else:
        turtle_name = sys.argv[1]
    rospy.loginfo("///乌龟:%s",turtle_name)

    rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
    #     4.创建订阅对象
    #     5.回调函数处理订阅的 pose 信息
    #         5-1.创建 TF 广播器
    #         5-2.将 pose 信息转换成 TransFormStamped
    #         5-3.发布
    #     6.spin
    rospy.spin()

Finally, we need to write a coordinate message topic subscription node, by subscribing to the TF coordinate broadcast information of turtle1 and turtle2, find and convert the latest TF coordinate information, convert turtle1 into coordinates relative to turtle2, then calculate the linear velocity and angular velocity and publish it to the turtle movement control topic

#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    # 4.处理订阅到的 TF
    rate = rospy.Rate(10)
    # 创建速度发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
    while not rospy.is_shutdown():

        rate.sleep()
        try:
            #def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
            # rospy.loginfo("相对坐标:(%.2f,%.2f,%.2f)",
            #             trans.transform.translation.x,
            #             trans.transform.translation.y,
            #             trans.transform.translation.z
            #             )   
            # 根据转变后的坐标计算出速度和角速度信息
            twist = Twist()
            # 间距 = x^2 + y^2  然后开方
            twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)

            pub.publish(twist)

        except Exception as e:
            rospy.logwarn("警告:%s",e)

Add executable permissions to python files in the terminal

chmod +777 *.py

Configure the CMakeLists.txt file according to the method in the last note. You will compile three .py files to declare, and then catkin_make:build to see if there is any error after compiling. If there is no error, you can write the launch file.

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>

    <node pkg="xxx" type="xxx.py" name="turtle_spawn" output="screen"/>

    <node pkg="xxx" type="xxx.py" name="tf_pub1" args="turtle1" output="screen"/>
    <node pkg="xxx" type="xxx.py" name="tf_pub2" args="turtle2" output="screen"/>
    <node pkg="xxx" type="xxx.py" name="tf_sub" output="screen"/>
</launch>

After writing the launch file, right-click the launch file to open the terminal and run the launch file to see the effect of the turtle following

roslaunch xxx(功能包) xxx.launch（launch文件）

Summarize

The above are the study notes of robot coordinate transformation. This note briefly introduces the theoretical basis of robot coordinate transformation and the implementation process of TF coordinate transformation in ROS. The importance of robot coordinate transformation is that it can make the robot maintain consistent motion accuracy in different coordinate systems. Because in different coordinate systems, the trajectory and position information of the robot will change. For example, the motion of the robot in the base coordinate system may be linear motion, but the motion in the tool coordinate system may be rotational motion. If the coordinate transformation is not carried out, the robot's trajectory and position information will deviate, causing the robot to be unable to complete the task accurately.