开源机器人库orocos KDL 学习笔记二：Geometric

开源机器人库orocos KDL 学习笔记二：Geometric

如果按照上一篇搭建的VS2015 orocos KDL环境，会出现3个例程：geometry, chainiksolverpos_lma_demo, trajectory_example. 这一篇具体看一下Geometric有哪些定义。这些Geometric的概念都是机器人学的基础，本文只是解析一下KDL里面是如何实现的，具体几何上的含义可以参考【1】【2】两本机器人学书籍。

在frames.hpp中，定义了这些基础的几何类型:
- KDL::Vector 向量
- KDL::Rotation 旋转矩阵
- KDL::Frame 座标变换矩阵
- KDL::Twsit 平移和旋转速度向量
- KDL::Wrench 力和力矩向量
- KDL::Vector2 二维向量
- KDL::Rotation2 二维旋转矩阵
- KDL::Frame2 二维座标变换矩阵

这些类可以用来描述机器人的位置、姿态、座标系等。geometry例程主要是介绍这些类的用法。这里主要介绍这些类的功能和可以实现的操作。

KDL::Vector

Vector是一个三维向量，包括X-Y-Z的座标值。表示一个点相对于参考座标系的三维座标，KDL::Vector=[x y z]T$KDL::Vector=[xyz{]}^T$ 。

1. Vector初始化有三种：

inline Vector() {data[0]=data[1]=data[2] = 0.0;}
inline Vector(double x,double y, double z);
inline Vector(const Vector& arg);

Vector v1; //The default constructor, X-Y-Z are initialized to zero
Vector v2(x,y,z); //X-Y-Z are initialized with the given values 
Vector v3(v2); //The copy constructor
Vector v4 = Vector::Zero(); //All values are set to zero

分别表示：v1表示使用默认初始化函数初始化一个零向量；v2表示用x,y,z初始化；v3表示将v2复制初始化v3；v4表示用零向量初始化；

2. Get/Set 元素

有两种方式get/set单个元素：使用索引[ ]和( )操作；使用x()，y()，z()；

v1[0]=v2[1];//copy y value of v2 to x value of v1 
v2(1)=v3(3);//copy z value of v3 to y value of v2
v3.x( v4.y() );//copy y value of v4 to x value of v3

[ ]和( )操作索引从 0-2，DEBUG/NDEBUG的定义确定是否使用索引检查。

inline double x() const;
inline double y() const;
inline double z() const;
inline void x(double);
inline void y(double);
inline void z(double);

x，y，z可以单独取值和赋值；

3. 标量乘除

v2=2*v1;
v3=v1/2;

Vector的每个元素都与标量进行乘除；向量放在乘号左边或右边都行；

inline friend Vector operator*(const Vector& lhs,double rhs);
inline friend Vector operator*(double lhs,const Vector& rhs);

4. 向量之间的加减

v2+=v1;
v3-=v1;
v4=v1+v2;
v5=v2-v3;

5. 叉乘和点乘

v3=v1*v2; //Cross product
double a=dot(v1,v2)//Scalar product

符合向量的叉乘和点乘规则；

6. Reset一个向量

SetToZero(v1);

7. 向量之间的比较

v1==v2;
v2!=v3;
Equal(v3,v4,eps);//with accuracy eps

逐个元素进行比较，可以自定义精度eps；如果不使用自定义精度，那么精度就是默认定义在文件utility.cxx的KDL::epsilon中；

namespace KDL {

int STREAMBUFFERSIZE=10000;
int MAXLENFILENAME = 255;
const double PI=       3.1415926535897932384626433832795;
const double deg2rad = 0.01745329251994329576923690768488;
const double rad2deg = 57.2957795130823208767981548141052;
double epsilon = 0.000001;
}

8. 将Vector里的每个元素取反

v1.ReverseSign();
v2 = -v1;

inline void ReverseSign();
inline friend Vector operator-(const Vector& arg);

9. 向量归一化

double n1 = v1.Normalize();
double n2 = v2.Norm();

/*
* Normalizes this vector and returns it norm
* makes v a unitvector and returns the norm of v.
* if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
* if this is not good, check the return value of this method.
*/
double Normalize(double eps=epsilon);

//!    @return the norm of the vector
double Norm() const;

Normalize()函数将向量归一化，并返回它的范数，让其成为单位向量。如果范数小于精度eps则返回向量为：(1,0,0)，范数为0。求范数的方法是：norm=x2+y2+z2−−−−−−−−−−√$norm=\sqrt{x^2+y^2+z^2}$ ，求归一化向量的方法是：(x/norm,y/norm,z/norm)$(x/norm,y/norm,z/norm)$ ；
Norm()函数返回向量的范数，不归一化；

10. 二维向量转成三维向量

v1.Set2DXY(v2);//v1.x=v2.x, v1.y=v2.y, v1.z=0

这里v2表示XY平面的二维向量，v1表示三维向量；其他平面二维向量也是同样；

inline void Set2DXY(const Vector2& v);
inline void Set2DYZ(const Vector2& v);
inline void Set2DZX(const Vector2& v);

inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);

Set2DPlane函数表示将二维XY平面的向量转成三维，并在某一座标系中表示。其中F_someframe_XY表示一个座标变换T，T由3*3旋转矩阵R和平移向量p组成，那么：

v1.Set2DPlane(T,v2);

表示先进行：v1.Set2DXY(v2)$v1.Set2DXY(v2)$ ，再进行：R∗v1+p$R\ast v1+p$ ；

KDL::Rotation

Rotation是一个3x3矩阵，表示物体相对于一个座标系的三维旋转；

⎡⎣⎢XxXyXzYxYyYzZxZyZz⎤⎦⎥$$\left[\begin{array}{ccc}Xx& Yx& Zx\\ Xy& Yy& Zy\\ Xz& Yz& Zz\end{array}\right]$$

用数组表示就是：

⎡⎣⎢data[0]data[3]data[6]data[1]data[4]data[7]data[2]data[5]data[8]⎤⎦⎥$$\left[\begin{array}{ccc}data[0]& data[1]& data[2]\\ data[3]& data[4]& data[5]\\ data[6]& data[7]& data[8]\end{array}\right]$$

1. 创建旋转矩阵

创建旋转矩阵有两种方式，安全的方式和不安全的方式；
以下的方式为安全的方式创建旋转矩阵：
所谓安全，是指通过下面的方式创建的旋转矩阵是一致的，即矩阵是单位正交阵。

Rotation r1; //The default constructor, initializes to an 3x3 identity matrix
Rotation r2 = Rotation::Identity();//Identity Rotation = zero rotation
Rotation r3 = Rotation::RPY(roll,pitch,yaw); //Rotation built from Roll-Pitch-Yaw angles
Rotation r4 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation built from Euler Z-Y-Z angles
Rotation r5 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation built from Euler Z-Y-X angles
Rotation r6 = Rotation::Rot(vector,angle); //Rotation built from an equivalent axis(vector) and an angle.

r1调用默认构造函数，初始化为单位矩阵；r2表示用单位矩阵初始化；r3使用RPY角初始化；r4使用ZYZ欧拉角初始化；r5使用ZYX欧拉角初始化；r6采用绕任意轴vector旋转角度angle的方式初始化；
这里提到的概念：RPY角、ZYZ欧拉角、ZYX欧拉角、绕任意轴旋转，都是表示两个座标系之间旋转关系的方式，也可以说是表示物体姿态的方式，具体见参考文献；

以下创建旋转矩阵的方式为不安全的方式：
不安全的方式意味着旋转矩阵不一定是单位正交阵，程序也不会去检查是否是单位正交阵；

Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//Give each individual element (Column-Major)
Rotation r7(vectorX,vectorY,vectorZ);//Give each individual column

2. 取值

取旋转矩阵中单个元素的值：

double Zx = r1(0,2);

Zx表示旋转矩阵中第一行第三列的值；索引从0到2；
也可以获取ZYZ欧拉角、ZYX欧拉角、RPY角以及任意旋转轴和角度：

r1.GetEulerZYZ(alpha,beta,gamma);
r1.GetEulerZYX(alpha,beta,gamma);
r1.GetRPY(roll,pitch,yaw);
axis = r1.GetRot();//gives only rotation axis
angle = r1.GetRotAngle(axis);//gives both angle and rotation axis

除此之外，还可以获取单位向量的值：

vecX=r1.UnitX();//or
r1.UnitX(vecX);
vecY=r1.UnitY();//or
r1.UnitY(vecY);
vecZ=r1.UnitZ();//or
r1.UnitZ(vecZ);

3. 旋转矩阵的逆/转置

旋转矩阵是正交阵，逆与转置相同；

r1.SetInverse();//r1 is inverted and overwritten

将r1取逆，并将r1覆盖；

r2=r1.Inverse();//r2 is the inverse rotation of r1

r2等于r1的逆，r1没有被覆盖；
另外三种方式：

v2 = r1.Inverse(v1);
inline Vector Inverse(const Vector& v) const;
w2 = r2.Inverse(w1);
inline Wrench Inverse(const Wrench& arg) const;
t2 = r3.Inverse(t1);
inline Twist Inverse(const Twist& arg) const;

v2相当于v2=r1.Inverse()∗v1$v2=r1.Inverse()\ast v1$ ，更有效率的一种写法，矩阵的逆与向量相乘；
同理，w2=r2.Inverse()∗w1$w2=r2.Inverse()\ast w1$ ，t2=r3.Inverse()∗t1$t2=r3.Inverse()\ast t1$ 。
这里的Wrench是6*1的力和力矩向量，那3*3的旋转矩阵怎么能和6*1的向量相乘呢？其实是将力和力矩分成两个3*1的向量：f和t，在分别进行R1.Inverse()∗f$R1.Inverse()\ast f$ 和 R1.Inverse()∗t$R1.Inverse()\ast t$ 最后再拼成一个6*1向量返回；

4.构造旋转矩阵

可以将两个旋转矩阵构造一个新旋转矩阵，旋转顺序很重要：

r3=r1*r2;

构造围绕X-Y-Z旋转的旋转矩阵：

r1.DoRotX(angle);
r2.DoRotY(angle);
r3.DoRotZ(angle);

如果执行r1.DoRotX(angle);$r1.DoRotX(angle);$ 相当于是将当前旋转矩阵沿x轴旋转角度angle，注意当前矩阵会变成旋转后的矩阵，也相当于执行： r1*RotX(angle)；
另一种复杂的写法是：

r1 = r1*Rotation::RotX(angle)

绕座标轴的旋转矩阵：

r1 = RotX(angle)；
r2 = RotY(angle)；
r3 = RotZ(angle)；

r1返回沿x轴旋转角度angle的旋转矩阵：⎡⎣⎢1000cssn0−sncs⎤⎦⎥$\left[\begin{array}{ccc}1& 0& 0\\ 0& cs& -sn\\ 0& sn& cs\end{array}\right]$ ；
r2返回沿y轴旋转角度angle的旋转矩阵：⎡⎣⎢cs0−sn010sn0cs⎤⎦⎥$\left[\begin{array}{ccc}cs& 0& sn\\ 0& 1& 0\\ -sn& 0& cs\end{array}\right]$ ；
r3返回沿z轴旋转角度angle的旋转矩阵：⎡⎣⎢cssn0−sncs0001⎤⎦⎥$\left[\begin{array}{ccc}cs& -sn& 0\\ sn& cs& 0\\ 0& 0& 1\end{array}\right]$ ；

static Rotation Rot(const Vector& rotvec,double angle);
static Rotation Rot2(const Vector& rotvec,double angle);

Rot和Rot2函数都是沿任意向量旋转角度angle后返回旋转矩阵，区别是Rot不要求向量归一化，而Rot2要求向量归一化。在Rot中，如果向量的范数太小，则返回单位矩阵，首先会将向量归一化然后调用Rot2。在Rot2中具体实现是：
设向量v=[vx vy vz]T$v=[v_x{v}_y{v}_z{]}^T$ ，ct=cos(angle),st=sin(angle)$c_t=cos(angle),s_t=sin(angle)$ ，则绕向量v旋转角度angle的矩阵为：

⎡⎣⎢ct+(1−ct)v2xvzst+(1−ct)vxvy−vyst+(1−ct)vxvz−vzst+(1−ct)vxvyct+(1−ct)v2yvxst+(1−ct)vyvzvyst+(1−ct)vxvz−vxst+(1−ct)vyvzct+(1−ct)v2z⎤⎦⎥$$\left[\begin{array}{ccc}{c}_t+(1-c_t)v_x^2& -v_z{s}_t+(1-c_t)v_x{v}_y& v_y{s}_t+(1-c_t)v_x{v}_z\\ v_z{s}_t+(1-c_t)v_x{v}_y& c_t+(1-c_t)v_y^2& -v_x{s}_t+(1-c_t)v_y{v}_z\\ -v_y{s}_t+(1-c_t)v_x{v}_z& v_x{s}_t+(1-c_t)v_y{v}_z& c_t+(1-c_t)v_z^2\end{array}\right]$$

5. 向量的旋转

旋转矩阵与向量相乘：

 v2=r1*v1;

6. 旋转矩阵的比较

根据用户定义或者默认精度逐个元素进行对比：

r1==r2;
r1!=r2;
Equal(r1,r2,eps);

参考文献

[1] Introduction to Robotics Mechanics and Control, 机器人学导论（美）HLHN J.CRAIG著 贠超等译；
[2] Robot Modeling and Control, 机器人建模和控制（美）马克 W. 斯庞；