ikfastによる逆運動学（その１）
初稿: 2013/10/14, 最終更新: 2013/10/22
１６．ikfastのc++ソースコードを自分のプログラムから呼ぶための準備
１６．１ ikfastのダウンロード
まず、OpenRaveのWEBページのロボットライブラリの中からHIRONXを選び、公開してくださっている各部のIKのソースコードをダウンロードしてきます。VisualStudioのソリューションの中に、新しいフォルダkawada-hironxを作りその中にソースコードをコピーします。
- ikfast60.Lookat3D.1_2.cpp
- ikfast60.Lookat3D.1_2_f0.cpp
- ikfast60.Transform6D.3_4_5_6_7_8.cpp
- ikfast60.Transform6D.3_4_5_6_7_8_f0.cpp
- ikfast60.Transform6D.13_14_15_16_17_18.cpp
- ikfast60.Transform6D.13_14_15_16_17_18_f0.cpp
- ikfast60.TranslationDirection5D.3_4_5_6_7_f8.cpp
- ikfast60.TranslationDirection5D.13_14_15_16_17_f18.cpp
これに加えて、OpenRave本体に添付されているikfast.hというファイルが必要になりますので、ソースコードをダウンロードして、share/openrave-X.Y/python/ikfast.hのパスにあるikfast.hをkawada-hironxフォルダの中にコピーします。*1OpenRaveのソースコードのダウンロードが上手くいかなければ、下記GitHubから直接ダウンロードしてください。
https://github.com/rdiankov/openrave/blob/tags/0.8.0/python/ikfast.h
ここで、C++ソースコードを作成したときのIKFastのバージョンと同じikfast.hが必要になる点に注意してください。.cppのソースコードを見ると、
#define IKFAST_VERSION 60
とあり、これはOpenRave0.8.0のときの物のようです。
１６．２ Ｌａｐａｃｋのインストール
OpenRaveは線型計算のための数値解析ソフトウェアライブラリLapackを使っています。具体的には、lapack, blas, f2cのlibをリンクしてやる必要があります。正攻法でインストールしたい場合は、本家のWEBサイトからソースコードをダウンロードしてきてREADMEを見ながらビルドしてください。
http://icl.cs.utk.edu/lapack-for-windows/
WindowsでLapackのビルドをするのはとても大変なので、下記リンクのWEBで公開されているコンパイル済みのライブラリClapackWin.libで代用することもできます。
http://www.chino-js.com/ja/tech/clapack/index.html
プロジェクトのプロパティか、ソースコード中で、ライブラリをリンクする必要があります。VisualStudioでは、#pragma構文を使って次のようにライブラリのリンクを指定できます。
#pragma comment(lib, "C:\\workspace\\of_v0.8.0_vs_release\\examples\\addons\\assimpExample\\bin\\ClapackWin.lib")
#ifdef~#endifでDebug用ライブラリとRelese用ライブラリを分けておくと良いでしょう。*2
１６．３ ikfastの名前空間の修正
ikfastで自動生成されたc++のソースコードでは、すべて同じ名前空間の中で同じ関数名が与えられています。できるだけ基本的なC言語の使い方に倣いたいので、ソースコードを一部書き換えて関数名がかぶらないように修正します。全てのikfast60～.cppの中を一部編集して、名前空間を変更します。名前空間を次のように決めます。
- namespace IKFAST_LOOKAT3D
- namespace IKFAST_LOOKAT3DF0
- namespace IKFAST_LARM6D
- namespace IKFAST_RARM6DF0
- namespace IKFAST_RARM6D
- namespace IKFAST_RARM6DF0
- namespace IKFAST_LARM5DF18
- namespace IKFAST_RARM5DF8
また自分のプログラムから呼び出しやすいように、呼び出し用の関数を定義します。
main関数を参考にして、引数を入出力パラメータに変更したik_solveという関数を作り、namespace内で定義します。
#ifdef IKFAST_NAMESPACE
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint)
{
IkSolutionList<IkReal> solutions;
std::vector<IkReal> vfree(GetNumFreeParameters());
for(std::size_t i = 0; i < vfree.size(); ++i)
vfree[i] = free_joint[i];
bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
if( !bSuccess ) {
return -1;
}
sol_joint.resize(solutions.GetNumSolutions());
std::vector<IkReal> solvalues(GetNumJoints());
for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
std::vector<IkReal> vsolfree(sol.GetFree().size());
sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
sol_joint[i].resize(solvalues.size());
for( std::size_t j = 0; j < solvalues.size(); ++j)
sol_joint[i][j] = solvalues[j];
}
return 0;
}
} // end namespace
#endif
ikfast.hでメイン関数を使わないようにIKFAST_NO_MAINを定義します。また名前空間を使うようにIKFAST_NAMESPCAEを定義します。
#define IKFAST_NO_MAIN
#define IKFAST_NAMESPACE
ikfast内で関数名の定義がされていてこれが競合を起こしてしまうので、関数の宣言を外します。
#if 0
IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
~~~~
IKFAST_API const char* GetKinematicsHash();
#endif
便利に使えるようにik_solveの宣言を列挙したikfast-hironx.hを新規作成します。
#ifndef _IKFAST_HIRONX_H
#define _IKFAST_HIRONX_H
#include "ikfast.h"
typedef double IkReal;
namespace IKFAST_LOOKAT3D{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_LOOKAT3DF0{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_RIGHT6D{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_RIGHT6DF0{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_LEFT6D{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_LEFT6DF0{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_RIGHT5DF8{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
namespace IKFAST_LEFT5DF18{
int ik_solve(IkReal eerot[9], IkReal eetrans[3], std::vector<std::vector<IkReal> >& sol_joint, std::vector<IkReal> free_joint);
}
#endif // _IKFAST_HIRONX_H
