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

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

Eigen::Matrix<double, 6, 1> L;
 L(0) = 1; L(1) = 1; L(2) = 1;
 L(3) = 0.01; L(4) = 0.01; L(5) = 0.01;
 ChainIkSolverPos_LMA solver(puma560, L);
 JntArray q_init(n);
 JntArray q_sol(n);
 q_init.data.setRandom();
 q_init.data *= M_PI;
 int retval;
 retval = solver.CartToJnt(q_init, pos_goal, q_sol);