tms_rc_katana

導入

ROS groovyでkatanaを利用するための準備

KNI(KatanaNativeInterface)

Neuronics社が開発したKatanaを動かすためのライブラリ 現在この会社、倒産したか合併したかで存在しないらしいのでCDから取得します。 CDの中身はgemini\CITY\nagata\Katanaにあります。

[追記：新しい情報！]

上記フォルダ内にあるKNI_4.2.0で以下は説明していますが、同フォルダ内のKNI_4.3.0でROS化したほうがよいです！ バージョンが古い場合、(おそらく古いバージョン依存の)GCC関連のエラーが発生します。 KNI_4.3.0を利用する場合は、以下のことを行わずともCMakeLists.txtを利用すればcatkin_makeは通ります。 以下のKNI_4.2.0の説明は、ROSを介さずに動かす場合のみ使ってください。情報は覚書として残しておきます。

-------------------------古い情報-------------------------

これをUbuntu12.04で動かします。詳しくは解凍フォルダ「KNI_4.2.0」内のINSTALL.txtを読んでください。 しかし、ここでいくつか問題があります。

1. boost1.4.6にはバグがあり、makeによるコンパイルが通らない。しかも、ROS groovyを入れると、boost1.4.6が強制的に導入されてしまう。
2. GCC4.3以降はstdlib.hやstring.hを明示的にインクルードしなければならないが、デモプログラムではインクルードされていない。
3. makefileの情報が古いため、makeが通らない。

これらを解決すればデモプログラムは動作します。 KNIを利用してROS化も可能なはずです。

とりあえずKNI_4.2.0へcdで移動し、makeコマンドを打ちます。 エラーの内容に1つずつ対処してください。

まずは解決しやすい2.をクリアします。 エラーの特徴は、strncpyやexit等の基本関数が解決できないというものです。

KNI_4.2.0/src内のエラーを出す各cppファイルに以下2つを追記

 #include <stdlib.h>
 #include <string.h>

次に1.のboostライブラリのバグを解決します。 もう一度makeをかけて、エラーが出る場所を確認します。 なお、このバグはboostのバグなのでboost-devのバージョンを最新にできるのであれば 発生しない可能性があります。詳しくは以下を参照。

https://svn.boost.org/trac/boost/ticket/2069

要するに、

 #if !BOOST_PP_IS_ITERATING 
 // .... 
 #elif BOOST_PP_ITERATION_DEPTH() == 1 
 // .... 
 #elif BOOST_PP_ITERATION_DEPTH() == 2 
 // .... 
 #endif 

となっているところを

 #if !BOOST_PP_IS_ITERATING 
 // .... 
 #else 
 # if BOOST_PP_ITERATION_DEPTH() == 1 
 // .... 
 # elif BOOST_PP_ITERATION_DEPTH() == 2 
 // .... 
 # endif 
 #endif 

と変えるだけです。 エラーはKNI_4.2.0/include/boost/mpl内のいくつかのファイルに生じます。 これらの該当箇所をすべて置換してください。

次に、3.を解決します。もう一度makeをかけてください。 これはKNI_4.2.0/py/KNI_wrap.cxxで生じますが、エラー元はKNI_4.2.0/py/Makefileです。

PYTHON_INCLUDE = /usr/include/python2.5

が古すぎて存在していません。 導入したpython-devのバージョンにあわせてください。私の環境では

PYTHON_INCLUDE = /usr/include/python2.7

でコンパイルが通りました。 なお、python自体が導入されていない事でも生じます。 この場合はpython-devを導入してください。

最後に3.のもうひとつの問題を解決します。もう一度makeをかけてください。 pthreadに関するエラーが生じると思います。pthreadのリンクオプション抜けです。(-lpthreadで検索) これもMakefileを変更することで修正します。KNI_4.2.0/demo/Makefileに以下の記述を行います。

 CXX             =      g++
 CXXFLAGS        +=     -g -O2 -Wall
 CXXFLAGS        =      -pthread        ←追加
 LDLIBS          =      -lpthread       ←追加

以下、参照

http://stackoverflow.com/questions/19116248/compiling-from-source-undefined-reference-to-pthread-create

以上でmakeが通ればdemoを動作可能です。

---------------------古い情報ここまで---------------------

なお、katana関連のROSパッケージがすべてgroovy&catkinに対応すれば、それを導入すれば動くはずです。

tips1. Katanaとの接続

PC-Katana間はEthernet接続が楽です。PC側を192.168.168.XXXに固定し、KatanaControlBoxのEthernetAとPCの有線ポートを接続します。 pingで192.168.168.232が返ってくればOKです。

tips2. demoの起動コマンド

[実行ファイル] [Katanaのコンフィグファイル.cfg] [IPaddress(基本は192.168.168.232)]

詳しくはCD内の

Customer_documentation/Katana_450/en/233550-Katana_450-User_manual--v2.0.4.pdf
Customer_documentation/Katana_450/en/233551-Katana_450-Quickstart_Guide-Programming_with_KNI-v0.0.pdf

を参照してください。 なお、デモプログラムのいくつかは実行するだけでよい(.cfgやIPアドレスを指定しなくとも動く)ようにソースを変更しています。 やっていることは、デフォルトではargc,argv[]で指定される.cfgとIPaddressをソース内で先に指定しておくという簡易なものです。

tips3. katanaを動かすサービスについて

Katanaの関節を動かす関数は多数あります(moveRobotTo/moveRobotToEnc/movDegree/etc...) Choreonoidで出てくるのは逆運動学を解いた「関節角」です。(=中間姿勢の配列)

Choreonoidの中間点同士の関係について 中間姿勢1(motor16) → 中間姿勢2(motor16) のmotor1~6に対応するすべての関節は、「動き始めのタイミングと動き終わりのタイミングが一致」しなければ、接触判定の保証ができません。 (すなわち、各関節のうち、回転角度が大きい関節は遅く、回転角度が小さい関節は早く動く必要がある)

デフォルトで利用する場合、先に挙げた複数の関数のうち、「moveRobotTo」のみがこの条件を満たします(movDegree等を速度・時間計算して制御する場合を除く)。 moveRobotToは、Katanaの関節角ではなく、Katanaの姿勢を引数に与えるものであるため、 「Choreonoidの出力(関節角度)→katanaのmotorのEncoder値→getCoordinatesFromEncoders(運動学を解く)→moveRobotTo(逆運動学を解く)」 という変換を行なっています。

この時、katanaは肘を内側に曲げる、外側に曲げるの2通りの姿勢をとれるため、逆運動学を解いた時点で解がChoreonoidのものと一致するとは限りません。 逆運動学を解く関数内部をChoreonoidと一致させれば、この問題は解決します。

tips4. コンフィギュレーション・ファイル

KNI_4.3.0/configfiles450/katana6M180_G.cfg

が、katanaのコンフィグファイルになります。これは、様々なkatanaの幾何・制御情報を定義しているファイルです。 この中の

[GENERAL]
...
kinematics   = "Analytical"; # kinematics implementation. Analytical or RobAnaGuess

ここはAnalyticalにしておいてください。 RobAnaGuessにすると、正確に逆運動学を解こうとしすぎるために解が返ってこない場合があります。

run tms_rc_katana

rosrun tms_rc_katana roskatana_test1

作成済みサービス一覧

• katana_move_angle_array [/katana_move_angle_array(katana_pos_array.srv)] katanaの関節角の配列(6*移動回数分)を与え、順次動作させる 角関節の動作開始と動作終了は一致する→計画通り動作できる(はず)

:smiley_smile: 単位はdegree

ros_katana/katana_pos[] pose_array   katanaの関節角の配列
---
uint32 success                       失敗成功
string message                       LOGメッセージ(任意)

katana_pos.msg
float32[] pose                      katanaの各関節角

• katana_move_pose_array [/katana_move_pose_array(katana_pos_array.srv)] katanaの姿勢の配列(6*移動回数分)を与え、順次動作させる 角関節の動作開始と動作終了は一致しない

ros_katana/katana_pos[] pose_array   katanaの姿勢の配列
---
uint32 success                       失敗成功
string message                       LOGメッセージ(任意)

katana_pos.msg
float32[] pose                      katanaの姿勢(x,y,z,theta,phi,psi)

• katana_move_enc [/katana_move_enc(katana_pos_array.srv)] katanaの関節角のencoder値の配列(6*移動回数分)を与え、順次動作させる 角関節の動作開始と動作終了は一致しない

ros_katana/katana_pos[] pose_array   katanaの関節角のencoder値の配列
---
uint32 success                       失敗成功
string message                       LOGメッセージ(任意)

katana_pos.msg
float32[] pose                      katanaの各関節角のencoder値

• katana_move_motor_angle [/katana_move_motor_angle(katana_pos_array.srv)] katanaの関節角の配列(6*移動回数分)を与え、順次動作させる 角関節の動作開始と動作終了は一致しない

ros_katana/katana_pos[] pose_array   katanaの関節角の配列
---
uint32 success                       失敗成功
string message                       LOGメッセージ(任意)

katana_pos.msg
float32[] pose                      katanaの各関節角

• katana_gripper_action [/katana_gripper_action(gripper_action.srv)] katanaの関節角の配列(6*移動回数分)を与え、順次動作させる 角関節の動作開始と動作終了は一致しない

uint32 gripper  #1=open gripper 2=close gripper 3=close gripper for grasp(very slowly)
---
uint32 success