search

start-jsk / rtmros_choreonoid

using chreonoid for simulator with hrpsys and other ros system
9 stars 43 forks source link

4足歩行のインターフェース #66

Open YoheiKakiuchi opened 9 years ago

YoheiKakiuchi commented 9 years ago

https://github.com/start-jsk/rtmros_choreonoid/pull/65 jaxonの4足歩行用のファイルを追加しました。

@eisoku9618 さん、以下が気付き事項です。 毎回（毎歩行パターン）のIKを解いているので、パターン生成待ち時間が多いです。 途中パターンの生成はサボれるのでは？ （特に、xかyだけにしか値が入っていない時)

その場回転でIKが解けなくて、パターン生成で結構待たされて、その後に動かない。

初期姿勢を変えたい場合はどうしたらいいですか？ その場回転のIKが解けるかは初期姿勢で変化しますか？

snozawa commented 9 years ago

https://github.com/start-jsk/rtmros_choreonoid/pull/69 で少しパラメータを更新しました。 とくに:min-loop 2がないとIKがとけていても10回くらいループをまわしてしまって、おそくなります。 トロットがおすすめですが、x,y,thからtrotのfootstepを計算するところがうごいてなさそうで、

    ;; viewing                                                                                                                                                                                                                                                                                                         
    (dolist (fs fsl)
      (dolist (f fs)
        (send f :draw-on :flush t :size 200)))

の上くらいで

    (setq fsl
          (list (list (make-coords :coords (send *robot* :rleg :end-coords :copy-worldcoords) :name :rleg) (make-coords :coords (send *robot* :larm :end-coords :copy-worldcoords) :name :larm))
                (list (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0) :world) :name :lleg) (make-coords :coords (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 100 0 0) :world) :name :rarm))
                (list (make-coords :coords (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0) :world) :name :rleg) (make-coords :coords (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 200 0 0) :world) :name :larm))
                (list (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 300 0 0) :world) :name :lleg) (make-coords :coords (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 300 0 0) :world) :name :rarm))
                (list (make-coords :coords (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 400 0 0) :world) :name :rleg) (make-coords :coords (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 400 0 0) :world) :name :larm))
                (list (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 400 0 0) :world) :name :lleg) (make-coords :coords (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 400 0 0) :world) :name :rarm))
                ))
    (setq fsl (reverse fsl))

とするとあるきます。

eisoku9618 commented 9 years ago

ひとまずトロット歩行（含めていろんな歩き方）をできるようにしました．

https://github.com/start-jsk/rtmros_choreonoid/pull/70

毎回（毎歩行パターン）のIKを解いているので、パターン生成待ち時間が多いです。

野沢さんの変更とcrawlではなくtrotにしたことで，1m前進も許容できる程度の待ち時間になりましたが，どうでしょうか．

その場回転でIKが解けなくて、パターン生成で結構待たされて、その後に動かない。 初期姿勢を変えたい場合はどうしたらいいですか？ その場回転のIKが解けるかは初期姿勢で変化しますか？

初期姿勢にとても依存するみたいです．(go-pos-quad-real)する前に*robot*にangle-vectorを与えてあげるとそれが初期姿勢になります． たとえば，以下のようにしたら25度までは回転できることを確認しました．

(reset-pose-to-touch-down)
(go-pos-quad-real :th 25 :type :trot)
YoheiKakiuchi commented 9 years ago

競技としてやってみてみているかな？ そもそも、待ち時間が秒単位であると使えないように思うけどどうだろうか？

eisoku9618 commented 9 years ago

明日試してみますが，作戦としては

をいくつか用意しておいて，急を要するときだけ歩行パターンを生成する，というのはどうでしょうか．

※ hrpsysで地面に手をついて歩くパターンを生成するには，以下の3ステップくらい足りていなくて，現状だとできないです．すみません．

YoheiKakiuchi commented 9 years ago

作戦としてはそんなかんじでしょうか？ カメラとレンジセンサでやってみると、結構ぶつかる感覚がわからなくて柵にあたって転倒してしまいます。

otsubo commented 9 years ago

手先の接触点が実機と違うみたいで，それゆえ滑ってしまっている気がします．

irtviewer上にコレオノイド版のモデルを表示することは可能でしょうか？

otsubo commented 9 years ago

rvizを見ながらやる際は

が良さそうでした．

(progn
  (reset-pose-to-touch-down)
  (setq *1m-mae* (go-pos-quad :x 1000 :y 0 :th 0 :type :trot :dt 0.1
                              :default-step-height 50
                              :default-step-time 1.0))
  (reset-pose-to-touch-down)
  (setq *50cm-mae* (go-pos-quad :x 500 :y 0 :th 0 :type :trot :dt 0.1
                                :default-step-height 50
                                :default-step-time 1.0))
  (reset-pose-to-touch-down)
  (setq *30cm-back* (go-pos-quad :x -300 :y 0 :th 0 :type :trot :dt 0.1
                                 :default-step-height 50
                                 :default-step-time 1.0))
  (reset-pose-to-touch-down)
  (setq *5m-hidari* (go-pos-quad :x 0 :y 5000 :th 0 :type :trot :dt 0.1
                                 :default-step-height 50
                                 :default-step-time 1.0))
  (reset-pose-to-touch-down)
  (setq *20deg-hidari* (go-pos-quad :x 0 :y 0 :th 20 :type :trot :dt 0.1
                                    :default-step-height 50
                                    :default-step-time 1.0)))
YoheiKakiuchi commented 9 years ago

irtviewer上にコレオノイド版のモデルを表示することは可能でしょうか？

hrpsys_ros_bridge_jvrc/models/jaxon_jvrc.l にでます。

JVRCとは関係ないですが、 手先の接触点は実機で見ているのかな？ それともシミュレーション？ jaxon_redのモデルと実機も違うと思うけどこれは修正されているのかな？

eisoku9618 commented 9 years ago

hrpsys_ros_bridge_jvrc/models/jaxon_jvrc.l にでます。

ありがとうございます。

手先の接触点は実機で見ているのかな？ それともシミュレーション？

どこかのタイミングの実機で計測していました。

jaxon_redのモデルと実機も違うと思うけどこれは修正されているのかな？

「認識結果から target coords を決めて、そこに向けて手を動かして接触させる」というのを実機でやってみると、ぴったり合うので、合っているのかと思っていましたが、今度ちゃんと見てみます。

eisoku9618 commented 9 years ago

今日出た2足歩行で歩けちゃう説を検証したら行けたので，これが良さそうです．

という点が良さそうでした．

image

最後適当にやってしまってこけましたが，すべてgo-posのみでやってみた感じとしては

と思いました．

image

コミットしますが，以下のようなコマンドで最後まで行きました．

roseus `rospack find hrpsys_ros_bridge_jvrc`/euslisp/4legs-walking.l

(progn
  (init)
  (let ((avs (reset-pose-to-touch-down)))
    (send *ri* :angle-vector (cadr (memq :angle-vector (elt avs 0))))
    (send *ri* :wait-interpolation)
    (send *ri* :angle-vector (cadr (memq :angle-vector (elt avs 1))))
    (send *ri* :wait-interpolation)
    )
  (send *ri* :start-st)
  (send *robot* :angle-vector (send *ri* :state :potentio-vector))
  (send *robot* :fix-leg-to-coords (make-coords))
  (send *robot* :arms :move-end-pos #f(0 0 200) :world)
  (send *robot* :larm :move-end-pos #f(0 -100 0) :world)
  (send *robot* :larm :move-end-pos #f(0 +100 0) :world)
  (send *ri* :angle-vector (send *robot* :angle-vector))
  (send *ri* :wait-interpolation))

(send *ri* :go-pos 0 0 0)
eisoku9618 commented 9 years ago

rviz では以下のように見えます．

image

image

eisoku9618 commented 9 years ago

また、上記PRでハンドの接触点をjvrc用のハンドに合わせたので、四足歩行したときの滑りが減りました。

@YutaKojio 上のPRの4legs-walking.lをロードして (move-to-init-pose)で両手を地面についてabcを切って、 (go-pos-quad-real :x 1000 :type :crawl) すると、一歩ずつ動かしていくタイプの四足歩行ができると思います。

snozawa commented 9 years ago

２脚歩行姿勢は、よこからみた絵があるかな？ あたまは少し下げても良いですね。 これで、横幅も上もぶつからなくなったかな。

あと、STは一応４脚対応ほぼしています。

YutaKojio commented 9 years ago

上のPRの4legs-walking.lをロードして (move-to-init-pose)で両手を地面についてabcを切って、 (go-pos-quad-real :x 1000 :type :crawl) すると、一歩ずつ動かしていくタイプの四足歩行ができると思います。

わかりました．ありがとうございます．