上一篇主要讲述了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中逆运动学解的实现方式及其使用。

开源机器人库orocos KDL 学习笔记（四）：Forward Kinematric相关推荐

开源机器人库orocos KDL 学习笔记（五）：Inverse Kinematric
上一篇主要讲述KDL中正运动学解的实现方式及其使用.本篇主要讲述KDL中逆运动学解的实现方式及其使用. 1. puma560的逆运动学解首先还是以puma560作为例子,来看一下如何调用KDL的逆运 ...
python中seaborn库_[Python学习笔记(四)] Seaborn库基础学习——01
用 matplotlib能够完成一些基本的图表操作,而 Seaborn 库可以让这些图的表现更加丰富. 1. import seaborn as sns 用于导入 seaborn 库,并取别名为 sn ...
V-rep机器人仿真软件使用的学习笔记
本博客同时发布于个人主页:www.doctorsrn.cn 本博客续:V-rep机器人仿真软件使用的学习笔记-续相关博客:V-rep中机械臂惯性参数的获取方法前言这篇博客主要是记录自己学习和使用 ...
【http学习笔记四】安全篇
[http学习笔记四]安全篇文章目录 [http学习笔记四]安全篇一.HTTPS 与 SSL/TLS ① 什么是安全? 机密性完整性身份认证不可否认 ② 什么是HTTPS? ③ SSL/TL ...
python计算机器人运动学分析_V-rep学习笔记：机器人逆运动学数值解法（The Jacobian Transpose Method）...
机器人运动学逆解的问题经常出现在动画仿真和工业机器人的轨迹规划中:We want to know how the upper joints of the hierarchy would rotate ...
STM32F103学习笔记四时钟系统
STM32F103学习笔记四时钟系统本文简述了自己学习时钟系统的一些框架,参照风水月 1. 单片机中时钟系统的理解 1.1 概述时钟是单片机的脉搏,是单片机的驱动源用任何一个外设都必须打开相应 ...
华清远见fs4412开发板学习笔记(四)
fs4412开发板学习笔记(四) 今天的课程安排 1.复习 1.1 VIM 编辑器 [1] vim + filename 打开 [2] 工作模式命令模式编辑模式底行模式 [3] 模式切换命令- ...
MySQL高级学习笔记(四)
文章目录 MySQL高级学习笔记(四) 1. MySql中常用工具 1.1 mysql 1.1.1 连接选项 1.1.2 执行选项 1.2 mysqladmin 1.3 mysqlbinlog 1.4 ...
C#可扩展编程之MEF学习笔记(四)：见证奇迹的时刻
前面三篇讲了MEF的基础和基本到导入导出方法,下面就是见证MEF真正魅力所在的时刻.如果没有看过前面的文章,请到我的博客首页查看. 前面我们都是在一个项目中写了一个类来测试的,但实际开发中,我们往往要 ...

开源机器人库orocos KDL 学习笔记（四）：Forward Kinematric