tf in python · 機器人操作系統入門-課程講義

# 8.4 tf in python ### 8.4.1 簡介我們知道tf中不僅有C++的接口，也有Python的接口。相比C++，tf在Python中的具體實現相對簡單好用。 ### 8.4.2 數據類型 TF的相關數據類型，向量、點、四元數、矩陣都可以表示成類似數組形式，就是它們都可以用Tuple，List，Numpy Array來表示。例如： ``` t = (1.0,1.5,0) #平移 q = [1,0,0,0] #四元數 m = numpy.identity(3) #旋轉矩陣 ``` 第一個平移數據使用Tuple表示的，同時也可以用List表示成t=\[1.0,1.5,0\],也能用numpy.array$1.0,1.5,0$來表示都是可以的。這些數據類型沒有特殊對應，全部是通用的，所以這里也就沒有了各種數據類型的轉換的麻煩。 ### 8.4.3 TF庫 ##### tf.transformations 基本數學運算函數 | 函數 | 注釋 | | :---: | :---: | | euler\_matrix$ai,aj,ak,axes='sxyz'$ | 歐拉角到矩陣 | | eulaer\_form\_matrix$matrix,axes='sxyz'$ | 矩陣到歐拉角 | | eular\_from\_quaternion$quaternion,axes='sxyz'$ | 四元數到歐拉角 | | quaternion\_form\_euler$ai,aj,ak,axes='sxyz'$ | 歐拉角到四元數 | | quaternion\_matrix$quaternion$ | 四元數到矩陣 | | quaternion\_form\_matrix$matrix$ | 矩陣到四元數 | | ...... | ...... | 使用該函數庫時候，首先`import tf`，tf.transformations給我們提供了一些基本的數學運算函數如上，使用起來非常方便。在tf\_demo中教學包當中，我們列舉了一些tf.transformations常見的API和示例代碼，具詳見下表。 ###### 第1部分，定義空間點和空間向量 | 編號 | 函數名稱 | 函數功能 | | :---: | :---: | :---: | | 1.1 | tf.transformations.random\_quaternion$rand=None$ | 返回均勻隨機單位四元數 | | 1.2 | tf.transformations.random\_rotation\_matrix$rand=None$ | 返回均勻隨機單位旋轉矩陣 | | 1.3 | tf.transformations.random\_vector$size$ | 返回均勻隨機單位向量 | | 1.4 | tf.transformations.translation\_matrix$v$ | 通過向量來求旋轉矩陣 | | 1.5 | tf.transformations.translation\_from\_matrix$m$ | 通過旋轉矩陣來求向量 | ###### 第2部分，定義四元數 | 編號 | 函數名稱 | 函數功能 | | :---: | :---: | :---: | | 2.1 | tf.transformations.quaternion\_about\_axis$angle axis$ | 通過旋轉軸和旋轉角返回四元數 | | 2.2 | tf.transformations.quaternion\_conjugate$quaternion$ | 返回四元數的共軛 | | 2.3 | tf.transformations.quaternion\_from\_euler$ai,aj,ak, axes'ryxz'$ | 從歐拉角和旋轉軸，求四元數 | | 2.4 | tf.transformations.quaternion\_from\_matrix$matrix$ | 從旋轉矩陣中，返回四元數 | | 2.5 | tf.transformations.quaternion\_multiply$quaternion1,quaternion2$ | 兩個四元數相乘 | ###### 第3部分，定義四元數 | 編號 | 函數名稱 | 函數功能 | | :---: | :---: | :---: | | 3.1 | tf.transformations.euler\_matrix$ai,aj,ak,axes='xyz'$ | 由歐拉角和旋轉軸返回旋轉矩陣 | | 3.2 | tf.transformations.euler\_from\_matrix$matrix$ | 由旋轉矩陣和特定的旋轉軸返回歐拉角 | | 3.3 | tf.transformations.euler\_from\_quaternion$quaternion$ | 由四元數和特定的軸得到歐拉角 | 示例代碼： #!/usr/bin/env python # -*- coding:utf-8 -*- import rospy import math import tf if __name__ == '__main__': rospy.init_node('py_coordinate_transformation') #第1部分，定義空間點和空間向量 print '第1部分，定義空間點和空間向量' #1.1 返回均勻隨機單位四元數 q=tf.transformations.random_quaternion(rand=None) print '定義均勻隨機四元數：' print q #1.2 返回均勻隨機單位旋轉矩陣 m=tf.transformations.random_rotation_matrix(rand=None) print '定義均勻隨機單位旋轉矩陣：' print m #1.3 返回均勻隨機單位向量 v=tf.transformations.random_vector(3) print '定義均勻隨機單位向量：' print v #1.4 通過向量來求旋轉矩陣 v_m=tf.transformations.translation_matrix(v) print '通過向量來求旋轉矩陣：' print v_m #1.5 通過旋轉矩陣來求向量 m_v=tf.transformations.translation_from_matrix(m) print '通過旋轉矩陣來求向量：' print m_v #第2部分，定義四元數 print '第2部分，定義四元數' #2.1 通過旋轉軸和旋轉角返回四元數 axis_q=tf.transformations.quaternion_about_axis(0.123, (1, 0, 0)) print '通過旋轉軸和旋轉角返回四元數：' print axis_q #2.2 返回四元數的共軛 n_q=tf.transformations.quaternion_conjugate(q) print '返回四元數q的共軛：' print n_q #2.3 從歐拉角和旋轉軸，求四元數 o_q=tf.transformations.quaternion_from_euler(1, 2, 3, 'ryxz') print '從歐拉角和旋轉軸，求四元數：' print o_q #2.4 從旋轉矩陣中，返回四元數 m_q=tf.transformations.quaternion_from_matrix(m) print '從旋轉矩陣中，返回四元數：' print m_q #2.5 兩個四元數相乘 qxq=tf.transformations.quaternion_multiply(q,n_q) print '兩個四元數相乘' print qxq ` ### 8.4.4 TF類 ##### tf.TransformListener類 | 方法 | 作用 | | :---: | :---: | | canTransform$self,target\_frame,source\_frame,time$ | frame是否相通 | | waitForTransform$self,target\_frame,source\_frame,time,timeout$ | 阻塞直到frame相通 | | lookup Transform$self,target\_frame,source\_frame,time$ | 查看相對的tf，返回（trans，quat） | tf.TransformListener類中主要包含以上三種方法，它的構造函數不需要填值。注意這里的time參數，依然是使用`rospy.Time(0)`而不是`rospy.Time.now()`.具體原因上節已經介紹，這里不再贅述。除了上述三種重要的方法，這個類中還有一些輔助用的方法如下： | 方法 | 作用 | | :---: | :---: | | chain$target\_frame,target\_time,source\_frame,source\_time,fixed\_frame$ | frame的連接關系 | | frameExists$self,frame\_id$ | frame是否存在 | | getFrameStrings$self$ | 返回所有tf的名稱 | | fromTranslationRotation$translation,rotation$ | 根據平移和旋轉返回4X4矩陣 | | transformPoint$target\_frame,point\_msg$ | 將PointStamped消息轉換到新frame下 | | transformPose$target\_frame,pose\_msg$ | 將PoseStamped消息轉換到新frame下 | | transformQuaternion$target\_frame,quat\_msg$ | 將QuaternionStamped...返回相同類型 | | ... | ... | 在`tf_demo`教學包當中的`scripts/py_tf_listerner.py`給出了示例程序，詳見如下。 py\_tf\_listerner.py ``` #!/usr/bin/env python # -*- coding:utf-8 -*- import rospy import math import tf if __name__ == '__main__': rospy.init_node('py_tf_turtle') listener = tf.TransformListener() #TransformListener創建后就開始接受tf廣播信息，最多可以緩存10s 目前存在的問題，是四個數值的順序我還有點問題 rate = rospy.Rate(1.0) #1. 阻塞直到frame相通 print '1. 阻塞直到frame相通' listener.waitForTransform("/base_link", "/link1", rospy.Time(), rospy.Duration(4.0)) while not rospy.is_shutdown(): try: #2. 監聽對應的tf,返回平移和旋轉 print '2. 監聽對應的tf,返回平移和旋轉' (trans,rot) = listener.lookupTransform('/base_link', '/link1', rospy.Time(0)) #rospy.Time(0)不表示0時刻的tf，而是指最近一幀tf except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue rospy.loginfo('距離原點的位置: x=%f ,y= %f，z=%f \n 旋轉四元數: w=%f ,x= %f，y=%f z=%f ',trans[0],trans[1],trans[2],rot[0],rot[1],rot[2],rot[3]) #3. 判斷兩個frame是否相通 print '3. 判斷兩個frame是否相通' if listener.canTransform('/link1','/base_link',rospy.Time(0)) : print 'true' else : print 'false' rate.sleep() ``` ##### ##### tf.TransformBroadcaster類類似的，我們介紹的是發布方，tf.TransformBroadcaster類。該類的構造函數也是不需要填值，成員函數有兩個如下： * sendTransform$translation,rotation,time,child,parent$\#向/tf發布消息 * sendTransformMessage$transform$\#向/tf發布消息第一個sendTransform把transform的平移和旋轉填好，打上時間戳，然后表示出從父到子的frame流，然后發向`/tf`的topic。第二種是發送transform已經封裝好的Message給`/tf`，這兩種不同的發送方式，功能是一致的。在`tf_demo`教學包當中的`scripts/py_tf_broadcaster.py`和`scripts/py_tf_broadcaster02.py`給出了示例程序，詳見如下。 py\_tf\_broadcaster.py ``` #!/usr/bin/env python # -*- coding:utf-8 -*- import rospy import math import tf if __name__ == '__main__': rospy.init_node('py_tf_broadcaster') print '講解tf.transformBroadcaster類' print '第1種發布方式：sendTransform(translation,rotation,time,child,parent)' #第一部分，發布sendTransform(translation,rotation,time,child,parent) br = tf.TransformBroadcaster() #輸入相對原點的值和歐拉角 x=1.0 y=2.0 z=3.0 roll=0 pitch=0 yaw=1.57 rate = rospy.Rate(1) while not rospy.is_shutdown(): yaw=yaw+0.1 br.sendTransform((x,y,z), tf.transformations.quaternion_from_euler(roll,pitch,yaw), rospy.Time.now(), "base_link", "link1") #發布base_link到link1的平移和翻轉 rate.sleep() ``` py\_tf\_broadcaster02.py ``` #!/usr/bin/env python # -*- coding:utf-8 -*- import rospy import geometry_msgs.msg import tf2_ros.transform_broadcaster import math import tf if __name__ == '__main__': rospy.init_node('py_tf_broadcaster') print '講解tf.transformBroadcaster類' print '第2種發布方式：sendTransformMessage(transform)' #第二部分，發布sendTransformMessage(transform) m=tf.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.frame_id = 'base_link' t.header.stamp = rospy.Time(0) t.child_frame_id = 'link1' t.transform.translation.x = 1 t.transform.translation.y = 2 t.transform.translation.z = 3 t.transform.rotation.w=1 t.transform.rotation.x=0 t.transform.rotation.y=0 t.transform.rotation.z=0 #輸入相對原點的值和歐拉角 rate = rospy.Rate(1) while not rospy.is_shutdown(): m.sendTransformMessage(t) rate.sleep() ``` ### 8.4.5 TF相關工具命令 1. 根據當前的tf樹創建一個pdf圖： ``` $ rosrun tf view_frames ``` 這個工具首先訂閱`/tf`，訂閱5秒鐘，根據這段時間接受到的tf信息，繪制成一張tf tree，然后創建成一個pdf圖。 2. 查看當前的tf樹： ``` $ rosrun rqt_tf_tree rqt_tf_tree ``` 該命令同樣是查詢tf tree的，但是與第一個命令的區別是該命令是動態的查詢當前的tf tree,當前的任何變化都能當即看到，例如何時斷開何時連接，捕捉到這些然后通過rqt插件顯示出來。 3. 查看兩個frame之間的變換關系： ``` $ rosrun tf tf_echo[reference_frame][target_frame] ```