- 关节状态State物理意义；对旋转关节，即urdf中type=continuous或revolute，关节状态是旋转轴的旋转角度。举个例子，值1.57，表示此个关节绕旋转轴旋转90度，至于是顺时间还是逆时针，要看axis的正、负号以及开发者如何定义值的变化方向。
在读这篇文章前，一定要非常熟悉urdf中的<joint>、<link>标签，这部分内容可参考“urdf中的<link>、<joint>”，那文章也给出这里要用到的关节a示例。
一、逆运动学求解器概述
参考：【机器人学】机器人开源项目KDL源码学习：（2）牛顿拉普森迭代法求机器人的数值解
求逆解方法分为两类，一是求解析解，moveit中的ikfast，二是求数值解，moveit中的kdl、trac_ik。对第二种，可以将求机器人逆解的问题看成是一个求多元非线性方程组的问题，而牛顿拉普森迭代法就是求这个问题的一个可选的方法。
第一步：方程组可写成如下形式
表示机械臂的正运动学函数，操作空间共有m个自由度。
表示机械臂具有n个关节。如果n>m则机械臂运动学冗余。
表示机械臂末端的在操作空间维度的目标点。
第二步：求取雅可比矩阵：
位姿有6个变量，3个平移和3个旋转，求逆解时，需要同时满足这6个变量，即m=6。相对应的，雅可比矩阵是个6行矩阵。
第三步：迭代法求解下一个关节的位置
其中：
表示当前的误差（目标点的位姿-当前点的位姿），这与（1）式是相反的，所以（3）式的等式后是加号，这么写是为了要和KDL中的程序一致。
逆矩阵使用雅可比矩阵的伪逆矩阵，KDL代码中并没有单独的伪逆矩阵的求解器，而是直接求的伪逆矩阵和误差的乘积。
二、正向运动学方程
正向运动学方程的x是关节状态，y是笛卡儿位姿。
2.1 ChainFkSolverPos_recursive::JntToCart
JntToCart功能是根据关节状态q_in，算出该规划组内某个关节的笛卡儿位姿。过程就是求解正向运动学方程。
- @q_in[IN]。自变量关节状态x。
- @p_out[OUT]。应变量笛卡儿位姿y。
- @seg_nr[IN]。要计算是第几个关节的位姿。如果seq_nr小于0，要计算最后一个关节的位姿。
namespace KDL {
int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr) {
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;如果参数seg_nr小于0，像-1，意味着要计算最后一个关节的位姿。chain.getNrOfSegments()得到该规划组内的关节数，示例有4个关节，segmentNr=4，意味着要计算第4个，也就是最后一个关节的位姿。
p_out = Frame::Identity();
if(q_in.rows()!=chain.getNrOfJoints())
return (error = E_SIZE_MISMATCH);
else if(segmentNr>chain.getNrOfSegments())
return (error = E_OUT_OF_RANGE);
else{
int j=0;
for(unsigned int i=0;i<segmentNr;i++){
const Segment& segment = chain.getSegment(i);
const Joint& joint = segment.getJoint();
if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
p_out = p_out*chain.getSegment(i).pose(q_in(j));i是所有关节的索引。关节中有些是固定关节(type="fixed")，j是非固定关节中的索引。每个非固定关节有一个关节状态，因而j也是q_in的索引。
右侧p_out是索引i-1关节的位姿，左侧p_out是索引i关节在关节状态是q_in时的位姿。
j++;
}else{
p_out = p_out*chain.getSegment(i).pose(0.0);
}
}
return (error = E_NOERROR);
}
}
}2.2 p_out*chain.getSegment(i).pose(q_in(j))
p_out是位姿，“chain.getSegment(i).pose(q_in(j))”的结果也是位姿。为直观，可这么理解乘法的物理意义：向量a要依次经过两次坐标系变换，第一次是“chain.getSegment(i).pose(q_in(j))”，第二次是p_out。p_out就是一个累计位姿，pose较为复杂，或许吧，好多人不太熟悉KDL中的Frame、Rotation、Vector处理坐标变换，而是习惯了用Eigen，在深入前，让先看一段等价的用Eigen写的代码。
- @xyz[IN]：urdf中定义关节的xyz。关节“a”：(-0.02656, 0, 0.05)。
- @rpy[IN]：urdf中定义关节的rpy。关节“a”：(1.5708, 0, -1.5708)。
- @axis_tag_xyz[IN]：urdf中定义的<axis>内的xyz。关节“a”：(0, 0, -1)。
- @angle[IN]：要旋转的角度，即关节状态。值：0.5。
Eigen::Isometry3d chain_joint_pose(const Eigen::Vector3d& xyz, const Eigen::Vector3d& rpy, const Eigen::Vector3d& axis_tag_xyz, double angle)
{
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()));
Eigen::Isometry3d F_parent_jnt = Eigen::Isometry3d::Identity();
F_parent_jnt.rotate(yawAngle * pitchAngle * rollAngle);欧氏变换矩阵F_parent_jnt，此时位置0，角度rpy。
F_parent_jnt: {p:(0, 0, 0), M:(1.57079, 0.00000, -1.57079)}
Eigen::Vector3d _axis = F_parent_jnt.rotation() * axis_tag_xyz; Eigen::Vector3d axis = _axis / _axis.norm();
“F_parent_jnt.rotation() * axis_tag_xyz”将向量axis_tag_xyz变换到F_parent_jnt坐标系，变换时平移值是0。axis是_axis对应的单位向量。对关节a，F_parent_jnt坐标系指的是arm_link。从图1可看到，Link1的Z轴和arm_link的X轴是同一平面，如果“补上”平移，它们处于同一直线，但方向相反。因而上面这个坐标系变换，直观上可以这么为：link1上的“Z=-1”变成了arm_link中的“X=1”。
_axis = (1, 0, 0)。由于_axis长度是1，axis值也是(1, 0, 0)。
Eigen::Isometry3d joint_pose = Eigen::Isometry3d::Identity(); joint_pose.rotate(Eigen::AngleAxisd(angle, axis)); joint_pose.pretranslate(xyz);
欧氏变换矩阵joint_pose，它的位置是xyz，角度是绕单位向量axis旋转angle度。
joint_pose: {p:(-0.02655, 0, 0.05000), M:(0.49999, -0.00000, 0.00000)}
Eigen::Isometry3d f_tip = Eigen::Isometry3d::Identity(); f_tip.rotate(F_parent_jnt.rotation());
f_tip语义“frame from the end of the joint to the tip of the segment, default: Frame::Identity()”。f_tip中的“f”应该指“Frame”，但不知怎么理解tip。——还没理解官方对f_tip的这句英文注释，看翻译好像是从该规划组初端或末端到该关节啥的。但个人认为某个关节的f_tip只依赖该关节数值，和其它关节无关。举个例子，对关节b，计算出的f_tip和前后关节a、c数值无关。
f_tip: {p:(0, 0, 0), M:(1.57079, 0.00000, -1.57079)}
Eigen::Isometry3d result = joint_pose * f_tip;
如果有一个向量a，那result乘上它，等价要依次经过两次坐标变换，第一次变换到arm_link坐标系，但只是旋转，角度对了，没有平移，位置不对。第二次变换是在arm_link坐标系，角度上，绕旋转轴axis旋转angle度，同时位置平移<origin.xyz>。
result: {p:(-0.02655, 0, 0.05000), M:(1.57080, 0.49999, -1.57079)}
return result; }
回到KDL代码pose(...)
Frame Segment::pose(const double& q)const
{
return joint.pose(q)*f_tip;
}
Frame Joint::pose(const double& q)const
{
switch(type){
case RotAxis:
// calculate the rotation matrix around the vector "axis"
if (q != q_previous){
q_previous = q;
joint_pose.M = Rotation::Rot2(axis, scale*q+offset);Rot2用于创建一个绕任意（单位）向量旋转的旋转矩阵，axis是这个旋转轴，scale*q+offst是要旋转的角度。默认scale=1，offset=0，因而第q，这个关节状态值往往就是旋转角度。
} return joint_pose; ... } return Frame::Identity(); }
当中要用到的f_tip、axis表示什么，可参考上面Eigen代码中同名变量。以下是KDL如何计算f_tip。
Segment::Segment(const std::string& _name, const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
name(_name),
joint(_joint),I(_I),
f_tip(_joint.pose(0).Inverse() * _f_tip)
{
}
代入示例值后计算f_tip
_joint.pose(0).Inverse(): {p:(0.02655, 0, -0.05000), M:(0, 0, 0)}
* _f_tip: {p:(-0.02655, 0, 0.05000), M:(1.57079, 0, -1.57079)}
---------------
= f_tip: {p:(0, 0, 0), M:(1.57079, 0, -1.57079)}对“_joint.pose(0).Inverse()”，角度0，位置是_f_tip的逆。目的是把_f_tip的位置0，为何不直接把_f_tip的p部分置0，这不是比“乘”更快？
三、求解雅可比矩阵
还没理解雅可比矩阵细节，像Twist::RefPoint。已有注释只是目前理解的，可能有错误。参考：机械臂的雅可比矩阵这么厉害，怎么把它求出来呢？（雅可比矩阵中篇），【机器人学】机器人开源项目KDL源码学习：（5）KDL如何求解几何雅可比矩阵
- 为求矩阵，动态变量只有N个关节的此时状态q_in，和未端关节的前、后两次位姿误差无关。
- 过程中没有可调优化值的参数。
- 雅可比矩阵维度6xn。固定6行，每行表示线速度、角速度的一个分量。分别是线速度x、线速度y、线速度z、角速度x、角速度y、角速度z。n是该规划组中的非固定关节数，每一列表示一个关节，像示例中的a、b、c，那n=3。
3.1 ChainJntToJacSolver::JntToJac
- @q_in[IN]。此时各关节状态。q_in: [0.5, 0.6, -0.7]
- @jac[OUT]。存储求解出的雅可比矩阵。
- @seg_nr[IN]。要计算是第几个关节的位姿。如果seq_nr小于0，要计算最后一个关节的位姿。语义和ChainFkSolverPos_recursive::JntToCart中的seg_nr一样。
namespace KDL {
int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
{
if(locked_joints_.size() != chain.getNrOfJoints())
return (error = E_NOT_UP_TO_DATE);
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;segmentNr是关节数：4。a、b、c、grasping_frame_joint。
//Initialize Jacobian to zero since only segmentNr columns are computed
SetToZero(jac) ;
...
T_tmp = Frame::Identity();
SetToZero(t_tmp);
int j=0;
int k=0;
Frame total;
for (unsigned int i=0;i<segmentNr;i++) {
//Calculate new Frame_base_ee
if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
//pose of the new end-point expressed in the base
total = T_tmp*chain.getSegment(i).pose(q_in(j));total是在q_in角度时，索引i关节在arm_link坐标系下的笛卡儿位姿。
//changing base of new segment's twist to base frame if it is not locked //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0); if(!locked_joints_[j]) t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
求的是索引i关节对索引i+1连杆末端的速度（在arm_link坐标系下），设索引i关节的角速度为1，
}else{
total = T_tmp*chain.getSegment(i).pose(0.0);
}
//Changing Refpoint of all columns to new ee
changeRefPoint(jac,total.p-T_tmp.p,jac);total是索引i关节的位姿，T_tmp是索引i-1关节的位姿，“total.p-T_tmp.p”是这两姿的位置差。更新雅可比矩阵，第k列表示第k个关节角速度为1时对末端的造成的线速度和角速度
//Only increase jointnr if the segment has a joint
if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
//Only put the twist inside if it is not locked
if(!locked_joints_[j])
jac.setColumn(k++,t_tmp);把索引i关节对应的twist列（t_tmp）“添加”到雅可比矩阵。“添加”加引号，是因为该列已经预分配，严格应该说是更新。由于上面的changeRefPoint，此次更新值到雅可比矩形中的线速度将来会被修改。
j++; } T_tmp = total; } return (error = E_NOERROR); } }
3.2 ChainJntToJacSolver::JntToJac
t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
求的是索引i关节对索引i+1连杆末端的速度（在arm_link坐标系下），设索引i关节的角速度为1。它的核心是要理解Segment::twist。
Twist Segment::twist(const double& q, const double& qdot)const
{
return joint.twist(qdot).RefPoint(joint.pose(q).M * f_tip.p);
}Segment::twist中有4个操作。
[1/4] joint.twist(qdot)
namespace KDL {
Twist Joint::twist(const double& qdot)const
{
switch(type){
case RotAxis:
return Twist(Vector(0,0,0), axis * ( scale * qdot));scale默认1。RotAxis表示按单元向量axis旋转。值是(1, 0, 0)时，等价RotX。
case RotX: return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); case RotY: return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0)); case RotZ: return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); ... } }
joint.twist(qdot)得到的是一个twist，它的线速度是0，因为qdot固定是1，角速度值等于axis。
[2/4]joint.pose(q)
joint.pose(q)结果是个欧氏变换矩阵，它的位置是<origin.xyz>，角度是绕单位向量axis旋转q度。这个变换是在它关节的parent坐标系。
[3/4]joint.pose(q).M * f_tip.p
f_tip.p是个向量，joint.pose(q).M乘上它的结果是它把变换到joint.pose(q)，只是这次变换时平移是0。疑问：f_tip.p是不是总是0？
[4/4]twist(qdot).RefPoint(joint.pose(q).M * f_tip.p)
Twist Twist::RefPoint(const Vector& v_base_AB) const
// Changes the reference point of the twist.
// The vector v_base_AB is expressed in the same base as the twist
// The vector v_base_AB is a vector from the old point to
// the new point.
// Complexity : 6M+6A
{
return Twist(this->vel+this->rot*v_base_AB,this->rot);
}RefPoint会生成一个新的Twist，相比旧的，角速度不变，线速度加上“this->rot * v_base_AB”，这个“*”是叉乘。
Vector operator *(const Vector & lhs,const Vector& rhs)
// Complexity : 6M+3A
{
Vector tmp;
tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1];
tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2];
tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0];
return tmp;
}3.3 changeRefPoint(jac,total.p-T_tmp.p,jac)
namespace KDL {
bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));src1.columns返回的是chain中非固定关节数，不论此时调用changeRefPoint是哪个关节，它的值都不变，像3。但叉积中若有一个是0向量，那结果肯定是0向量，因此对还未赋过初始值的列(关节)，值不会变，即不会受影响，包括正次正计算的关节i。即changeRefRpoint只影响历史算过[0, i-1]的关节，而且不影响角速度。
return true; } }
3.4 Jacobian::setColumn
用参数t的值，更新到雅可比矩阵索引i列。
namespace KDL {
void Jacobian::setColumn(unsigned int i,const Twist& t){
data.col(i).head<3>()=Eigen::Map(t.vel.data);
data.col(i).tail<3>()=Eigen::Map(t.rot.data);
}
}
四、求公式(3)中的状态增量：
根据公式(3)，求状态增量分两步，第一步求出雅可比矩阵jac，第二步求用雅可比矩阵的逆乘以Err。在第二步时，由于直接求逆较耗资源，使用奇异值分解（SVD）。奇异值分解是在机器学习领域广泛运用的算法，可参考“SVD分解”。
- @q_in[IN]。此时各关节状态。q_in: [0.5, 0.6, -0.7]
- @v_in[IN]。公式(3)中的
。
- @qdot_out[OUT]。求出公式3中状态增量后，存储在这里。
<orocos_kdl>/src/chainiksolvervel_pinv.cpp
------
namespace KDL {
int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
...
//Let the ChainJntToJacSolver calculate the jacobian "jac" for
//the current joint positions "q_in"
error = jnt2jac.JntToJac(q_in,jac);
if (error < E_NOERROR) return error;jnt2jac.JntToJac用于求解雅可比矩阵，矩阵存放在jac。
double sum;
unsigned int i,j;
// Initialize near zero singular value counter
nrZeroSigmas = 0 ;
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
svdResult = svd.calculate(jac,U,S,V,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero();
return (error = E_SVD_FAILED);
}
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
// qdot_out = V*S_pinv*Ut*v_in
//first we calculate Ut*v_in
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
sum+= U[j](i)*v_in(j);
}
//If the singular value is too small (<eps), don't invert it but
//set the inverted singular value to zero (truncated svd)
if ( fabs(S(i))<eps ) {
tmp(i) = 0.0 ;
// Count number of singular values near zero
++nrZeroSigmas ;
}
else {
tmp(i) = sum/S(i) ;
}
}
//tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
//it with V to get qdot_out
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=V[i](j)*tmp(j);
}
//Put the result in qdot_out
qdot_out(i)=sum;
}由于直接求jac的逆较耗资源，使用奇异值分解。上面这段代码执行后，会算出公式(3)中的状态增量qdot_out。为提高效率，代码较难懂，后面eigen_jac_inv_mul_err是用Egien写的同功能代码，调用语句eigen_jac_inv_mul_err(jac, v_in)。
至此算出qdot_out，要没意外，它应该是很小值，像：(5.52382e-05, -5.70437e-05, -4.96106e-07)
// Note if the solution is singular, i.e. if number of near zero
// singular values is greater than the full rank of jac
if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
} else {
return (error = E_NOERROR); // have converged
}
}
}不考虑效率，已知(_jac)和
(_v_in)，计算
。
void eigen_jac_inv_mul_err(const Jacobian& _jac, const Twist& _v_in)
{
Eigen::Matrix<double, 6, 3> jac;
for (int row = 0; row < _jac.rows(); row ++) {
for (int col = 0; col < _jac.columns(); col ++) {
jac(row, col) = _jac(row, col);
}
}
Eigen::Matrix<double, 6, 1> v_in;
v_in(0, 0) = _v_in.vel.x();
v_in(1, 0) = _v_in.vel.y();
v_in(2, 0) = _v_in.vel.z();
v_in(3, 0) = _v_in.rot.x();
v_in(4, 0) = _v_in.rot.y();
v_in(5, 0) = _v_in.rot.z();
const Eigen::Matrix<double, 6, 6> I = Eigen::Matrix<double, 6, 6>::Identity(6, 6);
// 步骤1/2：计算雅可比矩阵的逆。根据jac * jac_inv = I，用QR分解算出jac的逆矩阵jac_inv。
const Eigen::Matrix<double, 3, 6> jac_inv = jac.colPivHouseholderQr().solve(I);
// 步骤2/2：jac_inv乘以Err，算出qdot_out。
const Eigen::Matrix<double, 3, 1> qdot_out = jac_inv * v_in;
// 算出的qdot_out值应该等于ChainIkSolverVel_pinv::CartToJnt中的qdot_out。
// 想用下面语句验证jac_inv是不是jac的逆，但算出的I2不是6x6单位阵，为什么？
const Eigen::Matrix<double, 6, 6> I2 = jac * jac_inv;
}moveit默认运行学求解器插件KDL计算时，调用的不是ChainIkSolverVel_pinv::CartToJnt，而是ChainIkSolverVelMimicSVD::CartToJnt。后者步骤类似ChainIkSolverVel_pinv::CartToJnt，同名变量qdot_out有着同样功能。