読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

ikfastによる逆運動学(その1)

初稿: 2013/10/14, 最終更新: 2013/10/22

16.ikfastのc++ソースコードを自分のプログラムから呼ぶための準備

16.1 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のときの物のようです。 

 16.2 Lapackのインストール

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

 16.3 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

*1:OpenRaveのWindows版インストーラは現在Boostの問題で正しく動作しません。

*2:名前の齟齬があるのか、Debug用ライブラリでは動かないようです。確認中。