开源机器人库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 是个开源库&