オドメトリをおかしくせずに2足から4足に移行する方法

[mttamtam]

@orikuma さんのSLAMを継続的に2足歩行の状態と4足歩行の状態を行き来しながら使えるようにしたいです。
いま、普通の2足の姿勢から4足の姿勢に移行するときに、hrpsys_ros_bridge_jvrcのgait_controller.lを使っていて、
https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l#L363-L400
腰を下ろした姿勢の状態で、一度stとautobalancerを切ってから四足の姿勢になり、そこでstとautobalancerをもう一度入れる、というふうになっていますが、こうすると途中でオドメトリが狂う？（少なくとも、octomapは乱れる)みたいで、オートバランサを入れっぱなしで姿勢変化ができたらうれしいのですが、なかなかうまくいかないです。
https://github.com/mttamtam/rtmros_choreonoid/blob/gctemp/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l#L363-L407
例えば、こんな感じに書き換えるとシミュレータでは姿勢変化はうまく行くが、go-posしようとすると、手のcoordsがおかしくなります。

@snozawa さん、先ほど口頭で教えていただいたのにまたわからなくなってしまったのですが、touch-down-poseの状態で、一度オートバランサを切る前に腰の座標をもらってから、それをどこに入れるんでしたっけ？

[YoheiKakiuchi]

オドメトリがおかしくならないようにするのに、どこを修正すべきかは、いくつか候補があるように思います。

1. SLAM側を修正する （なにをどうやって修正するかで作戦がいくつも出てきそう）

2. AutoBalancerを切ったときにAutoBalancerから、それらしいodom (baseTform)がでるようにしてほしい

3. できるだけautobalancerを切らずに動作を行う。

[snozawa]

さっき口頭ではなしていて、

2. AutoBalancerを切ったときにAutoBalancerから、それらしいodom (baseTform)がでるようにしてほしい
3. できるだけautobalancerを切らずに動作を行う。

がいまのところの候補です

[snozawa]

ちょっとABCにも修正が必要な気がしてきた

[snozawa]

@mttamtam

以下を試してみてください。

hrpsys-baseのfkanehiro/hrpsys-base#1065 のbranchを使って、以下を試してみてください。

rtmlaunch hrpsys_ros_bridge samplerobot.launch USE_UNSTABLE_RTC:=true

;; 初期化
(progn (load "samplerobot-interface.l")(samplerobot-init) (setq *robot* *sr*) (send *ri* :angle-vector (send *sr* :angle-vector) 2000)))
(send *ri* :start-auto-balancer)

;; 歩いたとする
(send *ri* :go-pos 0.2 0.1 0)

;; 今の立ち位置を反映させて、eus上の*robot*のロボット状態を更新
(send *robot* :reset-pose) ;; これはreset-poseでなくて良いです。多分４脚の姿勢かな。
(send *robot* :fix-leg-to-coords (send *ri* :get-foot-step-param :dst-foot-midcoords))    

;; 上記の状態をStateHolderにおくる
(let ((cc (send (car (send *robot* :links)) :copy-worldcoords)))   
(send *ri* :angle-vector-sequence-full 
(list (map float-vector #'deg2rad (send *robot* :angle-vector))) 
(list 2) 
:pos (list (scale 1e-3 (send cc :worldpos)))  
:rpy (list (car (send cc :rpy-angle)))  
:optional (list (float-vector 1 1 0 0 1 1 1 1))   
:zmp (list (scale 1e-3 (send cc :inverse-transform-vector (send (send *robot* :foot-midcoords) :worldpos)))))  
))

;; stopしてみる
(send *ri* :stop-auto-balancer) ;; この時点で、odomがstop前後でほぼかわってないのを確認

stop-auto-balancerしたときにstateHolderが送ってる値に動機するようになるのですが、
その前にstateHolderにAutoBalancerがだしてる値相当の状態を持たせておくことで（angle-vector-sequence-full)
stop時に値が大きく変化するのを防ぎます

[snozawa]

@mttamtam
fkanehiro/hrpsys-base#1065
がmergeされました。

[mttamtam]

@snozawa さん、 @YoheiKakiuchi さん、

先ほど見ていただいたときの、readlineは、このように入っていて、

 (send *robot* :angle-vector touch-down-pose)
 (send *robot* :fix-leg-to-coords (send *ri* :get-foot-step-param :dst-foot-midcoords))
 (read-line)
 (let ((cc (send (car (send *robot* :links)) :copy-worldcoords)))
   (send *ri* :angle-vector-sequence-full
         (list (map float-vector #'deg2rad (send *robot* :angle-vector)))
         (list 2)
         :pos (list (scale 1e-3 (send cc :worldpos)))
         :rpy (list (car (send cc :rpy-angle)))
         :optional (list (float-vector 1 1 0 0 1 1 1 1))
         :zmp (list (scale 1e-3 (send cc :inverse-transform-vector (send (send *robot* :foot-midcoords) :worldpos)))))
   )
 (send *ri* :wait-interpolation-seq)
 (read-line)

 (send *ri* :stop-st)
 (send *ri* :stop-auto-balancer)
 (read-line)
 (send* *ri* :set-auto-balancer-param (cadr (memq :auto-balancer-param quadruped-params)))
 (send *ri* :start-auto-balancer)

都度のrtprintの結果は（こんなコマンドがあるのですね）、
tamura@aconcagua:~/private-scripts (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1 comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0 [150.868000000] [0.4675083437061405, -0.0028284100937961054, 0.49110471872587297, 0.9966696209461231, -1.4902685512286942e-05, -0.08154548706716555, 1.224142086016248e-05, 0.9999999993761002, -3.3135288188584935e-05, 0.0815454875100941, 3.202670249242139e-05, 0.9966696205067311] tamura@aconcagua:~/private-scripts (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1 comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0 [175.102000000] [0.46750834370614036, -0.0028284100937961, 0.49110471872587297, 0.9966696209461231, -1.4902685512286942e-05, -0.08154548706716555, 1.224142086016248e-05, 0.9999999993761002, -3.3135288188584935e-05, 0.0815454875100941, 3.202670249242139e-05, 0.9966696205067311] tamura@aconcagua:~/private-scripts (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1 comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0 [191.686000000] [0.0, 0.0, 1.0225, 1.0, -0.0, 0.0, 0.0, 1.0, -0.0, 0.0, 0.0, 1.0]
という感じでした。rvizでodomでロボットモデルを表示させていても、最初の2つは特にこれといった変化はなく、最後の時に原点のベース座標に戻っているようでした（その後start-auto-balancerすると浮いたまま水平移動して元の位置に戻る)。

[snozawa]

報告ありがとう。
単体で起動できるeusのテストプログラムは作れそうかな？
問題の部分を切り出して、

(yy) ;; 立った状態の初期姿勢
(send *ri* :start-auto-balancer)
(xxx) ;; しゃがみ姿勢のangle-vector, (send (car (send *robot* :links)) :copy-worldcoords)を取得してコピペ。
(send *ri* :angle-vector (send *robot* :angle-vector) 5000) ;; しゃがみ姿勢をおくる
(send *ri* :wait-interpolation)
(xx)
(send *ri* :angle-vector-sequence-full xxx) ;; この辺もこぴぺ
(send *rI* :wait-interpolation-seq)
(send *ri* :stop-auto-balancer)

くらいの分量で際限するコードができるか作って見てください。
odomの確認自体はrvizで目視する、か、rtprintでというのでいいかな。

[mttamtam]

すいません、遅くなりましたが、
https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l#L177-L184
この multi-gait クラスの :slots を外から見る方法がわからなくて、結局ほとんどコピペみたいになってますが、
rtmlaunch hrpsys_choreonoid_tutorials jaxon_red_choreonoid.launch
を上げておいて、
https://gist.github.com/mttamtam/b43f062b45a0f10a14309f82e19865ef
これを実行して、しゃがみこんでからエンターを３階押すと、２回めと３回めの時にrviz上で移動します。

[snozawa]

:angle-vector-sequence-fullのrpy引数は
:rpy (list (car (send cc :rpy-angle)))

でなく
:rpy (list (reverse (car (send cc :rpy-angle))))
を試してもらえるかな？

[mttamtam]

ほとんど挙動が変わりませんでした。

:rpy (list (reverse (car (send cc :rpy-angle))))のとき、

tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[53.910000000] [0.4676571793451086, -0.0028125559122384224, 0.49110539810754317, 0.9966696210212994, -2.660502999111022e-06, -0.08154548746669599, 4.000944778565663e-08, 0.9999999994836061, -3.2136993449768066e-05, 0.08154548751008694, 3.2026702492421366e-05, 0.9966696205067317]
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[59.082000000] [0.46765720580638914, -0.0028124469754110476, 0.49110539810754317, 0.9966696210212994, -2.660502999111022e-06, -0.08154548746669599, 4.000944778565663e-08, 0.9999999994836061, -3.2136993449768066e-05, 0.08154548751008694, 3.2026702492421366e-05, 0.9966696205067317]
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[65.684000000] [0.0, 0.0, 1.0225, 1.0, -0.0, 0.0, 0.0, 1.0, -0.0, 0.0, 0.0, 1.0]
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[70.640000000] [0.20008673507203212, -0.004737614475807114, 1.0017494105893148, 0.996669620302389, 1.6094963225239654e-05, -0.08154549470843711, -1.867692849715373e-05, 0.9999999993481781, -3.090010095510242e-05, 0.08154549415794798, 3.232021126016192e-05, 0.9966696199533425]

reverseしてないとき
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[49.536000000] [0.4681620168921006, -0.0028206484402832593, 0.49097901608988337, 0.9966696205472445, -3.3463170381745376e-05, -0.08154548643810959, 3.07400922915723e-05, 0.9999999989272533, -3.4648811962924086e-05, 0.08154548751009104, 3.202670249243173e-05, 0.9966696205067314]
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[52.668000000] [0.46816214025031294, -0.0028206150987334553, 0.49097901608988337, 0.9966696205472445, -3.3463170381745376e-05, -0.08154548643810959, 3.07400922915723e-05, 0.9999999989272533, -3.4648811962924086e-05, 0.08154548751009104, 3.202670249243173e-05, 0.9966696205067314]
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[59.946000000] [0.0, 0.0, 1.0225, 1.0, -0.0, 0.0, 0.0, 1.0, -0.0, 0.0, 0.0, 1.0]
tamura@denali:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge/euslisp (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[68.574000000] [0.20008730436320663, -0.004737475344617651, 1.0017490110417029, 0.996669620307958, 1.579434194082351e-05, -0.081545494699153, -1.8377308395564186e-05, 0.9999999993529713, -3.09246152664677e-05, 0.08154549415795677, 3.232021126015522e-05, 0.9966696199533417]

[snozawa]

なるほど。ここだけが問題ではなかったっぽいね。
一度、何個か前に教えてもらった際限コードについて、
rvizでodomを表示させて見るというのでなく、rtprintのabc.rtc:baseTformOutの表示で、
値がとんでるかどうか、原点に戻ってるか調べられるかな？

odomはbaseTformOutの値をちょっとかえてだしてるかもしれないので。

[YoheiKakiuchi]

４足歩行でないパターンで見てみたけど、angle-vector-sequencce-fullを送るとabcのbaseTformは変わっているようだ。

なので、見てみるといいのは、
hrpsysはアップデート & ビルドされているか。
angle-vector-sequencce-full で送っているデータは正しいか。

(send (car (send *robot* :links)) :copy-worldcoords)

を送るまえにprintしてみる。

[snozawa]

あとは，baseTformは適切にでてるけど，odomの計算部分で何か違ってる，というのもありそうと思いました．
ということで，

rvizでodomを表示させて見るというのでなく、rtprintのabc.rtc:baseTformOutの表示で、

を試してみると良いと思いました．

こちら，すいません，まだ僕自身は，自分の手元では試せてないです

[snozawa]

テスト関数に

(defun get-abc-baseTformOut ()
  (let* ((tmp (read-from-string (format nil "#f(~A)" (substitute (elt " " 0) (elt "]" 0) (substitute (elt " " 0) (elt "[" 0) (substitute (elt " " 0) (elt "," 0) (cadr (piped-fork-returns-list "rtprint -n1 localhost:15005/abc.rtc:baseTformOut"))))))))
         (cc (make-coords :pos (scale 1e3 (subseq tmp 1 4)) :rot (make-matrix 3 3 (list (subseq tmp 4 7) (subseq tmp 7 10) (subseq tmp 10 13))))))
    (send *robot* :move-coords cc (car (send *robot* :links)))
    cc))

を定義してみて、(get-abc-baseTformOut)関数をよんで(send *irtviewer* :draw-objects)よんだ２行くらいを、
各read-lineの手前でよんで見てください。
baseTformの値をeusにとってきて、coordsになおして、*robot*にセットしてくれます。
それでぱっとこちらで試すと、試してもらった箇所では大丈夫そうでした。
一方、4脚でstart-auto-balancerしたらズレたっぽいです。
そちらの手元でも確認してもらえると助かります。

ところで、問題は何だっけ？トピックの/odomがうごいちゃうこと？それともtfの/odomがうごいちゃうこと？
もしかしたらabcからの出力がただしくて、odomとかの計算がおかしいのかもしれない。
それを確認したいです。

[YoheiKakiuchi]

ところで、問題は何だっけ？トピックの/odomがうごいちゃうこと？それともtfの/odomがうごいちゃうこと？
もしかしたらabcからの出力がただしくて、odomとかの計算がおかしいのかもしれない。
それを確認したいです。

一番最初の目標は、トピックの/odom (これは、位置姿勢は abc.baseTformOut そのまま）が、auto-balancerのon/off、４足歩行のシーケンスで連続性を保つこと。

それが、保たれている前提で、tfの/odomがずれるようなら、footcoordsを修正することになると思う。
https://github.com/jsk-ros-pkg/jsk_control/blob/master/jsk_footstep_controller/src/footcoords.cpp