开源机器人库orocos KDL 学习笔记四：Forward Kinematric
上一篇主要讲述了KDL中运动链的建立方式，以及与其相关的段（Segment）和关节（Joint）的概念，这些是串联机械臂运动学的基础。本篇主要讲述KDL中正运动学解的实现方式及其使用。
1. puma560的正运动学解
首先还是以puma560作为例子，来看一下如何调用KDL的正运动学求解器求解puma560的正运动学解。
Chain puma560;
puma560 = KDL::Puma560();
ChainFkSolverPos_recursive fwdkin(puma560);
int n = puma560.getNrOfJoints();
JntArray q(n);
Frame pos_goal;
q.data.setRandom();
q.data *= M_PI;
fwdkin.JntToCart(q, pos_goal);
其中ChainFkSolverPos_recursive是正运动学解求解器类；JntArray是表示Chain中Joint数据的类；这两个类是本篇的重点。
JntToCart是正运动学解函数，即通过关节空间数据（即旋转的角度、平移的长度等）求出笛卡尔空间的位置和姿态矩阵。
2. JntArray类
JntArray主要用于存放Chain中关节的数据，数据都存在data中：
Eigen::VectorXd data;
其中Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms. Eigen 是个开源库，可以看到源代码，但是源代码还是有点复杂的，以后如果有时间再详细学习一下。这里可以简单理解data就是一个类型为double，动态长度的向量（或数组）。
2.1 构造函数
JntArray();
默认构造函数，使用这个函数时NULL == data，0 == rows()，使用这个函数要注意use of an object constructed like this, without a resize() first, may result in program exit! 即在使用这个构造函数构造的对象之前必须使用resize()函数指定大小。
explicit JntArray(unsigned int size);
指定长度为size的关节向量，例如puma560的关节数是6，那么size就应该指定为6；
JntArray(const JntArray& arg);
JntArray& operator = ( const JntArray& arg);
复制和赋值构造函数。
2.2 其他函数
2.2.1 resize
void resize(unsigned int newSize);
使用这个函数需要注意的是：This causes a dynamic allocation (and potentially also a dynamic deallocation). This will negatively affect real-time performance!
这个函数直接使用了Eigen中的resize函数，实现过程比较复杂不用管它，这个函数的目的就是改变关节向量的长度。
2.2.2 下标操作
double operator()(unsigned int i,unsigned int j=0)const;
double& operator()(unsigned int i,unsigned int j=0);
这两个函数通过下标来取值或赋值，第二个参数必须是零，因为JntArray是个一维向量而不是二维矩阵。
2.2.3 获取行数或列数
unsigned int rows()const;
unsigned int columns()const;
获取一维向量JntArray的行数或列数，列数永远是1。
2.2.4 加减乘除
friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
friend void Multiply(const JntArray& src,const double& factor,JntArray& dest);
friend void Divide(const JntArray& src,const double& factor,JntArray& dest);
普通的向量加减，向量与常数之间的乘除。
2.2.5 MultiplyJacobian
friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
Jacobian类表示机器人的雅克比矩阵，这个函数的意义是雅克比与关节速度的乘积得到末端速度。
2.2.6 其他
friend void SetToZero(JntArray& array);
friend bool Equal(const JntArray& src1,const JntArray& src2,double eps);
bool operator==(const JntArray& src1,const JntArray& src2);
3. ChainFkSolverPos_recursive类
Implementation of a recursive forward position kinematics algorithm to calculate the position transformation from joint space to Cartesian space of a general kinematic chain (KDL::Chain).
3.1 构造函数
ChainFkSolverPos_recursive(const Chain& chain);
需要一个kinematic chain来构造这个递归运动学正解求解器。
3.2 正解函数
virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
这个函数的输入q_in表示关节角度（如果是旋转关节），输出p_out表示末端位置和姿态矩阵。
segmentNr表示需要求解的运动链中Segment的个数，如果segmentNr<0说明需要求解整条Chain的正解，segmentNr只能小于或等于Chain中Segment的个数。
这个函数的实现方式就是将Chain.Segment.pose进行连乘。Chain.Segment.pose的含义可以在上一篇中找到。
以PUMA560为例，由DH参数可以得到六个Chain.Segment.pose的通用公式：
T i − 1 i = [ c θ i − s θ i c α i s θ i s α i a i c θ i s θ i c θ i c α i − c θ i s α i a i s θ i 0 s α i c α i d i 0 0 0 1 ] T_{i-1}^{i}=\begin{bmatrix} c\theta _{i} & -s\theta _{i}c\alpha _{i} & s\theta _{i}s\alpha _{i} & a_{i}c\theta _{i}\\ s\theta _{i} & c\theta _{i}c\alpha _{i} & -c\theta _{i}s\alpha _{i} & a_{i}s\theta _{i}\\ 0 & s\alpha _{i} & c\alpha _{i} & d_{i}\\ 0 & 0 & 0 & 1 \end{bmatrix} Ti−1i=⎣⎢⎢⎡cθisθi00−sθicαicθicαisαi0sθisαi−cθisαicαi0aicθiaisθidi1⎦⎥⎥⎤
其中i=1…6，这里的theta就是JntToCart中的q_in。
例如：JntToCart(q_in, p_out, -1);
就表示：
p _ o u t = T 6 0 = T 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 p\_out=T_{6}^{0}=T_{1}^{0}T_{2}^{1}T_{3}^{2}T_{4}^{3}T_{5}^{4}T_{6}^{5} p_out=T60=T10T21T32T43T54T65
例如：JntToCart(q_in, p_out, 3);
就表示：
p _ o u t = T 3 0 = T 1 0 T 2 1 T 3 2 p\_out=T_{3}^{0}=T_{1}^{0}T_{2}^{1}T_{3}^{2} p_out=T30=T10T21T32
本篇主要讲述KDL中正运动学解的实现方式及其使用。下一篇主要讲述KDL中逆运动学解的实现方式及其使用。