２つのangle-vector-secquenceを滑らかに繋ぐ

[chiwunau]

74 と少し関係があるかもしれないが、今PR２でドアを開ける時に、avsAの終了時間をavsBの開始時間を一致させることで、見た目はたしかに滑らかに見えるが、どうしても一回速度落としてから再加速するので、結果としてフィードバック周期を小さくしたら重いものが開けられなくなります。

問題は、avsBを送る時にavsAのJointTrajectoryPointによる生成したスプラインを再計算？終点速度をゼロにしない方法がありますか？

参考として、ドアを開ける時に取ったvirtual forceのグラフを添付します。avsを繋ぐところで振動することがわかります

[garaemon]

何をやっているのかよくわからないので、短いサンプルコードを作ってください

◉ ryohei

2014-12-24 11:58 GMT+09:00 Chi Wun Au notifications@github.com:

74 https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/74

と少し関係があるかもしれないが、今PR２でドアを開ける時に、avsAの終了時間をavsBの開始時間を一致することで、見た目はたしかに滑らかに見えるが、どうしても一回速度落としてから再加速するので、結果としてフィードバック周期を小さくしたら重いものが開けられなくなります。

問題は、avsBを送る時にavsAのJointTrajectoryPointによる生成したスプラインを再計算？終点速度をゼロにしない方法がありますか？

参考として、ドアを開ける時に取ったvirtual forceのグラフを添付します。avsを繋ぐところで振動することがわかります [image: open_door_force] https://cloud.githubusercontent.com/assets/7259680/5545379/d1e9c86a-8b63-11e4-9455-47967e114ecd.png

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/81.

[YoheiKakiuchi]

サンプルコードを見てからですが、 avsAの終了時刻より前に、avsBの開始時刻が来ないといけないと思います。

[chiwunau]

例えば下のように、avs1が実行している間にavs2を送る場合、avs1とavs2の間に一瞬とまらずに繋ぎたいです。

(setq avs1 nil)
(setq avs2 nil)
(dotimes (i 10)
  (push (send *pr2* :larm :inverse-kinematics (send (send *pr2* :larm :end-coords :copy-worldcoords) :translate #f(30 0 0)))
        avs1))
(dotimes (i 10)
  (push (send *pr2* :larm :inverse-kinematics (send (send *pr2* :larm :end-coords :copy-worldcoords) :translate #f(30 0 0)))
        avs2))
(setq avs1 (reverse avs1))
(setq avs2 (reverse avs2))

(ros::rate 20)
(let ((st (ros::time+ (ros::time-now) (ros::time 0.5))))
  (send *ri* :angle-vector-sequence avs1 (list 500) :default-controller st)
  (while (ros::ok)
    (when (>= (send (ros::time- (ros::time-now) (ros::time+ st (ros::time (/ (* (/ (length avs1) 2) 500) 1000.0)))) :to-sec) 0.0)
      (send *ri* :angle-vector-sequence avs2 (list 500) :default-controller (ros::time+ st (ros::time (+ 0.0 (/ (* (length avs1) 500) 1000.0)))))
      (return))
    (ros::spin-once)
    (ros::sleep)))

[garaemon]

グラフをvirtual force sensorではなくてjoint_statesの興味のある関節にしてみよう

[garaemon]

1) ROS的にはvelocityを適切に入れると良いかも 2) avs1ももう一度送ったら良いかも

つまり、new trajectoryだけでなく、original trajectory (not deleted)も一緒に送る

[chiwunau]

植田さんが言ったようにしたらうまく行きました。 ありがとうございます

参考までに

(setq avs3 (append avs1 avs2))
(ros::rate 20)
(let ((st (ros::time+ (ros::time-now) (ros::time 0.5))))
  (send *ri* :angle-vector-sequence avs1 (list 500) :default-controller st)
  (while (ros::ok)
    (when (>= (send (ros::time- (ros::time-now) (ros::time+ st (ros::time (/ (* (/ (length avs1) 2) 500) 1000.0)))) :to-sec) 0.0)
      (send *ri* :angle-vector-sequence avs3 (list 500) :default-controller st)
      (return))
    (ros::spin-once)
    (ros::sleep)))

[garaemon]

https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/74#issuecomment-67021422 ここのケース5のやり方ですね

[garaemon]

どれだけスムーズになったかの比較動画があるといいですね

[k-okada]

これ，おもしろいですね．簡単なインターフェースをつくろうとすると，前に送ったデータを保存してその終了（予定）時刻をみておいて，それより同じか早ければ，それをapendして送るという感じですかね． ただ，一番カンタンにはケース４で行ける気がするんだけど，だめなのかな．つまり，適当な周期か，センサの入力を考えて，それより長い時間で指令値を出す．例えば極端に言えば指令値は毎回１秒の軌道を考えて送るけど，それを０．１秒毎に更新する．何もなければ，その１秒が実行されるけど，フィードバックのセンサがあれば０．１秒毎に更新される． 歩行の軌道なんかはそうやって送られているはずです．

◉ Kei Okada

On Wed, Dec 24, 2014 at 12:43 PM, Chi Wun Au notifications@github.com wrote:

植田さんが言ったようにしたらうまく行きました。 ありがとうございます

参考までに

(setq avs3 (append avs1 avs2)) (ros::rate 20) (let ((st (ros::time+ (ros::time-now) (ros::time 0.5)))) (send ri :angle-vector-sequence avs1 (list 500) :default-controller st) (while (ros::ok) (when (>= (send (ros::time- (ros::time-now) (ros::time+ st (ros::time (/ (* (/ (length avs1) 2) 500) 1000.0)))) :to-sec) 0.0) (send ri :angle-vector-sequence avs3 (list 500) :default-controller st) (return)) (ros::spin-once) (ros::sleep)))

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/81#issuecomment-68023519 .

[garaemon]

二つ目に送る指令値の最初の速度を良い感じに指定すれば、それはそれで大丈夫なはず。歩行とかは多分そんな感じですね？

問題点はハードリアルタイムなフィードバックでないと機能しなそうです。

✉︎ Ryohei

On Wed, Dec 24, 2014 at 2:58 PM, Kei Okada notifications@github.com wrote:

これ，おもしろいですね．簡単なインターフェースをつくろうとすると，前に送ったデータを保存してその終了（予定）時刻をみておいて，それより同じか早ければ，それをapendして送るという感じですかね． ただ，一番カンタンにはケース４で行ける気がするんだけど，だめなのかな．つまり，適当な周期か，センサの入力を考えて，それより長い時間で指令値を出す．例えば極端に言えば指令値は毎回１秒の軌道を考えて送るけど，それを０．１秒毎に更新する．何もなければ，その１秒が実行されるけど，フィードバックのセンサがあれば０．１秒毎に更新される．

歩行の軌道なんかはそうやって送られているはずです．

◉ Kei Okada On Wed, Dec 24, 2014 at 12:43 PM, Chi Wun Au notifications@github.com wrote:

植田さんが言ったようにしたらうまく行きました。 ありがとうございます

参考までに

(setq avs3 (append avs1 avs2)) (ros::rate 20) (let ((st (ros::time+ (ros::time-now) (ros::time 0.5)))) (send ri :angle-vector-sequence avs1 (list 500) :default-controller st) (while (ros::ok) (when (>= (send (ros::time- (ros::time-now) (ros::time+ st (ros::time (/ (* (/ (length avs1) 2) 500) 1000.0)))) :to-sec) 0.0) (send ri :angle-vector-sequence avs3 (list 500) :default-controller st) (return)) (ros::spin-once) (ros::sleep)))

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/81#issuecomment-68023519 .

---

Reply to this email directly or view it on GitHub: https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/81#issuecomment-68029051