orocos_kdl學習（一）：坐標系變換

　　KDL中提供了點(point)、坐標系(frame)、剛體速度(twist)，以及6維力/力矩(wrench)等基本幾何元素，具體可以參考 Geometric primitives 文檔。

 Creating a Frame, Vector and Rotation

　　PyKDL中創建一個坐標系時有下面4種構造函數：

__init__()         # Construct an identity frame

__init__(rot, pos) # Construct a frame from a rotation and a vector
# Parameters:    
# pos (Vector) – the position of the frame origin
# rot (Rotation) – the rotation of the frame

__init__(pos)      # Construct a frame from a vector, with identity rotation
# Parameters:     
# pos (Vector) – the position of the frame origin

__init__(rot)      # Construct a frame from a rotation, with origin at 0, 0, 0
# Parameters:    
# rot (Rotation) – the rotation of the frame

　　下面是一個例子：

#! /usr/bin/env python

import PyKDL

# create a vector which describes both a 3D vector and a 3D point in space
v = PyKDL.Vector(1,3,5)

# create a rotation from Roll Pitch, Yaw angles
r1 = PyKDL.Rotation.RPY(1.2, 3.4, 0)

# create a rotation from ZYX Euler angles
r2 = PyKDL.Rotation.EulerZYX(0, 1, 0)

# create a rotation from a rotation matrix
r3 = PyKDL.Rotation(1,0,0, 0,1,0, 0,0,1)

# create a frame from a vector and a rotation
f = PyKDL.Frame(r2, v)

print f

　　結果將如下所示，前9個元素為代表坐標系姿態的旋轉矩陣，后三個元素為坐標系f的原點在參考坐標系中的坐標。

[[    0.540302,           0,    0.841471;
            0,           1,           0;
    -0.841471,           0,    0.540302]
[           1,           3,           5]]

　　根據機器人學導論（Introduction to Robotics Mechanics and Control）附錄中的12種歐拉角表示方法，ZYX歐拉角代表的旋轉矩陣為：

　　代入數據驗證，KDL的計算與理論一致。

 

 Extracting information from a Frame, Vector and Rotation

　　PyKDL中坐標系類Frame的成員M為其旋轉矩陣，p為其原點坐標。

#! /usr/bin/env python

import PyKDL

# frame
f = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
                PyKDL.Vector(3,2,4))

# get the origin (a Vector) of the frame
origin = f.p

# get the x component of the origin
x = origin.x()
x = origin[0]
print x

# get the rotation of the frame
rot = f.M
print rot

# get ZYX Euler angles from the rotation
[Rz, Ry, Rx] = rot.GetEulerZYX()
print Rz,Ry,Rx

# get the RPY (fixed axis) from the rotation
[R, P, Y] = rot.GetRPY()
print R,P,Y

　　將上述獲取到的數據打印出來，結果如下：

3.0
[    0.540302,           0,    0.841471;
            0,           1,           0;
    -0.841471,           0,    0.540302]
0.0 1.0 0.0
0.0 1.0 0.0

 　　注意GetEulerZYX和GetRPY的結果是一樣的，這其實不是巧合。因為坐標系的旋轉有24種方式（本質上只有12種結果）：繞自身坐標系旋轉有12種方式（歐拉角），繞固定參考坐標系旋轉也有12種方式（RPY就是其中一種）。EulerZYX按照Z→Y→X的順序繞自身坐標軸分別旋轉$\alpha$、$\beta$、$\gamma$角；RPY按照X→Y→Z的順序繞固定坐標系旋轉$\gamma$、$\beta$、$\alpha$角，這兩種方式最后得到的旋轉矩陣是一樣的。

 

Transforming a point

　　frame既可以用來描述一個坐標系的位置和姿態，也可以用於變換。下面創建了一個frame，然后用其對一個空間向量或點進行坐標變換：

#! /usr/bin/env python

import PyKDL

# define a frame
f = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
                PyKDL.Vector(3,2,4))

# define a point
p = PyKDL.Vector(1, 0, 0)
print p

# transform this point with f
p = f * p
print p

 

Creating from ROS types

　　tf_conversions這個package包含了一系列轉換函數，用於將tf類型的數據(point, vector, pose, etc) 轉換為與其它庫同類型的數據，比如KDL和Eigen。用下面的命令在catkin_ws/src中創建一個測試包：

catkin_create_pkg test rospy tf geometry_msgs

　　test.py程序如下（注意修改權限chmod +x test.py）：

#! /usr/bin/env python
import rospy
import PyKDL
from tf_conversions import posemath
from geometry_msgs.msg import Pose

# you have a Pose message
pose = Pose()

pose.position.x = 1
pose.position.y = 1
pose.position.z = 1
pose.orientation.x = pose.orientation.y = pose.orientation.z = 0
pose.orientation.w = 1

# convert the pose into a kdl frame
f1 = posemath.fromMsg(pose)

# create another kdl frame
f2 = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
                 PyKDL.Vector(3,2,4))

# Combine the two frames
f = f1 * f2
print f

[x, y, z, w] = f.M.GetQuaternion()
print x,y,z,w

# and convert the result back to a pose message
pose = posemath.toMsg(f)

pub = rospy.Publisher('pose', Pose, queue_size=1)
rospy.init_node('test', anonymous=True)
rate = rospy.Rate(1) # 1hz

while not rospy.is_shutdown():
    pub.publish(pose)
    rate.sleep()

　　通過posemath.toMsg可以將KDL中的frame轉換為geometry_msgs/Pose類型的消息。最后為了驗證，創建了一個Publisher將這個消息發布到pose話題上。使用catkin_make編譯后運行結果如下：

 

 

參考：

KDL Geometric primitives

從URDF到KDL(C++&Python)

PyKDL 1.0.99 documentation

Orocos Kinematics and Dynamics

kdl / Tutorials / Frame transformations (Python)