KDL（kinematic-Dynamic Library）项目是欧洲一些搞机器人的大牛做的一个开源的项目OROCOS（Open Robot Control Software）的一部分，，它产生和发展的历史可见官网（www.orocos.org），对于机器人学领域的同学们，如果想要学习机器人路径规划、轨迹规划、逆解算法，甚至编程的话，这是一个很好的学习素材。 这篇文章主要介绍如何在ubuntu下编译和运行一部分代码（geometry.cpp），初步观察和学习KDL的旋转矩阵运行结果。 从GITHUB上下载源码（https://github.com/orocos/orocos_kinematics_dynamics）。 查看example路径下的CmakeLists.txt文件。在orocos_kdl目录下：
ccmake .
配置成如下形式（需要注意的是必须将BUILD_MODELS和ENABLE_EXAMPLES这两项设为ON），否则无法make通过:
BUILD_MODELS ON
BUILD_MODELS_DEMO OFF
CMAKE_BUILD_TYPE Release
CMAKE_INSTALL_PREFIX /usr/local
CPPUNIT /usr/lib/x86_64-linux-gnu/libcppunit.so
CPPUNIT_HEADERS /usr/include
ENABLE_EXAMPLES ON
ENABLE_TESTS OFF
Eigen_DIR Eigen_DIR-NOTFOUND
KDL_USE_NEW_TREE_INTERFACE OFF
在orocos_kdl目录下：
make
再查看example目录下的文件内容，可以看到已经生成了三个可执行文件：geometry、trajectory_example、chainiksolverpos_lma_demo。执行geometry：
aicrobo@ubuntu:~/kdl/orocos_kdl/examples$ ./geometry
v1 =[ 0, 0, 0]
v2 = [ 1, 2, 3]
v3 = [ 1, 2, 3]
v4 = [ 0, 0, 0]
v1: 4, 5, 6
v2: 7, 8, 9
v3: 10, 11, 12
2*v2 = [ 14, 16, 18]
v1*2 = [ 8, 10, 12]
v1/2 = [ 2, 2.5, 3]
v1+v2 = [ 11, 13, 15]
v3-v1 = [ 6, 6, 6]
v3-=v1; v3 = [ 6, 6, 6]
v2+=v1; v2 = [ 11, 13, 15]
cross(v1,v2) = [ -3, 6, -3]
dot(v1,v2) = 199
v1=-v2; v1=[ -11, -13, -15]
v1.ReverseSign(); v1 = [ 11, 13, 15]
v1==v2 ? 1
v1!=v2 ? 0
Equal(v1,v2,1e-6) ? 1
norm(v3): 10.3923
Normalize(v3)[ 0.57735, 0.57735, 0.57735]
SetToZero(v1); v1 = [ 0, 0, 0]
r1: [ 1, 0, 0;
0, 1, 0;
0, 0, 1]
r2: [ 0, 0, -1;
0, -1, 0;
1, 0, 0]
r3: [ 0, 0, -1;
1, 0, 0;
0, -1, 0]
r4: [ 1, 0, 0;
0, 1, 0;
0, 0, 1]
r5: [ 1, 0, 0;
0, 0.5, -0.866025;
0, 0.866025, 0.5]
r6: [ 0.5, 0, 0.866025;
0, 1, 0;
-0.866025, 0, 0.5]
r7: [ 0.5, -0.866025, 0;
0.866025, 0.5, 0;
0, 0, 1]
r8: [ 0.728028, -0.525105, 0.440727;
0.608789, 0.790791, -0.0634566;
-0.315202, 0.314508, 0.895395]
r9: [ 0.765682, -0.428256, 0.4799;
0.571734, 0.794968, -0.202787;
-0.294665, 0.429649, 0.853551]
r10: [ 0.103847, 0.86478, 0.491295;
0.422919, -0.485478, 0.765147;
0.900198, 0.12832, -0.416147]
r11: [ -0.224845, 0.902382, -0.36763;
-0.350175, -0.426918, -0.833738;
-0.909297, -0.0587266, 0.411982]
r12: [ 0.411982, -0.833738, -0.36763;
-0.0587266, -0.426918, 0.902382;
-0.909297, -0.350175, -0.224845]
r8(1,2): -0.0634566
equiv rot vector of r11: [ 1.10589, 0.772924, -1.78732]
equiv rot vector of r10:[ -0.72668, -0.466596, -0.504206]and angle: 2.68802
EulerZYZ: -0.399803, 0.548013, 0.969646
EulerZYX: 0.641385, 0.299109, 0.466338
Roll-Pitch-Yaw: 0.466338, 0.299109, 0.641385
UnitX of r8:[ -0.72668, -0.466596, -0.504206]
Unity of r8:[ -0.72668, -0.466596, -0.504206]
UnitZ of r8:[ -0.72668, -0.466596, -0.504206]
要搞明白上述运行结果的意义的话，可以查看geometry.cpp，这个程序是为了展示其定义的类Vector（包含向量的多种运算）和类Rotation。Rotation表示机器人的旋转矩阵（姿态），机器人学中表示机器人姿态有多种方法，orocos中定义的方法有RPY方法，角-轴，欧拉角方法，四元数方法。 参考资料： [1]http://www.orocos.org; [2]https://github.com/orocos/orocos_kinematics_dynamics
