开源机器人库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=[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=\sqrt{x^2+y^2+z^2}$，求归一化向量的方法是：$(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)$ ，再进行：$R\ast v1+p$；

二、KDL::Rotation

Rotation是一个3*3矩阵，表示物体相对于一个坐标系的三维旋转；
$$\left[\begin{array}{ccc}Xx& Yx& Zx\\ Xy& Yy& Zy\\ Xz& Yz& Zz\end{array}\right]$$
用数组表示就是：
$$\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()\ast v1$ ，更有效率的一种写法，矩阵的逆与向量相乘；
同理，$w2=r2.Inverse()\ast w1$，$t2=r3.Inverse()\ast t1$。
这里的Wrench是61的力和力矩向量，那33的旋转矩阵怎么能和61的向量相乘呢？其实是将力和力矩分成两个31的向量：f和t，在分别进行$R1.Inverse()\ast f$ 和 $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);$ 相当于是将当前旋转矩阵沿x轴旋转角度angle，注意当前矩阵会变成旋转后的矩阵，也相当于执行： r1*RotX(angle)；
另一种复杂的写法是：

r1 = r1*Rotation::RotX(angle)

绕坐标轴的旋转矩阵：

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

r1返回沿x轴旋转角度angle的旋转矩阵：$\left[\begin{array}{ccc}1& 0& 0\\ 0& cs& -sn\\ 0& sn& cs\end{array}\right]$；
r2返回沿y轴旋转角度angle的旋转矩阵：$\left[\begin{array}{ccc}cs& 0& sn\\ 0& 1& 0\\ -sn& 0& cs\end{array}\right]$；
r3返回沿z轴旋转角度angle的旋转矩阵：$\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=[v_x{v}_y{v}_z{]}^T$ ，$c_t=cos(angle),s_t=sin(angle)$ ，则绕向量v旋转角度angle的矩阵为：
$$\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);

三、KDL::Frame

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

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

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

1. 构造函数

inline Frame() {}

默认构造函数。

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

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

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

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

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

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

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

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

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

赋值构造函数。

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

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

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

这个函数的目的是将4*4的Frame存放到长度为16的double型数组里面：如下，矩阵里的数字表示数组的序号。
$$\left[\begin{array}{cccc}0& 1& 2& 3\\ 4& 5& 6& 7\\ 8& 9& 10& 11\\ 12& 13& 14& 15\end{array}\right]$$

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;

这个函数的目的是将Frame看成4*4的矩阵，如下：
$$\left[\begin{array}{cccc}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{array}\right]$$
然后返回第i行第j列的元素；

3. 求逆函数

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

刚才说过Frame可以看作是一个4*4的矩阵，那么这个函数的目的就是求解并返回这个矩阵的逆。
在机器人学里Frame又被称作齐次变换矩阵，那么求Frame的齐次变换逆矩阵就有一种简单有效的方法：
$$F^{-1}=\left[\begin{array}{cc}{M}^T& -M^{T}p\\ \begin{array}{ccc}0& 0& 0\end{array}& 1\end{array}\right]$$
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);
}

这个函数的返回值是一个Vector：
$$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;
}

这个函数中，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

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

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

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

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

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

4. 乘号重构

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

这个函数的意义是，已知B相对于A的变换矩阵${}_B^A\text{T}$和C相对于B的平移变换向量${}^B{\text{P}}_{CORG}$，求C相对于A的平移变换向量：
$${}^A{\text{P}}_{CORG}{=}_B^A{\text{R}}^B{\text{P}}_{CORG}{+}^A{\text{P}}_{BORG}$$

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

这里先约定一个符号：${}^A\text{W}$表示在坐标系A中表示的力和力矩向量；
这个函数已知的参数是坐标系A相对于B的变换${}_B^A\text{T}$，传入的参数是在坐标系B中表示的力和力矩向量${}^B\text{W}$，要求的是在坐标系A中表示的力和力矩向量${}^A\text{W}$。${}^A\text{W}$可以分成${}^A\text{f}$和${}^A\text{t}$：
$$\begin{gathered} {}^A\text{f}{=}_B^A{\text{R}}^B\text{f}{ \\ }^A\text{t}{=}_B^A{\text{R}}^B\text{t}{+}^A{\text{P}}_{BORG}{}^B\text{f} \end{gathered}$$

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

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

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

这个函数表示两个Frame相乘，已知B到A的变换及C到B的变换，求C到A的变换：
$${}_C^A\text{T}={}_B^A\text{T}{}_C^B\text{T}$$

5. DH参数

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

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

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

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

6. Frame积分

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

参数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);

这个函数是比较两个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);

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

四、KDL::Twist

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

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

1. 构造函数

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

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

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

通过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 )

下标操作时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);

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

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

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

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

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

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

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

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

3. 其他函数

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

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

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

inline friend void SetToZero(Twist& v);

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

static inline Twist Zero();

直接返回一个零Twist。

inline void ReverseSign();

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

inline Twist RefPoint(const Vector& v_base_AB) const;

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

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

4. Twist的比较

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

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

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

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

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

与==相反。

五、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. 构造函数

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

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

2. 操作符重构

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

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

inline double& operator()(int i);
inline double operator()(int i) const;
double operator[] ( int index ) const;
double& operator[] ( int index );

下标操作，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);

两个乘号操作都是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);

将Wrench设置成零向量。

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

返回一个零Wrench向量。

inline void ReverseSign();

将Wrench中每个元素取反。

inline Wrench RefPoint(const Vector& v_base_AB) const;

改变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);

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

六、KDL::Vector2

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

double data[2];

1. 构造函数

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

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

2. 操作符重构

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

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

inline double operator()(int index) const;
inline double& operator() (int index);
double operator[] ( int index ) const;
double& operator[] ( int index );

下标操作，i=0,1。

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

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

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);

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

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

向量的叉乘。

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

各元素取反。

3. 其他函数

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

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

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

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

inline void ReverseSign();

Vector2中的各元素取反。

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

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

double Norm() const;

返回向量的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);

向量归一化。返回向量的2-范数n=Norm()，并将向量v转换成单位向量v=v/n。如果n

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

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);

向量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);
}

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);

七、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.

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

1. 构造函数

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

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

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

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

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

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

2. 操作符重构

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

可以通过等号直接赋值。

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

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

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

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

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

两个2*2矩阵的乘法。

3. 其他函数

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

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

inline Vector2 Inverse(const Vector2& v) const;

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

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

分别是将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));
}

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

inline double GetRot() const;

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

4. 比较函数

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

八、KDL::Frame2

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

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

可以看成是3*3的矩阵：
$$\left[\begin{array}{ccc}cos(angle)& sin(angle)& x\\ -sin(angle)& cos(angle)& y\\ 0& 0& 1\end{array}\right]$$

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);

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

2. 操作符重构

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

可以用等号赋值。

inline Vector2 operator * (const Vector2& arg);

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

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

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

3. 其他函数

inline void Make4x4(double* d);

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

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

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

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

将Frame2看成3*3矩阵，求Frame2的逆：
$$\left[\begin{array}{cc}{M}^{-1}& -Mp\\ \begin{array}{cc}0& 0\end{array}& 1\end{array}\right]$$

inline Vector2 Inverse(const Vector2& arg) const;

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

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

设置成单位矩阵。

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

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

4. 比较函数

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

参考文献

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

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