search

jsk-ros-pkg / jsk_robot

jsk-ros-pkg/jsk_robot
https://github.com/jsk-ros-pkg/jsk_robot
73 stars 97 forks source link

[naoqieus] :move-to メソッドが実機に対して使えません #646

Closed kochigami closed 8 years ago

kochigami commented 8 years ago

pepperを特定の座標に動かしたいのですが、 *pepper*モデルでは :move-toメソッドが使えますが、 実機 *ri*で動かず困っています。

実機に特定の座標系の移動をさせる時に何か他の方法がありますでしょうか。 もしくは、実機で:move-toメソッドが動くようにしないといけないのでしょうか。

naoqi-interface.lの中で (load "package://pr2eus/robot-interface.l")を読んでおり、この中に:move-toがありました。 pr2-interface.lも見てみたのですが、naoqi-interface.lと同じようになっているようです。 pr2では*ri*で:move-toが使えるようです。

pepper-interface.lはnaoqi-interface.lを読んでいるので、きちんと継承されているかを確認してみます。

(setq co (make-coords))
#<coordinates #Xd1c6c28  0.0 0.0 0.0 / 0.0 0.0 0.0>
(send co :translate (float-vector -1150.0 6300.0 0.0))
#<coordinates #Xd1c6c28  -1150.0 6300.0 0.0 / 0.0 0.0 0.0>
(send co :rotate -3.142 :z)
#<coordinates #Xd1c6c28  -1150.0 6300.0 0.0 / 3.141 0.0 0.0>

(send *pepper* :move-to (send co :worldcoords))
#<pepper-robot #X76c4130 pepper  -1147.434 6300.468 0.0 / 3.141 0.0 0.0>
(send *pepper* :worldcoords)
#<coordinates #X76c6c68  -1147.434 6300.468 0.0 / 3.141 0.0 0.0>

(send *ri* :move-to (send *pepper* :worldcoords))
=> error: 
/opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :move-to in (send *ri* :move-to (send *pepper* :worldcoords))
kochigami commented 8 years ago

645 で書かせていただいたように、理解があっていれば

robot-move-base-interface に対して :move-to が使える。 robot-interface に対して :move-to のようなメソッドがないかを調べる。

k-okada commented 8 years ago

https://github.com/jsk-ros-pkg/jsk_robot/pull/645#issuecomment-239623112 に書きましたが，pepperはrobot-move-base-interface は（今の所）使わなくていいので pepper_bringupを使うとMoevToというトピックがあってつかえるとおもいますが，これを呼び出すような eus側の:move-toをnaoqi-interfaceについかしたらいいとおもいます．

◉ Kei Okada

On Sat, Aug 13, 2016 at 12:54 PM, Kanae Kochigami notifications@github.com wrote:

645 https://github.com/jsk-ros-pkg/jsk_robot/pull/645

で書かせていただいたように、理解があっていれば

robot-move-base-interface に対して :move-to が使える。 robot-interface に対して :move-to のようなメソッドがないかを調べる。

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/646#issuecomment-239600251, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3CGcQxyhDhs2gbTj0NqwYbrKYJMHks5qfT_wgaJpZM4Ji_IX .

kochigami commented 8 years ago

ご教示ありがとうございました。

:move-to #(ゴールの座標) １．移動前のロボットの現在の座標を得る：（(send *ri* :state :worldcoords) が使えない代わりに、 /world /base_link 間のtfを取ってくる）

２．ゴールの座標と引き算する

３．go-posで相対距離分進む：（関数 moveToは相対距離分進むので、最初の座標とゴールの座標の相対距離分進むようにする）

というようにしました。

ALMotionでgetRobotPosition というメソッドがあったけれど、pepperが起動したところがworld座標の原点となっているので今回は使わない

今後直すところ tfが古くて実行するたびにpepperの位置がrvizの地図上でずれていく ロボットの回転を入れる（今はカニ移動のみ）

大体の位置まで部屋の前を動くようになったらプルリクエストを送って、駄目そうだったら諦めます。

k-okada commented 8 years ago

getRobotPositionで起動したところを原点にしたworld座標が帰ってくるのはこれでOKで，pr2もそうなっています． (http://www.hessmer.org/blog/wp-content/uploads/2015/06/tf_frames.png みたいな感じで，/odom -> /base_link がそれに該当） で，それとは別に自己位置同定プログラムをつかって，自分で決めた原点とロボットの初期値の間の相対位置 (/map -> /odom )を きめたらよくて，それは例えばヨセフ君のプログラムがつかえるはず，最初は装置がおおきくなるけど，それでPepperがブサイクに なるのを見ると，小さいカメラで，精度はおとしてもそここそ自己位置同定できると，嬉しい，という用途があると彼もきづくだろうから， やってみるのが大切．その装置一式は @mmurooka 君がもっているので，ひと通り相対位置移動ができたら，これをつかって絶対位置移動を ためしてみるのがいいですね．ちなみに，全体は /world -> /map -> /odom -> /base_link みたいになっているはず．．．．で，/world -> /map はJSK使用

なので，作戦としては毎回電源をいれる位置を決めておいて，そこから自分の位置まではgetRobotPosition ( -> :state :worldcoords )で取ってこれるようにして， 現在位置としてはまずはこれを信じて，:move-to をこの位置と，現在位置との相対から :go-pos を発行する，というのがいいとおもいます．

で，どれぐらい信じられるかどうかを知りたい所，あまりにも信じられなければ，ヨセフ君のシステムをつかってみる，というのにいってみましょう．

◉ Kei Okada

On Mon, Aug 15, 2016 at 2:36 PM, Kanae Kochigami notifications@github.com wrote:

Closed #646 https://github.com/jsk-ros-pkg/jsk_robot/issues/646.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/646#event-755299863, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3HWFDRDebAn-gTLabGLBxq4xJHpyks5qf_rGgaJpZM4Ji_IX .

kochigami commented 8 years ago

ご教示ありがとうございました。 今は障害物検知で止まってgo-posが終了してしまうので、そこを直してから（明日やる） 安定して、隣の部屋の前まで相対移動で往復できるか確認します。

できたら 往復を繰り返してずれるか確認する 相対移動で遠くに行ってみる を試してみます。

メモ （地図なし） １．:move-to による相対移動 (やったこと1： jsk_maps以下のモデル　＋　スタート地点のスポットにロボットを置いてsend *ri* :go-pos スポット間の距離 スポット間の距離があればよいので、問題なく動く)

やったこと2: スタート地点のスポットらしきところにロボットを置く。(モデルとロボットの座標系の向きを揃える。) NAOqi API getRobotPosition で得られる位置とモデルで定義されている座標を比べる。 モデル上でのロボットの位置が大体分かる。　(/map -> /odom -> /base_link)

（地図あり） まだよく分かっていない。 ありがたみを理解して、まずamclの仕組みを勉強する（今は動いても、きちんとした出力にはなっていない）