开源机器人库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=[x\ y\ z]^T$KDL::Vector=[x y z]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);
1
2
3

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
1
2
3
4

分别表示：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
1
2
3

[ ]和( )操作索引从 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);
1
2
3
4
5
6

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

3. 标量乘除

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

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

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

4. 向量之间的加减

v2+=v1;
v3-=v1;
v4=v1+v2;
v5=v2-v3;
1
2
3
4

5. 叉乘和点乘

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

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

6. Reset一个向量

SetToZero(v1);
1

7. 向量之间的比较

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

逐个元素进行比较，可以自定义精度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;
}
1
2
3
4
5
6
7
8
9

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

v1.ReverseSign();
v2 = -v1;
1
2

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

9. 向量归一化

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

/*
* 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;
1
2
3
4
5
6
7
8
9
10

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

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

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

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

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

v1.Set2DPlane(T,v2);
1

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

二、KDL::Rotation

Rotation是一个3*3矩阵，表示物体相对于一个坐标系的三维旋转；
⎡⎣⎢XxXyXzYxYyYzZxZyZz⎤⎦⎥$\begin{bmatrix}Xx & Yx & Zx \\Xy & Yy & Zy \\Xz & Yz & Zz\end{bmatrix}$⎣⎡​XxXyXz​YxYyYz​ZxZyZz​⎦⎤​
用数组表示就是：
⎡⎣⎢data[0]data[3]data[6]data[1]data[4]data[7]data[2]data[5]data[8]⎤⎦⎥$\begin{bmatrix}data[0] & data[1] & data[2] \\data[3] & data[4] & data[5] \\data[6] & data[7] & data[8]\end{bmatrix}$⎣⎡​data[0]data[3]data[6]​data[1]data[4]data[7]​data[2]data[5]data[8]​⎦⎤​

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.
1
2
3
4
5
6

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
1
2

2. 取值

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

double Zx = r1(0,2);
1

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
1
2
3
4
5

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

vecX=r1.UnitX();//or
r1.UnitX(vecX);
vecY=r1.UnitY();//or
r1.UnitY(vecY);
vecZ=r1.UnitZ();//or
r1.UnitZ(vecZ);
1
2
3
4
5
6

3. 旋转矩阵的逆/转置

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

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

将r1取逆，并将r1覆盖；

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

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;
1
2
3
4
5
6

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

4.构造旋转矩阵

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

r3=r1*r2;
1

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

r1.DoRotX(angle);
r2.DoRotY(angle);
r3.DoRotZ(angle);
1
2
3

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

r1 = r1*Rotation::RotX(angle)
1

绕坐标轴的旋转矩阵：

r1 = RotX(angle)；
r2 = RotY(angle)；
r3 = RotZ(angle)；
1
2
3

r1返回沿x轴旋转角度angle的旋转矩阵：⎡⎣⎢1000cssn0−sncs⎤⎦⎥$\begin{bmatrix}1 & 0 & 0 \\0 & cs & -sn \\0 & sn & cs\end{bmatrix}$⎣⎡​100​0cssn​0−sncs​⎦⎤​；
r2返回沿y轴旋转角度angle的旋转矩阵：⎡⎣⎢cs0−sn010sn0cs⎤⎦⎥$\begin{bmatrix}cs & 0 & sn \\0 & 1 & 0 \\-sn & 0 & cs\end{bmatrix}$⎣⎡​cs0−sn​010​sn0cs​⎦⎤​；
r3返回沿z轴旋转角度angle的旋转矩阵：⎡⎣⎢cssn0−sncs0001⎤⎦⎥$\begin{bmatrix}cs & -sn & 0 \\sn & cs & 0 \\0 & 0 & 1\end{bmatrix}$⎣⎡​cssn0​−sncs0​001​⎦⎤​；

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

Rot和Rot2函数都是沿任意向量旋转角度angle后返回旋转矩阵，区别是Rot不要求向量归一化，而Rot2要求向量归一化。在Rot中，如果向量的范数太小，则返回单位矩阵，首先会将向量归一化然后调用Rot2。在Rot2中具体实现是：
设向量v=[vx vy vz]T$v = [v_x\ v_y\ v_z]^T$v=[vx​ vy​ vz​]T ，ct=cos(angle),st=sin(angle)$c_t=cos(angle), s_t=sin(angle)$ct​=cos(angle),st​=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⎤⎦⎥⎥$\begin{bmatrix}c_t+(1-c_t)v_x^2 & -v_zs_t+(1-c_t)v_xv_y & v_ys_t+(1-c_t)v_xv_z \\v_zs_t+(1-c_t)v_xv_y & c_t+(1-c_t)v_y^2 & -v_xs_t+(1-c_t)v_yv_z \\-v_ys_t+(1-c_t)v_xv_z & v_xs_t+(1-c_t)v_yv_z & c_t+(1-c_t)v_z^2\end{bmatrix}$⎣⎡​ct​+(1−ct​)vx2​vz​st​+(1−ct​)vx​vy​−vy​st​+(1−ct​)vx​vz​​−vz​st​+(1−ct​)vx​vy​ct​+(1−ct​)vy2​vx​st​+(1−ct​)vy​vz​​vy​st​+(1−ct​)vx​vz​−vx​st​+(1−ct​)vy​vz​ct​+(1−ct​)vz2​​⎦⎤​

5. 向量的旋转

---

旋转矩阵与向量相乘：

 v2=r1*v1;
1

6.旋转矩阵的比较

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

r1==r2;
r1!=r2;
Equal(r1,r2,eps);
1
2
3

三、KDL::Frame

Frame 是由一个Vector 和一个Rotation组合而成。在机器人学中，一个3*1的Vector可以表示位置，而一个3*3的Rotation可以表示姿态，组合起来就可以表示一个坐标系相对于另一个坐标系的位置和姿态。
Frame类中保存数据的两个变量是：

Vector p;       //!< origine of the Frame
Rotation M;     //!< Orientation of the Frame
1
2

其中，Vector的物理意义是3*1的向量，其具体实现是一个长度为3的double型数组；而Rotation在机器人学里的物理意义是3*3的旋转矩阵，其实现是一个长度为9的double型数组；
这里先约定一种符号表示方法，即ABT$_{B}^{A}\textrm{T}$BA​T表示坐标系B相对于坐标系A的变换，相当于Frame；而APBORG$^{A}\textrm{P}_{BORG}$APBORG​表示坐标系B的原点在坐标系A中的平移向量，相当于Frame::p；ABR$_{B}^{A}\textrm{R}$BA​R表示坐标系B相对于坐标系A的旋转变换，相当于Frame::M；

1. 构造函数

inline Frame() {}
1

默认构造函数。

inline Frame(const Rotation& R,const Vector& V);
1

由一个Rotation和一个Vector构造Frame。

//! The rotation matrix defaults to identity
explicit inline Frame(const Vector& V);
1
2

由一个Vector构造Frame，其中Rotation被构造成单位矩阵。

//! The position matrix defaults to zero
explicit inline Frame(const Rotation& R);
1
2

由一个Rotation构造Frame，其中Vector被构造成零向量。

//! The copy constructor. Normal copy by value semantics.
inline Frame(const Frame& arg);
1
2

复制构造函数，由另一个Frame构造Frame。

//! Normal copy-by-value semantics.
inline Frame& operator = (const Frame& arg);
1
2

赋值构造函数。

//! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
inline static Frame Identity();
1
2

返回一个由单位矩阵M和零向量p组成的Frame。

//! Reads data from an double array
//\TODO should be formulated as a constructor
void Make4x4(double* d);
1
2
3

这个函数的目的是将4*4的Frame存放到长度为16的double型数组里面：如下，矩阵里的数字表示数组的序号。
⎡⎣⎢⎢⎢⎢0481215913261014371115⎤⎦⎥⎥⎥⎥$\begin{bmatrix}0 & 1 & 2 & 3 \\4 & 5 & 6 & 7 \\8 & 9 & 10 & 11\\12 & 13 & 14 & 15\end{bmatrix}$⎣⎢⎢⎡​04812​15913​261014​371115​⎦⎥⎥⎤​

2. 取值函数

//!  Treats a frame as a 4x4 matrix and returns element i,
//!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
inline double operator()(int i,int j);

//!  Treats a frame as a 4x4 matrix and returns element i,j
//!    Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
inline double operator() (int i,int j) const;
1
2
3
4
5
6
7

这个函数的目的是将Frame看成4*4的矩阵，如下：
⎡⎣⎢⎢⎢⎢M(0,0)M(1,0)M(2,0)0M(0,1)M(1,1)M(2,1)0M(0,2)M(1,2)M(2,2)0p(0)p(1)p(2)1⎤⎦⎥⎥⎥⎥$\begin{bmatrix}M(0,0) & M(0,1) & M(0,2) & p(0) \\M(1,0) & M(1,1) & M(1,2) & p(1) \\M(2,0) & M(2,1) & M(2,2) & p(2)\\0 & 0 & 0 & 1\end{bmatrix}$⎣⎢⎢⎡​M(0,0)M(1,0)M(2,0)0​M(0,1)M(1,1)M(2,1)0​M(0,2)M(1,2)M(2,2)0​p(0)p(1)p(2)1​⎦⎥⎥⎤​
然后返回第i行第j列的元素；

3. 求逆函数

//! Gives back inverse transformation of a Frame
inline Frame Inverse() const;
1
2

刚才说过Frame可以看作是一个4*4的矩阵，那么这个函数的目的就是求解并返回这个矩阵的逆。
在机器人学里Frame又被称作齐次变换矩阵，那么求Frame的齐次变换逆矩阵就有一种简单有效的方法：
F−1=[MT000−MTp1]$F^{-1}=\begin{bmatrix}M^{T} & -M^{T}p\\ \begin{matrix}0 & 0 & 0\end{matrix} & 1\end{bmatrix}$F−1=[MT0​0​0​​−MTp1​]
KDL中也是这么实现的。其中，旋转矩阵M是正交矩阵，所以M的转置和M的逆相同，这一点在Rotation中也有提到。

//! The same as p2=R.Inverse()*p but more efficient.
inline Vector Inverse(const Vector& arg) const;

Vector Frame::Inverse(const Vector& arg) const
{
    return M.Inverse(arg-p);
}
1
2
3
4
5
6
7

这个函数的返回值是一个Vector：
M−1(arg−p)$M^{-1}(arg-p)$M−1(arg−p)
暂时不知道这个函数有什么用。

//! The same as p2=R.Inverse()*p but more efficient.
inline Wrench Inverse(const Wrench& arg) const;

Wrench Frame::Inverse(const Wrench& arg) const
{
    Wrench tmp;
    tmp.force =  M.Inverse(arg.force);
    tmp.torque = M.Inverse(arg.torque-p*arg.force);
    return tmp;
}
1
2
3
4
5
6
7
8
9
10

这个函数中，Wrench的物理意义是力和力矩向量，具体实现是由两个Vector组成：

Vector force;       //!< Force that is applied at the origin of the current ref frame
Vector torque;      //!< Torque that is applied at the origin of the current ref frame
1
2

Wrench类的细节可以参见KDL::Wrench章节。
这个函数的物理意义是力和力矩向量在两个坐标系间的变换，即：
WA=FABWB$W^{A}=F_{B}^{A}W^{B}$WA=FBA​WB
在实际实现中，分两步进行，力向量的变换和力矩向量的变换。
力向量的变换只与坐标系的旋转变换有关，所以力向量变换的结果是：M−1f⃗ $M^{-1}\vec{f}$M−1f​
力矩向量的变换与坐标系的旋转变换和平移变换有关，所以力矩向量变换的结果是：M−1(t⃗ −p⃗ ×f⃗ )$M^{-1}(\vec{t}-\vec{p}\times \vec{f})$M−1(t−p​×f​)

//! The same as p2=R.Inverse()*p but more efficient.
inline Twist  Inverse(const Twist& arg) const;
1
2

这个函数中，Twist的物理意义是平移速度和旋转速度向量，具体实现是由两个Vector组成：

Vector vel; //!< The velocity of that point
Vector rot; //!< The rotational velocity of that point.
1
2

Twist类的细节可以参见KDL::Twist章节。
这个函数与上一个类似，物理意义是平移速度和旋转速度向量在两个坐标系间的变换，即：TA=FABTB$T^{A}=F_{B}^{A}T^{B}$TA=FBA​TB
在实际实现中，分两步进行，平移速度向量的变换和旋转速度向量的变换。
旋转速度向量的变换只与坐标系的旋转变换有关，所以平移速度向量变换的结果是：M−1r⃗ $M^{-1}\vec{r}$M−1r
平移速度向量的变换与坐标系的旋转变换和平移变换有关，所以旋转速度向量变换的结果是：M−1(v⃗ −p⃗ ×r⃗ )$M^{-1}(\vec{v}-\vec{p}\times \vec{r})$M−1(v−p​×r)

4. 乘号重构

//! Transformation of the base to which the vector
//! is expressed.
inline Vector operator * (const Vector& arg) const;
1
2
3

这个函数的意义是，已知B相对于A的变换矩阵ABT$_{B}^{A}\textrm{T}$BA​T和C相对于B的平移变换向量BPCORG$^{B}\textrm{P}_{CORG}$BPCORG​，求C相对于A的平移变换向量：
APCORG=ABRBPCORG+APBORG$^{A}\textrm{P}_{CORG}=_{B}^{A}\textrm{R}^{B}\textrm{P}_{CORG}+^{A}\textrm{P}_{BORG}$APCORG​=BA​RBPCORG​+APBORG​

inline Wrench operator * (const Wrench& arg) const;
1

这里先约定一个符号：AW$^{A}\textrm{W}$AW表示在坐标系A中表示的力和力矩向量；
这个函数已知的参数是坐标系A相对于B的变换ABT$_{B}^{A}\textrm{T}$BA​T，传入的参数是在坐标系B中表示的力和力矩向量BW$^{B}\textrm{W}$BW，要求的是在坐标系A中表示的力和力矩向量AW$^{A}\textrm{W}$AW。AW$^{A}\textrm{W}$AW可以分成Af$^{A}\textrm{f}$Af和At$^{A}\textrm{t}$At：
Af=ABRBfAt=ABRBt+APBORGBf$^{A}\textrm{f}=_{B}^{A}\textrm{R}^{B}\textrm{f}\\^{A}\textrm{t}=_{B}^{A}\textrm{R}^{B}\textrm{t}+^{A}\textrm{P}_{BORG}{^{B}\textrm{f}}$Af=BA​RBfAt=BA​RBt+APBORG​Bf

inline Twist operator * (const Twist& arg) const;
1

这个函数与上一个函数类似，这里不再赘述。

inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
1

这个函数表示两个Frame相乘，已知B到A的变换及C到B的变换，求C到A的变换：
ACT=ABTBCT$_{C}^{A}\textrm{T}={_{B}^{A}\textrm{T}}{_{C}^{B}\textrm{T}}$CA​T=BA​TCB​T

5. DH参数

static Frame DH_Craig1989(double a,double alpha,double d,double theta);
1

这个函数是根据Craig的书P59公式3-6，由DH参数得到各个连杆的变换矩阵。Craig书里的DH就称为Modified DH吧。

static Frame DH(double a,double alpha,double d,double theta);
1

这个函数与上个函数一样，也是根据DH参数求连杆变换矩阵，不同的是这个DH是Non-Modified DH，来自于DH的创始人Denavit, J. and Hartenberg的一篇论文：A kinematic notation for lower-pair mechanisms based on matrices.

6. Frame积分

//! The twist <t_this> is expressed wrt the current
//! frame.  This frame is integrated into an updated frame with
//! <samplefrequency>.  Very simple first order integration rule.
inline void Integrate(const Twist& t_this,double frequency);
1
2
3
4

参数t_this是一个相对于本Frame的Twist向量；frequency是更新频率即采样频率；
这个函数的意义是在本Frame（坐标系）上作用一个相对于本坐标系的平移和旋转速度向量Twist::t_this，以频率为frequency更新Frame；这个函数的具体实现就不写了，源码里面很清楚。

7. Frame比较

//! do not use operator == because the definition of Equal(.,.) is slightly
//! different.  It compares whether the 2 arguments are equal in an eps-interval
inline friend bool Equal(const Frame& a,const Frame& b,double eps);
1
2
3

这个函数是比较两个Frame a和b是否相同，eps是精度，只有当p和M全都相同才返回True。

//! The literal equality operator==(), also identical.
inline friend bool operator==(const Frame& a,const Frame& b);
//! The literal inequality operator!=().
inline friend bool operator!=(const Frame& a,const Frame& b);
1
2
3
4

operator== 的底层实现还是使用了Equal函数，与Equal函数作用相同，只不过使用的是默认精度；
operator!= 的底层实现是对operator== 取反。

四、KDL::Twist

Twist由一个平移速度向量和一个旋转速度向量组成：

Vector vel; //!< The velocity of that point
Vector rot; //!< The rotational velocity of that point.
1
2

1. 构造函数

Twist():vel(),rot() {};
1

默认构造函数，vel和rot都是零向量。

Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
1

通过vel和rot来构造Twist。

2. 操作符重构

inline Twist& operator-=(const Twist& arg);
inline Twist& operator+=(const Twist& arg);
inline double& operator()(int i);
inline double operator()(int i) const;
double operator[] ( int index ) const
double& operator[] ( int index )
1
2
3
4
5
6

下标操作时i=0…5。

inline friend Twist operator*(const Twist& lhs,double rhs);
inline friend Twist operator*(double lhs,const Twist& rhs);
inline friend Twist operator/(const Twist& lhs,double rhs);
inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
inline friend Twist operator-(const Twist& arg);
1
2
3
4
5
6

前两个乘号都是向量乘标量；除号表示向量除标量；加减都是向量的加减；最后一个是向量所有元素都取反。

inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
1

两个Twist做叉乘，其结果是：

return Twist(lhs.rot*rhs.vel+lhs.vel*rhs.rot,lhs.rot*rhs.rot);
1

其中向量相乘都表示叉乘。

inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);
1

一个Twist和一个Wrench进行叉乘，其结果是一个Wrench:

return Wrench(lhs.rot*rhs.force,lhs.rot*rhs.torque+lhs.vel*rhs.force);
1

3. 其他函数

inline friend double dot(const Twist& lhs,const Wrench& rhs);
inline friend double dot(const Wrench& rhs,const Twist& lhs);
1
2

向量的点乘，不管Twist放在左边还是右边结果都是：

return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1

inline friend void SetToZero(Twist& v);
1

这个函数就是把Twist里的vel和rot都设置成零向量。

static inline Twist Zero();
1

直接返回一个零Twist。

inline void ReverseSign();
1

将Twist向量中的每个元素都取反。

inline Twist RefPoint(const Vector& v_base_AB) const;
1

这个函数是改变Twist的参考点。即假设原来Twist参考点是A，需要改到新的参考点B，其中A和B在同一个坐标系中，参数v_base_AB表示向量AB，返回值是新参考点的Twist:

return Twist(this->vel+this->rot*v_base_AB,this->rot);
1

4. Twist的比较

inline friend bool Equal(const Twist& a,const Twist& b,double eps);
1

根据精度eps比较Twist中的每个元素是否相等，都相等返回True。

inline friend bool operator==(const Twist& a,const Twist& b);
1

跟Equal一样，只是采用默认参数。

inline friend bool operator!=(const Twist& a,const Twist& b);
1

与==相反。

五、KDL::Wrench

Wrench由一个力向量和一个力矩向量组成：

Vector force;       //!< Force that is applied at the origin of the current ref frame
Vector torque;      //!< Torque that is applied at the origin of the current ref frame
1
2

1. 构造函数

Wrench():force(),torque() {};
Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
1
2

默认构造函数，force和torque都是零向量；也可以指定force和torque向量进行构造。

2. 操作符重构

inline Wrench& operator-=(const Wrench& arg);
inline Wrench& operator+=(const Wrench& arg);
1
2

这两个操作符很容易理解，对Wrench里的每一个元素都进行操作。

inline double& operator()(int i);
inline double operator()(int i) const;
double operator[] ( int index ) const;
double& operator[] ( int index );
1
2
3
4

下标操作，i=0…5。

//! Scalar multiplication
inline friend Wrench operator*(const Wrench& lhs,double rhs);
//! Scalar multiplication
inline friend Wrench operator*(double lhs,const Wrench& rhs);
//! Scalar division
inline friend Wrench operator/(const Wrench& lhs,double rhs);

inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);

//! An unary - operator
inline friend Wrench operator-(const Wrench& arg);
1
2
3
4
5
6
7
8
9
10
11
12

两个乘号操作都是Wrench与标量相乘；除号是Wrench与标量相除；加减都是在两个Wrench中进行；最后一个-是将Wrench中各元素取反。

3. 其他函数

//! Sets the Wrench to Zero, to have a uniform function that sets an object or
//! double to zero.
inline friend void SetToZero(Wrench& v);
1
2
3

将Wrench设置成零向量。

//! @return a zero Wrench
static inline Wrench Zero();
1
2

返回一个零Wrench向量。

inline void ReverseSign();
1

将Wrench中每个元素取反。

inline Wrench RefPoint(const Vector& v_base_AB) const;
1

改变Wrench的参考点。两个参考点要在同一个坐标系，类似Twist的RefPoint函数。

4. 比较函数

inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);
inline friend bool operator==(const Wrench& a,const Wrench& b);
inline friend bool operator!=(const Wrench& a,const Wrench& b);
1
2
3

Equal按精度eps比较两个Wrench，相等返回True；==按默认精度比较，相等返回True；!=按默认精度比较，不等返回True；

六、KDL::Vector2

Vector2是二维向量，底层实现是一个二维数组：

double data[2];
1

1. 构造函数

//! Does not initialise to Zero().
Vector2() {data[0]=data[1] = 0.0;}
inline Vector2(double x,double y);
inline Vector2(const Vector2& arg);
1
2
3
4

默认构造函数是零向量；可以直接通过x,y赋值；也可以通过另一个Vector2进行构造。

2. 操作符重构

inline Vector2& operator = ( const Vector2& arg);
1

两个Vector2可以直接通过等号赋值。

inline double operator()(int index) const;
inline double& operator() (int index);
double operator[] ( int index ) const;
double& operator[] ( int index );
1
2
3
4

下标操作，i=0,1。

inline Vector2& operator-=(const Vector2& arg);
inline Vector2& operator +=(const Vector2& arg);
1
2

各元素对应的做-=和+=操作。

inline friend Vector2 operator*(const Vector2& lhs,double rhs);
inline friend Vector2 operator*(double lhs,const Vector2& rhs);
inline friend Vector2 operator/(const Vector2& lhs,double rhs);
inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
1
2
3
4
5

前两个乘号都是向量与标量相乘；除号是向量与标量相除；加减都是向量之间的操作。

inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
1

向量的叉乘。

inline friend Vector2 operator-(const Vector2& arg);
1

各元素取反。

3. 其他函数

inline double x() const;
inline double y() const;
1
2

x()返回data[0]，y()返回data[1]。

inline void x(double);
inline void y(double);
1
2

分别给data[0]和data[1]赋值。

inline void ReverseSign();
1

Vector2中的各元素取反。

inline friend void SetToZero(Vector2& v);
inline static Vector2 Zero();
1
2

将向量设置成零向量；返回零向量。

double Norm() const;
1

返回向量的2-范数，2-范数的求法见Vector一节。

/** 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);
1
2
3
4
5
6

向量归一化。返回向量的2-范数n=Norm()，并将向量v转换成单位向量v=v/n。如果n<eps，则v=[1,0]，返回0。

inline void Set3DXY(const Vector& v);
inline void Set3DYZ(const Vector& v);
inline void Set3DZX(const Vector& v);
1
2
3

Set3DXY()函数将3维向量Vector中XY设置到二维向量Vector2中，即：data[0]=v[0],data[1]=v[1]；其他两个向量作用相同。

inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
1

向量v_someframe经过F_someframe_XY进行坐标变换后，在XY平面的投影，即该函数的实现：

IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe) 
// projects v in the XY plane of F_someframe_XY, and sets *this to these values
// expressed wrt someframe.
{
    Vector tmp = F_someframe_XY.Inverse(v_someframe);
    data[0]=tmp(0);
    data[1]=tmp(1);
}
1
2
3
4
5
6
7
8

4. 比较函数

inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);
inline friend bool operator==(const Vector2& a,const Vector2& b);
inline friend bool operator!=(const Vector2& a,const Vector2& b);
1
2
3

七、KDL::Rotation2

Rotation2是二维旋转变换矩阵，由cos(angle)和sin(angle)组成：

double s,c;
//! c,s represent  cos(angle), sin(angle), this also represents first col. of rot matrix
//! from outside, this class behaves as if it would store the complete 2x2 matrix.
1
2
3

对外表现为2*2的旋转变换矩阵：
[cos(angle)−sin(angle)sin(angle)cos(angle)]$\begin{bmatrix}cos(angle) & sin(angle)\\ -sin(angle) & cos(angle)\end{bmatrix}$[cos(angle)−sin(angle)​sin(angle)cos(angle)​]

1. 构造函数

Rotation2() {c=1.0;s=0.0;}
1

默认构造函数，c初始化成1，s初始化成0。

explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
1

也可以通过一个角度angle_rad来初始化。

Rotation2(double ca,double sa):s(sa),c(ca){}
1

也可以直接给c或s赋值来初始化。

2. 操作符重构

inline Rotation2& operator=(const Rotation2& arg);
1

可以通过等号直接赋值。

inline Vector2 operator*(const Vector2& v) const;
1

可以跟一个二维向量直接相乘，相当于对该二维向量进行旋转变换。

inline double operator() (int i,int j) const;
1

可以进行下标操作，因为Rotation对外表现出的是一个2*2矩阵，所以下标要有行和列两个。

inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
1

两个2*2矩阵的乘法。

3. 其他函数

inline void SetInverse();
inline Rotation2 Inverse() const;
1
2

分别是求逆和返回逆矩阵。

inline Vector2 Inverse(const Vector2& v) const;
1

返回的向量就是向量v进行旋转变换之后的向量，假设Rotation2表示成旋转矩阵是M，那么返回向量是：Mv。

inline void SetIdentity();
inline static Rotation2 Identity();
1
2

分别是将Rotation2单位化和返回单位化后的Rotation2，单位Rotation2表示c=1,s=0。

IMETHOD void Rotation2::SetRot(double angle) {
    c=cos(angle);s=sin(angle);
}
IMETHOD Rotation2 Rotation2::Rot(double angle) {
    return Rotation2(cos(angle),sin(angle));
}
1
2
3
4
5
6

将角度转成旋转变换矩阵。

inline double GetRot() const;
1

根据旋转矩阵得到旋转角度。

4. 比较函数

inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
1

八、KDL::Frame2

Frame2由二维旋转矩阵和二维向量组成：

Vector2 p;          //!< origine of the Frame
Rotation2 M;        //!< Orientation of the Frame
1
2

可以看成是3*3的矩阵：
⎡⎣⎢cos(angle)−sin(angle)0sin(angle)cos(angle)0xy1⎤⎦⎥$\begin{bmatrix}cos(angle) & sin(angle) & x\\ -sin(angle) & cos(angle) & y\\ 0 & 0 & 1\end{bmatrix}$⎣⎡​cos(angle)−sin(angle)0​sin(angle)cos(angle)0​xy1​⎦⎤​

1. 构造函数

inline Frame2(const Rotation2& R,const Vector2& V);
explicit inline Frame2(const Vector2& V);
explicit inline Frame2(const Rotation2& R);
inline Frame2(void);
inline Frame2(const Frame2& arg);
1
2
3
4
5

默认Vector2初始化成零向量，Rotation2初始化成单位矩阵。

2. 操作符重构

inline Frame2& operator = (const Frame2& arg);
1

可以用等号赋值。

inline Vector2 operator * (const Vector2& arg);
1

这个函数的返回值是：M*arg+p。

inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
1

这个函数是将Frame2看出3*3的矩阵进行相乘。

3. 其他函数

inline void Make4x4(double* d);
1

这个函数在这里感觉是有问题的，因为Frame2只能看成是3*3的矩阵。

inline double operator()(int i,int j);
inline double operator() (int i,int j) const;
1
2

下标操作，i=0,1,2, j=0,1,2。

inline void SetInverse();
inline Frame2 Inverse() const;
1
2

将Frame2看成3*3矩阵，求Frame2的逆：
[M−100−Mp1]$\begin{bmatrix}M^{-1} & -Mp\\ \begin{matrix}0 & 0\end{matrix} & 1\end{bmatrix}$[M−10​0​​−Mp1​]

inline Vector2 Inverse(const Vector2& arg) const;
1

这个函数的返回向量是：M(arg-p)。

inline void SetIdentity();
inline static Frame2 Identity()
1
2

设置成单位矩阵。

inline void Integrate(const Twist& t_this,double frequency);
1

这个函数放在这里没有实现。

4. 比较函数

inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
1

参考文献

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

本篇主要讲述了机器人学中几何描述、变换等基础知识，以及在KDL中如何实现和使用。下一篇将讲解在KDL中如何构造关节串联型机器人以及KDL中的实现方式。