上一篇主要讲述了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=\left[\begin{array}{cccc}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{array}\right]$$
其中i=1…6，这里的theta就是JntToCart中的q_in。
例如：JntToCart(q_in, p_out, -1);
就表示：
$$p\_out=T_6^0=T_1^0{T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^5$$
例如：JntToCart(q_in, p_out, 3);
就表示：
$$p\_out=T_3^0=T_1^0{T}_2^1{T}_3^2$$

本篇主要讲述KDL中正运动学解的实现方式及其使用。下一篇主要讲述KDL中逆运动学解的实现方式及其使用。

最后

以上就是传统雪碧为你收集整理的开源机器人库orocos KDL 学习笔记（四）：Forward Kinematric1. puma560的正运动学解2. JntArray类3. ChainFkSolverPos_recursive类的全部内容，希望文章能够帮你解决开源机器人库orocos KDL 学习笔记（四）：Forward Kinematric1. puma560的正运动学解2. JntArray类3. ChainFkSolverPos_recursive类所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。