开源机器⼈库orocosKDL学习笔记（⼆）：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
KDL::V ector=[x y z]T
Vector是⼀个三维向量，包括X-Y-Z的坐标值。表⽰⼀个点相对于参考坐标系的三维坐标，。
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 t to zero
分别表⽰：v1表⽰使⽤默认初始化函数初始化⼀个零向量；v2表⽰⽤x,y,z初始化；v3表⽰将v2复制初始化v3；v4表⽰⽤零向量初始化；
2. Get/Set 元素
有两种⽅式get/t单个元素：使⽤索引[ ]和( )操作；使⽤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. Ret⼀个向量
SetToZero(v1);
7. 向量之间的⽐较
cpu工作原理
v1==v2;
v2!=v3;
Equal(v3,v4,eps);//with accuracy eps
逐个元素进⾏⽐较，可以⾃定义精度eps；如果不使⽤⾃定义精度，那么精度就是默认定义在⽂件的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.ReverSign();
v2 = -v1;
inline void ReverSign();
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()函数返回向量的范数，不归⼀化；
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);
表⽰先进⾏： ，再进⾏：；
⼆、KDL::Rotation
Rotation是⼀个3*3矩阵，表⽰物体相对于⼀个坐标系的三维旋转；
⽤数组表⽰就是：
1. 创建旋转矩阵
创建旋转矩阵有两种⽅式，安全的⽅式和不安全的⽅式；
以下的⽅式为安全的⽅式创建旋转矩阵：
所谓安全，是指通过下⾯的⽅式创建的旋转矩阵是⼀致的，即矩阵是单位正交阵。
norm =x +y +z 222(x /norm ,y /norm ,z /norm )v 1.Set 2DXY (v 2)R ∗v 1+p ⎣⎡Xx Xy Xz Y x Y y Y z Zx Zy Zz ⎦
⎤⎣⎡data [0]data [3]data [6]data [1]data [4]data [7]data [2]data [5]data [8]⎦
⎤
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.SetInver();//r1 is inverted and overwritten
将r1取逆，并将r1覆盖；
冬天泡温泉
r2=r1.Inver();//r2 is the inver rotation of r1
r2等于r1的逆，r1没有被覆盖；
另外三种⽅式：
v2 = r1.Inver(v1);
inline Vector Inver(const Vector& v) const;
w2 = r2.Inver(w1);
inline Wrench Inver(const Wrench& arg) const;
t2 = r3.Inver(t1);
inline Twist Inver(const Twist& arg) const;
v2相当于 ，更有效率的⼀种写法，矩阵的逆与向量相乘；
同理，，。
这⾥的Wrench是61的⼒和⼒矩向量，那33的旋转矩阵怎么能和61的向量相乘呢？其实是将⼒和⼒矩分成两个31的向量：f和t，在分别进⾏ 和 最后再拼成⼀个6*1向量返回；
4.构造旋转矩阵
可以将两个旋转矩阵构造⼀个新旋转矩阵，旋转顺序很重要：
r3=r1*r2;
构造围绕X-Y-Z旋转的旋转矩阵：
r1.DoRotX(angle);
r2.DoRotY(angle);
r3.DoRotZ(angle);
如果执⾏ 相当于是将当前旋转矩阵沿x轴旋转⾓度angle，注意当前矩阵会变成旋转后的矩阵，也相当于执⾏：r1*RotX(angle)；
另⼀种复杂的写法是：
r1 = r1*Rotation::RotX(angle)
绕坐标轴的旋转矩阵：
r1 = RotX(angle)；
r2 = RotY(angle)；
r3 = RotZ(angle)；
r1返回沿x轴旋转⾓度angle的旋转矩阵：；
r2返回沿y轴旋转⾓度angle的旋转矩阵：；
r3返回沿z轴旋转⾓度angle的旋转矩阵：；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旋转⾓度angle的矩阵为：
5. 向量的旋转
旋转矩阵与向量相乘：
v2=r1*v1;v 2=r 1.Inver ()∗v 1w 2=r 2.Inver ()∗w 1t 2=r 3.Inver ()∗t 1R 1.Inver ()∗f R 1.Inver ()∗t r 1.DoRotX (angle );⎣⎡1000
cs sn 0−sn cs ⎦⎤⎣⎡cs 0−sn 010sn 0cs ⎦⎤⎣⎡cs sn 0
−sn cs 0001⎦⎤v =[v v v ]x y z T c =t cos (angle ),s =t sin (angle )⎣⎡c +(1−c )v t t x 2
v s +(1−c )v v z t t x y −v s +(1−c )v v y t t x z −v s +(1−c )v v z t t x y c +(1−c )v t t y 2v s +(1−c )v v x t t y z v s +(1−c )v v y t t x z −v s +(1−c )v v x t t y z c +(1−c )v t t z 2⎦⎤
