てけノート

on the foot of giants

[ROS-2] kinectのデータでPR2にjojo立ちさせてみた

   


この記事は学生エンジニア Advent Calendar20日目の記事です。

やったこと

こんな感じで、自分がやったのと同じような感じでPR2がjojo立ち?してくれるROSパッケージを作りました。

こまかいこと

ROSとMoveIt+PR2を使ってます。環境構築編
まず、kinectの前でjojo立ちとかして動作を計測します。

jojoextract

肩、肘、手のpositionをcsvとかで出力します。csvで出力する部分はkinect sdkのサンプルをちょっといじっただけなので省略。

これで動作の軌跡が取れました。
で、得られたのは作業空間軌道なので、ここから関節空間軌道にするために関節角を計算して、PR2の座標系に移してやって、moveitのserviceに合うように変換して、serviceをcallすればOKです。

腕の位置情報 -> 関節角 に変換。計算した関節角とPR2の関節座標系が合わないのでうまく変換します。
PR2の座標系はこんな感じ。

DSC_0138

あとは関節位置から関節速度、関節加速度を微分で出してやって、動作軌道を変換し、軌道を可視化するためにdisplay_planned_pathを使って軌道をpublishしてあげます。
この状態だと軌道計画がどんな感じか見れるだけなので、実際にその軌道を実行するためexecute_kinematic_pathというserviceに対して軌道を投げてあげれば実行してくれます。
シミュレータ上だと実行してる感が出ないので、もし貴方が実ロボットをお持ちで壊れたりしても大丈夫であればぜひお試しください。

コード

github
いらないヘッダー読み込んでたりコードが底辺レベルだったりはご容赦ください。もともと底辺ですが、さらに進捗のやばさに影響されています。
腕の3自由度だけ使ってます。前腕の回旋が間に合わなかった。

githubのパッケージをcloneしてcatkin_makeしてやって、PR2のdemo.launchを立ち上げてからrosrunで走らせればOKです。
demo.launchで立ちあがったRviz側で
・Query Start State
・Query Goal State
・RobotState
のチェックを外しておくと動作が見やすくなります。

精度低い問題

指揮したつもりが「今でしょ」を繰り返してるっぽくなった。そもそも指揮が下手か

ボクシングさせたら腕がどっかいった。座標変換ミス、kinectに正対したのでうまく取得できなかった?っぽい。

smoothingとかフィルタ処理必須。

 - ROS, プログラミング, 研究