KDLを使ってみました
後で追加
KDLを使ってマニピュレータのヤコビ行列を取得する際の注意点
KDL::Chainがある状態で以下の関数を呼び出すとヤコビ行列が取得できます.(便利)
KDL::ChainJntToJacSolver::JntToJac(KDL::JntArray,KDL::Jacobian&);
ヤコビ行列を受け取るクラスKDL::Jacobianを初期化しておかないと結果が入らないっぽい? 以下のように4行目のJacobianのコンストラクタにジョイントの数を渡すとJntToJacで受け取れました.
KDL::Chain chain;
KDL::JntArray q;
//...
KDL::Jacobian J(chain.getNrOfJoints());
KDL::ChainJntToJacSolver jsolver(chain);
jsolver.JntToJac(q, J);