start-jsk/rtmros_choreonoid

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

mttamtam opened this issue · 34 comments

mttamtam commented

@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 commented

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

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

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

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

snozawa commented

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

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

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

snozawa commented

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

snozawa commented

@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 commented

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

mttamtam commented

@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 commented

報告ありがとう。
単体で起動できる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 commented

すいません、遅くなりましたが、
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 commented

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

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

mttamtam commented

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

: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 commented

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

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

YoheiKakiuchi commented

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

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

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

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

snozawa commented

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

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

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

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

snozawa commented

テスト関数に

(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 commented

ところで、問題は何だっけ？トピックの/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

YoheiKakiuchi commented

それが、保たれている前提で、tfの/odomがずれるようなら、footcoordsを修正することになると思う。

パット見のコードや、立っている時の値を見るに、トピックの/odomとtfの/odomに差はないように見える。

tfの/odomまで、思ったとおりの連続性を保っていて、rvizの表示がずれるようなら、
odom_initあたりを疑う必要が出てくる。

mttamtam commented

hrpsysとhrpsys_ros_bridgeを最新にしてビルドして試してみましたが、やはりauto-balancerを切ったときにbaseTformが戻ってしまいます（snozawaさんの方法を使ってもirtviewer内で原点に戻る）。

fix-leg-to-coordsしたあと
tamura@aconcagua:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[79.784000000] [0.46779981444491814, -0.0028163545266661296, 0.49109431452413244, 0.9966696209427899, 9.931345486694397e-06, -0.0815454878648955, -1.2509903416793295e-05, 0.9999999994378294, -3.111018502382531e-05, 0.08154548751008701, 3.2026702492421155e-05, 0.9966696205067317]
wait-interpolation-seqのあと
tamura@aconcagua:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[88.068000000] [0.4678000235672189, -0.0028164401029874614, 0.49109431452413244, 0.9966696209427899, 9.931345486694397e-06, -0.0815454878648955, -1.2509903416793295e-05, 0.9999999994378294, -3.111018502382531e-05, 0.08154548751008701, 3.2026702492421155e-05, 0.9966696205067317]
オートバランサを切ったあと
tamura@aconcagua:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[99.436000000] [0.0, 0.0, 1.0225, 1.0, -0.0, 0.0, 0.0, 1.0, -0.0, 0.0, 0.0, 1.0]
start-auto-balancerしたあと
tamura@aconcagua:/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge (master)$ rtprint localhost:15005/abc.rtc:baseTformOut -n1
comp_args: rtprint_reader0?exec_cxt.periodic.type=PeriodicExecutionContext&exec_cxt.periodic.rate=100.0
[109.946000000] [0.20008677555549542, -0.004738065585975912, 1.0017494983157111, 0.9966696202850966, 1.6998985701253187e-05, -0.08154549473634795, -1.957794023565153e-05, 0.9999999993332193, -3.082638199498302e-05, 0.08154549415795775, 3.232021126013792e-05, 0.9966696199533416]

https://gist.github.com/mttamtam/b43f062b45a0f10a14309f82e19865ef

snozawa commented

あれ、手元でhrpsys-simulatorでやったら値が上記のときにかわらなかった。
hrpsys-simulatorを試してもらえるかな？
:start-graspのところだけコメントアウトしたら手元なら動きました。

mttamtam commented

すいません、先程のはカッコが抜けててそのような挙動にこちらでもなったので、修正しました。

mttamtam commented

snozawaさんのおっしゃるとおりで、hrpsys-simulatorだと確かにauto-balancerを切ったときに最初の姿勢に戻らないでとどまりますね。。。。

YoheiKakiuchi commented

すいません。choreonoidで使っているconf/モデルに問題があります。
以下のPRでconfは修正されます。
#214

モデルは、ハンドの関節についてjaxonと異なるので、angle-vectorの長さが違ってしまいます。
choreonoidについては以下のようにしてください。実機は元のコードでOKと思います。

 (let ((cc (send (car (send *robot* :links)) :copy-worldcoords)))
   (send *ri* :angle-vector-sequence-full
         (list (coerce (append (map cons #'deg2rad (send *jaxon_red* :angle-vector)) (list 0 0 0 0)) float-vector)) ;;; angle-vectorに４つ０を足す (ハンド関節に相当）
         ;;; (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)))))
   )
mttamtam commented

修正ありがとうございます。今度はchoreonoidでもうまくいくようになりました。
これってchoreonoidで(send *ri* :angle-vector *nice-pose*)を使いたいときは毎回後ろに０を４つつける必要が出てくるんでしょうか・・・？

YoheiKakiuchi commented

これってchoreonoidで(send ri :angle-vector nice-pose)を使いたいときは毎回後ろに０を４つつける必要が出てくるんでしょうか・・・？

send *ri* :angle-vector は問題ありません。
これが通っていたために、あまり気にされていなかった問題です。
play patternなどでは必要になります。

mttamtam commented

研究会で言っていた問題点：

  • オートバランサを切るときｰ>解決している
  • オートバランサを再度入れるとき->動いてしまう（いきなり飛ぶのではなく補完されて移動する）
  • 更にgo-pos 0 0 0を送るときｰ>手は問題ないが足が前方に伸びきって死ぬ

2足から4足に変化するとき、足先位置はずっと固定であることを使うと変にならないだろうという気がします。
２番めと３番めは違う問題な気がしていて、３番めはgaitgeneratorに与える :leg-default-translate-posをかえて試してみる。

というふうに考えております。

snozawa commented

そもそも今いきなり４脚モードで開始して４脚歩行できてるのかな？
何かエンバグしてるんだろうか？

YoheiKakiuchi commented

まったくコードを見ていないヤマカンだけど、footstepがbaseTform相対でいい感じになるのが入っていない
（もしくは何かを仮定してしまっている 例えば z = 0が地面とか。。）

go-posした時にhrpsysのconsoleにステップ表示されると思うのだけど、それ見たらなにか分かることあるかな？

YoheiKakiuchi commented

anglevector-sequence-fullでデータを送っているけれど、
set-base-pose or set-base-coordsでshのbasePos/baseRotだけ変更するとなにか問題あるかな？

YoheiKakiuchi commented

anglevector-sequence-fullでデータを送っているけれど、
set-base-pose or set-base-coordsでshのbasePos/baseRotだけ変更するとなにか問題あるかな？

seqがqRefを出してしまってダメだった。

snozawa commented

seqがqRefを出してしまってダメだった。

これなんでしょう？
少なくともＳＴや他のコードでつかってる状態量は

  • angle-vector
  • zmp
  • basepos+rot

くらいがほぼマストなので，これらは同期したものをいっきに送る必要があります．
auto-blancerをいれてるときにはなんとなくその辺を整合性あわせてくれてましたが，
auto-balancerなしでは今まではload-patternをつかう必要がありました．
最近ではangle-vector-sequence-fullが使えるようになると思ってます．

set-base-pos, set-base-coordsなどは，他に必要な状態量と同期させられず整合性がとれないので，
あんまり使いどころがないと思ってます（非推奨）

YoheiKakiuchi commented

今回の件では、STやABCは止めている状態なので、とにかくshからbasepos/rotが出ればいいと思っている。
最も短くその状態になれるコードは何かなと思って。

それで、set-base-posを使ったが、seqからbaseposのみの出力ではなく、qRefも出力された。よく分かっていないがこれはseqの仕様なのだろう。
そのときqRefがゼロだった（確認していないがこれは、一度もseqにqRefを送っていないからのような気がする）

snozawa commented

今回の件では、STやABCは止めている状態なので、とにかくshからbasepos/rotが出ればいいと思っている。

おそらく再度どちらかをいれたら問題がでてくるので、やはり個別に状態量を送るのはおすすめでなく、
必要そうな状態量すべてを送ってあげるのがおすすめです。

start-jsk/rtmros_common#988
が進めばコードは短くなります。

また、今回のようにst, abcのstop/startでの切り替わりでの用途なので、
eusでの状態量セット相当の処理をrtc内部で行うのができそうな気がしてきました。
さらに、今回は2脚4脚遷移がabc内部でこなれてないためstop/startしているので、
2脚4脚遷移をちゃんとやるように対応できたら、それはそれでなおってくると思います。

ですので、将来的にはこの遷移のためのangle-vector-sequence-fullのeusのコードが０になります。
なので現状は、この遷移のコードをeusで短くかくのでなく問題ない使い方を勇戦してコードをかいていただけると良いのではと思います。

YoheiKakiuchi commented

ここのコード自体はそんなに問題だと思っていないのだけど、別件？で、
とりあえずはodomの初期値は足元に出てほしい (start-jsk/rtmros_common#986)
があって、それの対応で初期位置を入れるスクリプトを短く書きたい（これ自体はできた）ためでした。

odomの初期位置は理論上は足元(end-coordsと同じ高さ）である必要はないはずですが、
事実上そうなっているとして書かれているコードを修正するのに少し時間がかかりそうと言うことです。

snozawa commented

了解です．
set-target-pos, set-target-coords系のコードはメンテナンスされてなくてあまり使ってないので，現状はangle-vector-sequence-fullで

があって、それの対応で初期位置を入れるスクリプトを短く書きたい（これ自体はできた）ためでした。

をしていただくのが良いと思います．

Naoki-Hiraoka commented

(メモ)
現在のhrpsysで、オドメトリをおかしくせずに2足時にAutoBalancerを切る/入れる方法

;; AutoBalancerのrootlinkの位置姿勢
(setq *odom-coords* (make-coords))
(ros::subscribe "/odom" nav_msgs::Odometry
                #'(lambda (m)
                    (setq *odom-coords* (ros::tf-pose->coords (send m :pose :pose)))
                    ))

(defun start-abc ()
  ;; AutoBalancerがオフの間は、常にユーザーがrootlinkの位置姿勢をangle-vector-sequence-fullのpos+rpyで与えて、odom-BODYのtfを自分で管理する必要がある。
  ;; AutoBalancerを入れる切り替えの際、odomは[今のStateHolderのbaseTform]から、[今のStateHolderの立ち位置で、重心が両足の中心になるようなangle-vectorのroot位置姿勢]へと線形補間される. 実機のangle-vectorは[今のStateHolderのangle-vector]から、[今のStateHolderの立ち位置で、重心が両足の中心になるようなangle-vector]へと線形補間される. 補間の間、angle-vectorと位置姿勢の非線形性から、odomは足の固定が考慮されない不正確な値になる。
  ;; よって、重心が両足の中心の上にありAutoBalancerが修正する必要がないangle-vector+root位置姿勢をStateHolderに持たせてから、AutoBalancerを入れる必要がある。ここまではAutoBalancerがオフの間にユーザーがやってある前提
  (ros::spin-once)
  (send *robot* :move-coords *odom-coords* (car (send *robot* :links)))
  (send *robot* :angle-vector (send *ri* :state :reference-vector))

  (when (< 10 (distance (subseq (send *robot* :centroid) 0 2) (subseq (send (send *robot* :foot-midcoords) :worldpos) 0 2)))
    (print "centroid should be on the foot-midcoords")
    (return-from start-abc nil))

  ;; zmpとoptionalをセットする
  (let ((cc (send (car (send *robot* :links)) :copy-worldcoords)))
    (send *ri* :angle-vector-sequence-full
          (list (send *robot* :angle-vector))
          (list 100)
          :pos (list (send cc :worldpos))
          :rpy (list (reverse (car (send cc :rpy-angle))))
          :optional (list (float-vector 1 1 0 0 1 1 1 1))
          :zmp (list (scale 1e-3 (send (send *robot* :foot-midcoords) :worldpos))))
    (send *ri* :wait-interpolation-seq)
    )

  (send *ri* :start-auto-balancer)
  )

(defun stop-abc ()
  ;; AutoBalancerを切る切り替えの際、odomは[今のAutoBalancerのroot位置姿勢]から、[今のStateHolderのbaseTform]へと線形補間される. 実機のangle-vectorは[今のAutoBalancerのangle-vector]から、[今のStateHolderのangle-vector]へと線形補間される. 補間の間、angle-vectorと位置姿勢の非線形性から、odomは足の固定が考慮されない不正確な値になる。また、切り替え前後で両足の絶対位置や相対位置が固定されない.
  ;; よって、AutoBalancerと全く同じ値をStateHolderに持たせてから、AutoBalancerを切る必要がある。
  ;; 今のAutoBalancerのroot位置姿勢は、/odomトピックからわかる。
  ;; ところが、AutoBalancerのangle-vectorを得る手段が存在しない。取得可能な一番近い値は、実機の位置制御指令値。Stabilizerをオフにした後であれば、CollisionDetectorやSoftErrorLimitorが作動していなければ、Autobalancerのangle-vectorと一致する。

  ;;事前にStabilizerを切る必要がある。
  (send *ri* :stop-st)
  ;;ImpedanceControllerの設定をデフォルトから変えて:set-impedance-controller-param :use-sh-base-pos-rpy tにしている場合、事前にImpedanceControllerを切る必要がある。
  (when (send (send *ri* :get-impedance-controller-param :rarm) :use_sh_base_pos_rpy)
    (send *ri* :stop-impedance :arms))

  (ros::spin-once)
  (send *robot* :move-coords *odom-coords* (car (send *robot* :links)))
  (send *robot* :angle-vector (map float-vector #'rad2deg (send (send (send *ri* :robothardwareservice_getStatus2) :rs) :command)))

  ;; 上記のAutoBalancerの状態をStateHolderにおくる
  (let ((cc (send (car (send *robot* :links)) :copy-worldcoords)))
    (send *ri* :angle-vector-sequence-full
          (list (send *robot* :angle-vector))
          (list 100)
          :pos (list (send cc :worldpos))
          :rpy (list (reverse (car (send cc :rpy-angle))))
          :optional (list (float-vector 1 1 0 0 1 1 1 1))
          :zmp (list (scale 1e-3 (send (send *robot* :foot-midcoords) :worldpos))))
    (send *ri* :wait-interpolation-seq)
    )

  (send *ri* :stop-auto-balancer)
  )