机器人学与OROCOS-KDL（二）位置描述与向量

机器人学中，有一些基本的线性代数知识需要了解，会使用向量、旋转矩阵、齐次矩阵等去描述空间中物体的位置和姿态。在KDL库的 frames.hpp头文件中定义了以下的类：

/*源码 orocos_kdl/src/frames.hpp*/
class Vector;		//三维向量类的具体实现
class Rotation;		//三维空间的旋转矩阵
class Frame;		//齐次矩阵
class Wrench;		//表示力和扭矩的组合
class Twist;		//表示平动速度和旋转速度
class Vector2;		//二维Vector
class Rotation2;	//二维Rotation
class Frame2;		//二维Frame

接下来几篇将探究空间描述，以及KDL的Vector、Rotation及Frame类，本篇先研究位置的描述与KDL的Vector类。

一、位置的数学表示

在空间直角坐标系中，我们可以用一个$3\times 1$的位置向量来确定该空间内任意一点的位置。对于直角坐标系 { A } \{A\} {A}，空间任意一点P的位置可以用$3\times 1$的列矢量${}^{A}P$表示：
$${}^{A}P=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$$
式中$p_x,p_y,p_z$是点P在坐标系 { A } \{A\} {A}中的三个坐标分量。${}^{A}P$为位置向量，${}^{A}P$的上标$A$代表参考坐标系 { A } \{A\} {A}。

二、OROCOS-KDL应用

1.主要成员变量

Vector类的主要成员变量为三个双精度浮点型的数组，数组储存向量的xyz值。

/*源码 orocos_kdl/src/frames.hpp*/
//class Vector
public:
    double data[3];

2.创建向量

/*实例 orocos_kdl/examples/geometry.cpp*/
#include "frames.hpp"
#include <frames_io.hpp>
KDL::Vector v1;	//默认
KDL::Vector v2(1.0,2.0,3.0);	//带初始值
KDL::Vector v3(v2);	//复制其他向量
KDL::Vector v4 = KDL::Vector::Zero();//静态成员函数
//打印向量
std::cout<<"v1 ="<<v1<<std::endl;
std::cout<<"v2 = "<<v2<<std::endl;
std::cout<<"v3 = "<<v3<<std::endl;
std::cout<<"v4 = "<<v4<<std::endl;

打印结果：

3.设置和获取向量值

//设置向量值
v1[0]=4.0;
v1[1]=5.0;
v1[2]=6.0;
v2(0)=7.0;
v2(1)=8.0;
v2(2)=9.0;
v3.x(10.0);
v3.y(11.0);
v3.z(12.0);
//获取向量值
std::cout<<"v1: "<<v1[0]<<", "<<v1[1]<<", "<<v1[2]<<std::endl;
std::cout<<"v2: "<<v2(0)<<", "<<v2(1)<<", "<<v2(2)<<std::endl;
std::cout<<"v3: "<<v3.x()<<", "<<v3.y()<<", "<<v3.z()<<std::endl;

打印结果：

4.向量运算

• 基本运算

/*实例 orocos_kdl/examples/geometry.cpp*/
//向量与数值的运算
std::cout<<"2*v2 = "<<2*v2<<std::endl;	//乘法
std::cout<<"v1*2 = "<<v1*2<<std::endl;	
std::cout<<"v1/2 = "<<v1/2<<std::endl;	//除法

//向量与向量的运算
std::cout<<"v1+v2 = "<<v1+v2<<std::endl;	//加法
std::cout<<"v3-v1 = "<<v3-v1<<std::endl;	//减法

v3-=v1;	//减等于，相当于v3=v3-v1;
v2+=v1;	//加等于，相当于v2=v2+v1;
std::cout<<"v3-=v1; v3 = "<<v3<<std::endl;
std::cout<<"v2+=v1; v2 = "<<v2<<std::endl;

打印结果：

• 叉乘和点乘

/*实例 orocos_kdl/examples/geometry.cpp*/
//两个向量的叉乘和点乘
std::cout<<"cross(v1,v2) =  "<<v1*v2<<std::endl;	//叉乘
std::cout<<"dot(v1,v2) = "<<dot(v1,v2)<<std::endl;	//点乘

打印结果：

• 向量取反

/*实例 orocos_kdl/examples/geometry.cpp*/
//相反向量
v1=-v2;
std::cout<<"v1=-v2; v1="<<v1<<std::endl;
v1.ReverseSign();
std::cout<<"v1.ReverseSign(); v1 = "<<v1<<std::endl;

打印结果：

• 其他

/*实例 orocos_kdl/examples/geometry.cpp*/
//相等比较
std::cout<<"v1==v2 ? "<<(v1==v2)<<std::endl;
std::cout<<"v1!=v2 ? "<<(v1!=v2)<<std::endl;
std::cout<<"Equal(v1,v2,1e-6) ? "<<Equal(v1,v2,1e-6)<<std::endl;

//计算向量长度，以及设置单位向量
std::cout<<"norm(v3): "<<v3.Norm()<<std::endl;
v3.Normalize();
std::cout<<"Normalize(v3)"<<v3<<std::endl;
	
//设置向量为零向量
SetToZero(v1);
std::cout<<"SetToZero(v1); v1 = "<<v1<<std::endl;

打印结果：