2010年1月31日日曜日

オリジナルロボットをシミュレータで動かすための作戦

オリジナルロボットをシミュレータに作り、ROSプログラムで操作するにはどうしたらいいのでしょうか?


http://www.ros.org/wiki/pr2_simulator/Overview
を見るて、よくよくドキュメントを追っていくと、少しわかりました。
まずはこれまでにやったURDFファイルにシミュレーション用の情報を付加して、
gazeboで表示、計算できるようにします。
そして次に、そのロボットを自在に動かすためのコントローラーを作り、ROSノードと接続します。



1)シミュレーションモデルの作成

シミュレータ上にロボットを表示するためには、前回作ったURDFに、
衝突用の形状、表示用の形状、重さ、などの物理パラメータを追加する必要があります。

ロボットモデル定義とプラグイン(OpenHRPでいうところのコントローラー)の指定。
形状はSTL形式で与えるようです。

URDFのフォーマットはここに少し書いてありますが、これでは何もわかりません。

urdfで定義したオブジェクトをシミュレータ世界に出現させるチュートリアルがありますので、次回はこれをやります。

その次に、pr2_descriptionを見ながら1関節くらいの簡単なロボットを作ってみようと思います。




2)コントローラーの作成

ここで言うコントローラーとは、シミュレータの中のロボットを直接操作するプログラムのことです。

PR2の場合、
の、
gazebo_ros_controller_managerというpluginでシミュレータ上のPR2の操作、状態取得を行っているように見えます。

PR2とgazeboの接続はどうやら以下のようになっているようです。

gazebo <==> gazebo_ros_controller_manager <==> pr2_controller_manager

pr2_controller_managerから先は実機と完全互換のようです。
gazebo_ros_controller_managerがシミュレータと実機をつないでいます。
非常にきれいにつながっていますね。

しかし、これはPR2の場合です。
オリジナルロボットの場合はどうするのがよいでしょうか?

以前作ったi-SOBOTの場合、
iSOBOT_nodeと同じインタフェースを持つgazebo_ros_iSOBOTプラグインを
作るのが良さそうです。

gazebo <==> gazebo_ros_iSOBOT <==> teleop_iSOBOT_joy

こうすればPS3のジョイスティックで動かしたデモがシミュレータ上でも、
teleop_iSOBOT_joyをいっさい変更せずに実行することができるはずです。

でも、iSOBOT_nodeでは、以下のフレームワークを使ったほうがよかったかもしれません。

http://www.ros.org/wiki/robot_mechanism_controllers

手っ取り早く動かしたかったため、たんなるトピックにしましたが。
まあ、ここまで複雑なものは必要ないので、あれはあれで正しいかもしれませんね。


2010年1月30日土曜日

rosemacsを使う

rosemacsというパッケージをたまたま見つけたので紹介します。

名前でわかるようにemacsの中でrosを開発するためのemacsライブラリです。

http://www.ros.org/wiki/rosemacs

標準パッケージに入っていますが、experimentalみたいです。
emacsの中だけで、簡単にファイルを開いたり、rosrunしたり、topicの中を見たりできます。

使い方は以下を.emacsなどに追加です。

(add-to-list 'load-path "/path/to/rosemacs")
(require 'rosemacs)
(invoke-rosemacs)

/path/to/rosemacsは普通は、~/ros/ros/tools/rosemacsになりますか?
$ rospack find rosemacs
すればわかりますね。

そしてさらに以下も追加しておきましょう。
  • (global-set-key "\C-x\C-r" ros-keymap)
    • ;Activate keyboard shortcuts for the rosemacs commands.
    (set-ros-topic-update-interval 5)
    • ;Initiate background tracking of the set of active ROS topics.
C-x C-r がrosemacsのプレフィックスになります。
C-x C-r C-hとするとショートカット一覧がでます。



例えば、
C-x C-r C-c でros-coreを走らせて、
C-x C-r r  して、プログラムを走らせて、

C-x C-r C-t   (display-ros-topic-info)すると、以下のように
トピック一覧がでます。
rostopic list
と同じ感じですね。



この状態で、見たいトピック(/chatter)でリターンを押すと、


というようにトピックの中が流れるようすが見れます。
rostopic echoと同じですね。


これは非常に面白いパッケージですね。
もっと早く知りたかったです。

2010年1月28日木曜日

URDFファイルをパースしてプログラムから読み込む

前回作ったURDFファイルをプログラムからパースします。

http://www.ros.org/wiki/urdf/Tutorials/Parse%20a%20urdf%20file

では、さっそくいつものように練習用パッケージを作ります。

$ roscd sandbox
 $ roscreate-pkg learning_urdf urdf
 $ roscd learning_urdf
 $ rosmake

また、前回作ったurdfファイルをカレントディレクトリに持ってきます。

そして以下をsrc/parser.cppとして保存しましょう。


#include <urdf/model.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_parser");
  if (argc != 2){
    ROS_ERROR("Need a urdf file as argument");
    return -1;
  }
  std::string urdf_file = argv[1];

  urdf::Model model;
  if (!model.initFile(urdf_file)){
    ROS_ERROR("Failed to parse urdf file");
    return -1;
  }
  ROS_INFO("Successfully parsed urdf file");
  return 0;
}


意味のある中身は以下の2行です。

urdf::Model model;
  if (!model.initFile(urdf_file)){


urdf::Model型のオブジェクトを、initFIle()メソッドでファイルから読み出して初期化しています。
成功するとtrueがかえります。

$ bin/parser my_urdf.xml
[ INFO] 1264608023.141038000: Successfully parsed urdf file
となり成功しました。

urdf::Modelクラスは以下のメソッドでリンクやジョイントにアクセスできます。



boost::shared_ptr< const Joint > getChildJoint (const std::string &name) const
 get child Joint of a Link given name 
boost::shared_ptr< const Link > getChildLink (const std::string &name) const
 get child Link of a Link given name 
boost::shared_ptr< const Joint > getJoint (const std::string &name) const
boost::shared_ptr< const Link > getLink (const std::string &name) const
void getLinks (std::vector< boost::shared_ptr< Link > > &links) const
const std::string & getName () const
boost::shared_ptr< const Joint > getParentJoint (const std::string &name) const
 get parent Joint of a Link given name 
boost::shared_ptr< const Link > getParentLink (const std::string &name) const
 get parent Link of a Link given name 
boost::shared_ptr< const Link > getRoot (void) const
bool initFile (const std::string &filename)
 Load Model given a filename. 
bool initString (const std::string &xmlstring)
 Load Model from a XML-string. 
bool initXml (TiXmlDocument *xml)
 Load Model from TiXMLDocument. 
bool initXml (TiXmlElement *xml)
 Load Model from TiXMLElement. 
 Model ()



これでロボティクスっぽいことができそうですね。

URDFファイルを作る

URDFはUnified Robot Description Formatの略で、
XMLフォーマットのロボットモデルの記述です。
ROSオリジナルでしょうか?

ROSのurdfパッケージではこれをC++から読み込むことができます。

概要
http://www.ros.org/wiki/urdf

詳細
http://www.ros.org/wiki/urdf/XML

チュートリアル
http://www.ros.org/wiki/urdf/Tutorials/Create%20your%20own%20urdf%20file

そしてURDFから運動学用のモデルKDL (Kinematics and Dynamics Library) treeを作るようです。
http://www.ros.org/wiki/kdl_parser
URDFは運動学のモデルよりは広い概念のようです。

また、このファイルにシミュレータのデータやセンサ取り付け位置なども統合されています。


ではチュートリアルからやりましょう。

以下のような構造のロボットがいたとします。


するとURDFでは以下のようなXMLで記述します。
これをmy_urdf.xmlとして保存しましょう。

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="world"/>
    <child link="link1"/>
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
  </joint>

  <joint name="joint4" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
  </joint>
</robot>



world -> link1にjoint1という仮想的なジョイントを作っています。
それ以外は普通にジョイントがリンク(剛体)2つをつなぐ構造で、
図とマッチしていることがわかります。


ではこのmy_urdf.xmlをツールを使ってパースできるかチェックしてみましょう。


$ rosmake urdf
  $ rosrun urdf check_urdf my_urdf.xml

すべて正しければ以下のように表示されます。


robot name is: test_robot
  ---------- Successfully Parsed XML ---------------
  root Link: world has 1 children
      child(1):  link1
          child(1):  link2
          child(2):  link3
              child(1):  link4


次にこれに空間を加えていきます。
図をみるとリンクの根元に緑色の矢印(参照フレーム)がついています。そしてそのフレームはジョイントと一致しています。
なので、このツリー構造の空間はリンクの参照フレームから子供のジョイントまでのオフセットを与えることで記述できます。
<origin>を使って、位置のオフセットをxyz、姿勢のオフセットをrpyで表すと以下のように書くことができます。
<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="world"/>
    <child link="link1"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 2 0" rpy="0 0 3.14" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="5 0.5 0" rpy="0 0 0" />
  </joint>

  <joint name="joint4" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
  </joint>
</robot>

ではもう一度チェックしてみましょう。
$ rosrun urdf check_urdf my_urdf.xml


結果は同じですね。では次にいきましょう。

次はジョイントの回転を決めます。
これでロボットの運動学モデルが完成です。<axis>を使ってジョイントの記述をします。
ローカルフレームでの回転軸を指定します。
ローカルとは子リンクの参照フレームの座標系における値となりますので、
joint1では、
<axis xyz="0 1 0" />

となります。
すると全部はだいたいこんな感じになります。
<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="world"/>
    <child link="link1"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
    <axis xyz="0 1 0" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 2 0" rpy="0 0 3.14" />
    <axis xyz="0.707 0.707 0" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="5 0.5 0" rpy="0 0 0" />
    <axis xyz="0.9 0.15 0" />
  </joint>

  <joint name="joint4" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
    <axis xyz="0.707 0.707 0" />
  </joint>
</robot>

ではチェックして、
$ rosrun urdf check_urdf my_urdf.xml

これで可視化しましょう。
$ rosrun urdf urdf_to_graphiz my_urdf.xml


こんなんでました。

では次にこれをプログラムから読み込みましょう。




2010年1月27日水曜日

今後の進め方 4

このBlogの個人的な目標は

・PR2の動かし方を理解すること
・オリジナルロボットをROSで動かすこと
・オリジナルロボットをシミュレータ上で動かすこと
・C++を勉強すること

でした。

PR2の動かし方の基本的な部分は大体分かりました。
あとは上位の動作計画系と、実時間制御の部分が残っています。

オリジナルロボット(改造i-SOBOT)をROSで動かすまではとりあえずできたことにして、
当面、オリジナルロボット(改造i-SOBOT)をシミュレータ上で動かすことを目標にしてやっていきたいと思います。

ROSではロボットモデル(関節構造は)urdfという形式で扱っています。
そして、これに表示用メッシュを足してgazebo用モデルにしていると思います。
また、このurdfモデルを使って逆運動学を解いています。

なので、urdf、gazeboを見て、PR2の逆運動学を見て、その勢いでPR2の上位系をさらっと
やりたいと思います。

もちろん途中でi-SOBOTっぽいモデルをgazebo上に表示したいと思います。
C++は少しずつ本を読んで勉強中です。

2010年1月24日日曜日

i-SOBOTをPS3のコントローラーで動かしてみる(動画)

前回の続きです。


とりあえず動画を公開しておきます。



これってたぶん、ROSで動いた世界で最小のヒューマノイドロボットじゃないですかね?

世界で最小のPS3で動いたヒューマノイドロボットでもありますね、多分。


詳細は興味のある人がもしいれば解説したいと思います。


ぶっちゃけ、PS3コントローラーとつなぐだけならROS使わなくてもできると思いますが、
例えば、画像処理をさらにこれに追加する、とか、複雑になってくるとさらに真価を発揮するような気がします。

i-SOBOTをROSノードを使ってPS3コントローラーから動かす(設計)

ROS1.0でましたね。

これでドキュメントも落ち着いてかけます。
この記録でも最初のほうはすでに現状とマッチしない部分がありますから、
バージョンが落ち着くのはありがたいです。

というのは置いておいて、i-SOBOTをROSで動かしましょう。

i-SOBOTはそもそもPCから自由に動かすことはできないのですが、
私は自作でコントローラーを作ったので、関節角度を自由に指定して動かすことができます。
そちらに興味がある人はここに少しヒントを書きましたので参照ください。

1. ノードの設計

i-SOBOTは関節角度を指定して動かすことができて、C++のライブラリとして実現されています。
これを使ってi-SOBOTノードを作ってROSとつなげます。
いったんROSとつないでしまえば、これまでに勉強したライブラリが使い放題になります。
例として前回勉強したPS3のコントローラーとつないでみます。

今回作るのは下の図にある、
iSOBOT_node・・・iSOBOTを動かすためのノード
teleop_iSOBOT_joy・・・iSOBOTとjoyノードをつなぐためのノード
の2つです。

ポイントは
・iSOBOT_nodeのインタフェースを何にするか?
です。

ROSのインタフェースはトピックとサービスの2つに別れ、サービスの場合はさらに、actionlibを使った非同期通信にするかどうかを考えなければいけません。
今回はi-SOBOTの関節を動かすだけの単純な操作だけなので、トピックで関節目標角度を受け取ることにしました。

2.トピックの設計

そしてトピックにするとしても次はどんなメッセージ型にするか、ということを考えます。
PR2がジョイントを動かす・角度を公開するときにどういう型だったかを思い出します。
すると、
sensor_msgs/JointState
という型を使っていましたね。

そこでこのメッセージをそのまま使うことにしました。

3.ノード間アーキテクチャ設計

ということで、大体の概要の設計が終わりました。
トピックの名前(チャネル)を決めて、以下のように設計しました。
楕円がノードで、四角がトピックです。




4.実装

ここまでできてから実装を開始します。
今までやってきたことを思い出しながら、
willowのサンプルを真似しながら実装すればそれほど難しくはありませんでした。

5.ロボットを動かす

次回動画で紹介しますね。


だんだん、ROS勉強記録というより、ROSを楽しむ記録になりつつありますが、
こういうのが一番早く勉強する方法だと思います。