機械臂的逆動力學問題可以認為是：已知機械臂各個連桿的關節的運動（關節位移、關節速度和關節加速度），求產生這個加速度響應所需要的力/力矩。KDL提供了兩個求解逆動力學的求解器，其中一個是牛頓尤拉法，這個方法是最簡單和高效的方法。
   牛頓尤拉法演算法可以分為三個步驟： step1：計算每個連桿質心的速度和加速度； step2：計算產生這些加速度所需要的合力； step3：計算其它連桿通過關節對每個連桿施加的力。
  KDL中的牛頓尤拉法的程式碼是基於文獻《Rigid Body Dynamics Algorithms》寫的，這本書中使用了spatial vector的概念，spatial vector將3維的剛體的線性運動（力）和3維的旋轉運動（力）組合到一起形成6維的，這樣處理會使程式碼便於閱讀（關於Spatial Vector可以看另外一篇部落格-

圖1 牛頓尤拉法的虛擬碼

        int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
    {
        if
 
(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
            return (error = E_SIZE_MISMATCH);
        unsigned int j=0;

        for(unsigned int i=0;i<ns;i++){
            double q_,qdot_,qdotdot_;
            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
                q_=q(j);
                qdot_=q_dot(j);
                qdotdot_=q_dotdot(j);
                j++;
            }else
                q_=qdot_=qdotdot_=0.0;

            X[i]=chain.getSegment(i).pose(q_);
            Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
            S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));

            if(i==0){
                v[i]=vj;
                a[i]=X[i].Inverse(ag)+S[i]*qdotdot_+v[i]*vj;
            }else{
                v[i]=X[i].Inverse(v[i-1])+vj;
                a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
            }

            RigidBodyInertia Ii=chain.getSegment(i).getInertia();
            f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];

        }

        j=nj-1;
        for(int i=ns-1;i>=0;i--){
            if(chain.getSegment(i).getJoint().getType()!=Joint::None)
                torques(j--)=dot(S[i],f[i]);
            if(i!=0)
                f[i-1]=f[i-1]+X[i]*f[i];
        }
    return (error = E_NOERROR);
    }

在閱讀程式碼的時候，大家比較關心的可能是程式碼段與公式的對應關係，由於KDL的程式碼非常簡短(原因是使用了Spatial Vector)，所以這裡把關鍵程式碼與文獻中的公式對應起來，便於閱讀程式碼）：

X[i]=chain.getSegment(i).pose(q_);

  求解轉換矩陣${}^{\lambda \left( i \right)}{{X}_{i}}$，表示父連桿座標系向子連桿座標系的座標變換。

vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));

   求解關節引起的連桿速度${v}_{J}$，它的意義是求出連桿的速度後，再左乘一個轉置矩陣，將速度的參考座標系變換到本體上的座標系，這與上圖中的虛擬碼的公式（${{v}_{Ji}}={{s}_{i}}{{\overset{\centerdot }{\mathop{q}}\,}_{i}}$）不一樣。

v[i]=X[i].Inverse(v[i-1])+vj;

  求解連桿末端的速度（${{v}_{i}}={}^{i}{{X}_{\lambda (i)}}{{v}_{\lambda (i)}}+{{v}_{J}}$），它的參考座標系為與本體固連的座標系。

a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;

   這行程式碼是求解連桿的加速度，${{a}_{i}}={}^{i}{{X}_{\lambda \left( i \right)}}{{a}_{\lambda \left( i \right)}}+{{S}_{i}}{{\overset{\centerdot \centerdot }{\mathop{q}}\,}_{i}}+{{c}_{Ji}}+{{v}_{i}}\times {{v}_{Ji}}$，${{c}_{Ji}}=0$。

            RigidBodyInertia Ii=chain.getSegment(i).getInertia();
            f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];

  獲取機械臂的動力學引數（質量、慣性張量、連桿座標系到連桿質心偏移）、$f[i]$求解父連桿通過關節施加給連桿的力。程式碼中的運算子 * 是KDL中自定義的運算子，可見另一篇部落格-KDL的精髓。

torques(j--)=dot(S[i],f[i]);

  這行程式碼求解關節力或力矩（${{\tau }_{i}}={{S}_{i}}^{T}{{f}_{i}}$），至此，各個關節的力或力矩均會被求出，演算法結束。
參考文獻：
[1] 《Rigid Body Dynamics Algorithms》. Roy Featherstone, 2008