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

[peppereus] (send *ri* :wait-interpolation)なしで :head :look-at メソッドで得られた角度列に近いところまでロボットを動かしたいです #638

Closed kochigami closed 8 years ago

kochigami commented 8 years ago

while文の中で :go-velocity メソッドでペッパーを進ませながら:head :look-atメソッドで人の顔を追いかけながら同時に関節（頭）を動かしたいです。（下のcの場合です。） a. (send *pepper* :head :neck-p :joint-angle 30) とリセットポーズを繰り返しながら前進

(send *ri* :wait-interpolation) なしでなんとなく頭が動く

b. `(send pepper :head :look-at (float-vector 900 50 900)) ;; 座標は固定`` とリセットポーズを繰り返しながら前進

(send *ri* :wait-interpolation) なしでなんとなく頭が動く

c. (send *pepper* :head :look-at *people-pos*) ;; 座標は逐一変わる、人の位置をパブリッシュするトピックから得られる　とリセットポーズを繰り返しながら前進

(send *ri* :wait-interpolation) なしで全く頭が動かない時がある

cの場合に(send *ri* :wait-interpolation) を加えると、頭は動くようになるのですが、 前進する、止まる、首を動かす、また前進する・・・のような動作になり困っています。

(send *ri* :wait-interpolation) を使わずにしっかり頭を動かすコツのようなものがもしあれば、 他のロボットでやられている例等ございましたら、キーワードやサンプルコード等教えていただけないでしょうか。 ご迷惑をお掛けし申し訳ございません。 => :look-at と :inverse-kinematics-loop-for-look-at などを見て、 逆運動学で解けた最終結果だけ取り出せないか調べる。今はきっと関節角度列のリストが出ている（？）

試したのは以下のようなプログラムです。

#!/usr/bin/env roseus
;;setup                                                                                                                                                    (require :pepper-interface "package://peppereus/pepper-interface.l")
(unless (boundp '*ri*)
  (pepper-init))
(send *ri* :servo-on)
(send *ri* :angle-vector (send *pepper* :reset-pose))

(ros::load-ros-manifest "peppereus")
(defvar *headsensor* 0)

;;ros init                                                                                                                                         (ros::roseus "test")
(ros::rate 5)

;;main function     
(defun test ()
;; go straight and look up                                                                                                                              
  (send *ri* :go-velocity 0.1 0 0)
  (send *pepper* :head :neck-p :joint-angle 30)
  ;;(send *pepper* :head :look-at (float-vector 900 50 900)) ;; 座標は固定
  ;;(send *pepper* :head :look-at *people-pos*) ;; 座標が逐一変わる　*people-pos*を得る関数については省略
  (send *ri* :angle-vector (send *pepper* :angle-vector))
  (send *ri* :wait-interpolation);;                                                                                                                                      
  (send *pepper* :reset-pose)
  (send *ri* :angle-vector (send *pepper* :angle-vector))
  (send *ri* :wait-interpolation);;                                                                                                                                      
  )

(defun show_head_sensor (msg)
  (setq *headsensor* (send msg :state)))

(ros::subscribe "/pepper_robot/tactile_touch"  naoqi_bridge_msgs::TactileTouch #'show_head_sensor)

(defun main ()
  (while (not (= *headsensor* 1))
    (test)
    (ros::spin-once))
  (send *ri* :angle-vector (send *pepper* :reset-pose))
  (send *ri* :speak "おわり"))

(main)
k-okada commented 8 years ago

基本的には， while t send-go-velocity-to-base x y theta send-angular-velocity-to-joints joint-velocity-list

みたいな速度制御型の周期実行プログラムを書けばいいということになります． 周期実行のプログラムはROSだと簡単で

(ros::rate 10) ;; 100mesc 周期で動かす (while (ros::ok) ;; do work (ros::spin-once) (ros::sleep))

です．

台車は:go-velocityで速度制御できます．

で，問題は関節の方ですが， http://wiki.ros.org/robot_mechanism_controllers/JointTrajectoryActionController#Trajectory_Replacement みたいに指令関節軌道の上書きを使うのがeus流です．

つまり，１自由度のロボットがあるとして，現在の関節角度が０度だとして send ri :anlge-vector #f(90) 1000 とすると１秒で90度動いてくれる，ということで，90 [deg/sec] の速度で動かした，ということになります． 関節指令の上書きができるはずですから，これを100msec毎に呼び出してあげれば期待したように動くはずです．

実際に関節指令軌道がうわがかれるかは，試していないのでわかりませんね．ということで， (send ri :anlge-vector av0 5000) (unix:sleep 1) (send ri :anlge-vector av1 5000) としてトータル何秒ロボットが動くかしらべてみてください．１１秒動き続けたとすると上書きしてくれていないことになります また，上書き時に，速度，加速度が連続した軌道を内部で生成してくれているか，というのも結構重要なポイントです．

もしこれで上書きしてくれていない，となった時には (send ri :angle-vector av0 100) (unix:usleep (* 100 1000)) (send ri :anlge-vector av1 100)

または， (send ri :angle-vector av0 100) (send ri :anlge-vector av1 100)

と，してみてください．正確な位置制御ループが組んであるロボットだとこれはav0まで100msecで動いて，一回止まって，av1に動き出す， という挙動をするはずですが，pepperみたいなロボットだと案外連続して動いてくれちゃうかもしれない．

これで上手く動くとすると，ros::rate で周期をかけていくと，だんだんズレていくだろうから， (send ri :wait-interpolation-somoth 10) みたいにすると，本当に止まる10msec前に関数から帰って来てくれるので，これで次の指令値を入れてあげて，なんとなく 滑らかに動くようにしてみる，ということができるはずです．

:wait-interpolation-smooth も，これを送りつける側のプログラムがそこそこちゃんとしていないと動かなくて，HRP2でも最近まで 使われていなかったということで，デバッグは結構難しい（ https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233 ）ので， 動かなかったらおしえて下さい．

◉ Kei Okada

2016-07-21 19:37 GMT+09:00 Kanae Kochigami notifications@github.com:

while文の中で :go-velocity メソッドでペッパーを進ませながら:head :look-at メソッドで人の顔を追いかけながら同時に関節（頭）を動かしたいです。（下のcの場合です。） a. (send pepper :head :neck-p :joint-angle 30) とリセットポーズを繰り返しながら前進

(send ri :wait-interpolation) なしでなんとなく頭が動く

b. (send pepper :head :look-at (float-vector 900 50 900)) ;; 座標は固定` とリセットポーズを繰り返しながら前進

(send ri :wait-interpolation) なしでなんとなく頭が動く

c. (send pepper :head :look-at people-pos) ;; 座標は逐一変わる、人の位置をパブリッシュするトピックから得られる とリセットポーズを繰り返しながら前進

(send ri :wait-interpolation) なしで全く頭が動かない時がある

cの場合に(send ri :wait-interpolation) を加えると、頭は動くようになるのですが、 前進する、止まる、首を動かす、また前進する・・・のような動作になり困っています。

(send ri :wait-interpolation) を使わずにしっかり頭を動かすコツのようなものがもしあれば、 他のロボットでやられている例等ございましたら、キーワードやサンプルコード等教えていただけないでしょうか。 ご迷惑をお掛けし申し訳ございません。

試したのは以下のようなプログラムです。

!/usr/bin/env roseus

;;setup (require :pepper-interface "package://peppereus/pepper-interface.l") (unless (boundp 'ri) (pepper-init)) (send ri :servo-on) (send ri :angle-vector (send pepper :reset-pose))

(ros::load-ros-manifest "peppereus") (defvar headsensor 0)

;;ros init (ros::roseus "test") (ros::rate 5)

;;main function (defun test () ;; go straight and look up (send ri :go-velocity 0.1 0 0) (send pepper :head :neck-p :joint-angle 30) ;;(send pepper :head :look-at (float-vector 900 50 900)) ;; 座標は固定 ;;(send pepper :head :look-at people-pos) ;; 座標が逐一変わる　_people-pos_を得る関数については省略 (send ri :angle-vector (send pepper :angle-vector)) (send ri :wait-interpolation);; (send pepper :reset-pose) (send ri :angle-vector (send pepper :angle-vector)) (send ri :wait-interpolation);; )

(defun show_head_sensor (msg) (setq headsensor (send msg :state)))

(ros::subscribe "/pepper_robot/tactile_touch" naoqi_bridge_msgs::TactileTouch #'show_head_sensor)

(defun main () (while (not (= headsensor 1)) (test) (ros::spin-once)) (send ri :angle-vector (send pepper :reset-pose)) (send ri :speak "おわり"))

(main)

— 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/638, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3BBWqse4WISgtYDY0x_8U0T4X7u2ks5qX0vkgaJpZM4JRqdp .

kochigami commented 8 years ago

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

(ros::rate 100)
(send *ri* :angle-vector (send *pepper* :angle-vector) 100)

のようにして周期と合わせたら人の顔を追いかけるようになりました。 初歩的ですみませんでした。 100のように値を指定しないと1秒間での軌道になり、周期ごとに軌道が上書きされるので全然動かないように見えるということでした。

ループを回す周期と関節角指令の周期を合わせると関節が期待通りに動く 関節指令軌道は上書きされる unix:sleep で立ち止まる ことがわかりました。

つまり，１自由度のロボットがあるとして，現在の関節角度が０度だとして send ri :angle-vector #f(90) 1000 とすると１秒で90度動いてくれる，ということで，90 [deg/sec] の速度で動かした，ということになります． 関節指令の上書きができるはずですから，これを100msec毎に呼び出してあげれば期待したように動くはずです．

期待通り動きました。

実際に関節指令軌道がうわがかれるかは，試していないのでわかりませんね．ということで， (send ri :anlge-vector av0 5000) (unix:sleep 1) (send ri :anlge-vector av1 5000) としてトータル何秒ロボットが動くかしらべてみてください．１１秒動き続けたとすると上書きしてくれていないことになります

初期姿勢をリセットポーズにして試しました。 頭を下げ、立ち止まり、リセットポーズをとりました。関節指令軌道は上書かれることがわかりました。

（頭を下に下げる動き）頭を下げて進む、unix:sleep 1で立ち止まる、頭を上げて進む（リセットポーズに戻る）を繰り返します。頭の下げ方は30度より小さいです。

 (send *ri* :go-velocity 0.1 0 0)
 (send *pepper* :head :neck-p :joint-angle 30)
 (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)                                     
 (unix:sleep 1)                                          
 (send *pepper* :reset-pose)
 (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)

もしこれで上書きしてくれていない，となった時には (send ri :angle-vector av0 100) (unix:usleep (* 100 1000)) (send ri :anlge-vector av1 100) または， (send ri :angle-vector av0 100) (send ri :anlge-vector av1 100) と，してみてください．正確な位置制御ループが組んであるロボットだとこれはav0まで100msecで動いて，一回止まって，av1に動き出す， という挙動をするはずですが，pepperみたいなロボットだと案外連続して動いてくれちゃうかもしれない．

unix:sleep を実行し、一回止まる挙動を見せました。

k-okada commented 8 years ago

確認ですが，reset-poseの状態から

 (send *ri* :go-velocity 0.1 0 0)
 (send *pepper* :head :neck-p :joint-angle 30)
 (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)                                     
 (unix:sleep 1)                                          
 (send *pepper* :head :neck-p :joint-angle 60)
 (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)

とすると一旦とまるかな．

kochigami commented 8 years ago

アドバイスありがとうございました。

ループの中身を以下にして、

  (send *ri* :go-velocity 0.1 0 0)            
  (send *pepper* :head :neck-p :joint-angle -30)
  (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)
  (unix:sleep 1)
  (send *pepper* :head :neck-p :joint-angle 30)  
  (send *ri* :angle-vector (send *pepper* :angle-vector) 4000)                                                                             
  (send *pepper* :reset-pose)

廊下でまっすぐ歩かせて調べました。 念の為externalCollisionProtection Move, Armsをfalse(安全装置を解除 = 障害物があっても止まらない)にしました。

(unix:sleep 1)があることで、1秒くらいのリズムで止まります。 (ros::rate 10) (ros::rate 100)でどちらも同じテンポでした。 (ros::rate 10)の元, (unix:sleep 1)なしでは止まらずに進みました。

首の曲げ方はずれていって、どの場合もどんどん変わっていきます。