:angle-vector内の:angle-vector-durationに関して

[snozawa]

:angle-vector内の:angle-vector-durationに関して、先週@itohdakと見ていてふと思ったのですが、
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L375
で:angle-vector-durationの部分で「あまり速く動きすぎないようにする」目的で:potentio-vectorを使ってると思いますが、
これは:reference-vectorでなくて良いでしょうか。

rtmros_commonのロボットでは、:angle-vectorはSequencePlayerに届き（値が:reference-vectorに保持されます）、その後その値がimpedanceController、Stabilizer、CollisionDetectorなど、センサフィードバック、フェイルセーフ用制御器が関節角値を上がいています（これが実機に送られ、エンコーダ値にでてくる最終結果が:potentio-vectorになります）。

なので、

• 中で:angle-vectorの受け手であるSequencePlayerにとって速度が過大かどうかチェックするのは、:reference-vectorと比較する
• 逆に、:potentio-vectorは同じ:angle-vectorを送りつづけてても上記の制御器の上書き状況によっては常に何かしら差分がでて、:angle-vector-durationの値が０以上の値がかえってきます。

などの理由から、:reference-vectorが良いようにも思いました。

:potentio-vectorが良いか、:reference-vectorが良いか、はもしかしたらロボットや状況によって違うかもしれなくて議論がいるかもしれないです。

ちなみに以下でrtmros_commonでは再現します

# Stabilizerなどをいれて起動する
openhrp-project-generator `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/sample1_bush.wrl `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/longfloor.wrl --use-highgain-mode false --output /tmp/SampleRobot_for_torquecontrol.xml --timeStep 0.001 --dt 0.002 --method RUNGE_KUTTA
export BUSH_CUSTOMIZER_CONFIG_PATH=`rospack find openhrp3`/../OpenHRP-3.1/customizer/sample1_bush_customizer_param.conf
rtmlaunch hrpsys_ros_bridge samplerobot.launch PROJECT_FILE:=/tmp/SampleRobot_for_torquecontrol.xml hrpsys_precreate_rtc:=PDcontroller USE_UNSTABLE_RTC:=true

(progn (load "euslisp/samplerobot-interface.l") (samplerobot-init) (setq *robot* *sr*) (send *robot* :reset-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *rI* :start-st))
;; :reference-vectorだとほぼ０                                                                                                                                                                                                                                                                                          
(* 1000 (send *ri* :angle-vector-duration (send *ri* :state :reference-vector) (send *robot* :angle-vector) 1 0.0 (*ri* . controller-type)))
;; :potentio-vectorだと０いじょう                                                                                                                                                                                                                                                                                       
(* 1000 (send *ri* :angle-vector-duration (send *ri* :state :potentio-vector) (send *robot* :angle-vector) 1 0.0 (*ri* . controller-type)))

なお、関節角度を少し速めて送りたい（短い時間で送りたい）ときに問題なるかと思ってましたが、
誤って:angle-vectorの引数の:min-timeを1.0秒のままにしてチェックしてたので、
:potentio-vectorで計算される時間があまり大きくなく、あまり問題でないかもしれないです。