[fetch] :cancel-angle-vectorが上手く動かない。

[708yamaguchi]

fetchを使っていたのですが、

(defun test (x)
  (send *ri* :angle-vector-raw (send *fetch* :rarm :move-end-pos (float-vector x 0 0)) 5000)
  (unix:sleep 2)
  (send *ri* :cancel-angle-vector :wait t)
  (send *fetch* :angle-vector (send *ri* :state :potentio-vector)))

という関数を定義して、

(send *ri* :angle-vector (send *fetch* :inverse-kinematics (make-coords :pos #f(800 0 800))) 10000)
(test 100)
(test -100)

とすると、(test -100)した直後に、fetchの手がとても速く動き出します。
test関数の
(send *ri* :angle-vector-raw (send *fetch* :rarm :move-end-pos (float-vector x 0 0)) 5000)
を
(send *ri* :angle-vector (send *fetch* :rarm :move-end-pos (float-vector x 0 0)) 5000)
とするとこの問題は起きなくなるのですが、どうしてでしょうか。

[k-okada]

早くなるのはcancel-angle-vectorを送った時でしょうか？それとも:angle-vector-rawを送った時でしょうか？
後者であれば、rarm_follow_joint_trajectory_action/goal みたいなものをrostopic echo して、結果を教えて下さい．

[708yamaguchi]

速くなるのは:angle-vector-rawを送った時です。
rostopic echoした結果は以下のようでした。

$ rostopic echo /arm_controller/follow_joint_trajectory/goal
header: 
  seq: 1
  stamp: 
    secs: 1506566311
    nsecs: 125774070
  frame_id: ''
goal_id: 
  stamp: 
    secs: 0
    nsecs:         0
  id: 1506566311125890734_/fetch_1506566054327641626_31807_/arm_controller/follow_joint_trajectory_12
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 1506566311
        nsecs: 125774070
      frame_id: ''
    joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']
    points: 
      - 
        positions: [1.5909900852381522, 1.4277171057035565, -1.09460291370634, 1.5614453706202456, -1.6783872192308156, 0.4341607904229965, -1.5645472773593063]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 5
          nsecs:         0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---

[k-okada]

次は、 (test 100) のところでsleepすると、ここでは、+100いく途中でcancelされた+40ぐらいの場所に手が有る？ (test -100) のときは、その+４０から0に戻るのいってが早い？それとも+40から一回+100に行って0に戻ったりしている？

…

-- ◉ Kei Okada 2017-09-28 11:40 GMT+09:00 Naoya Yamaguchi <[email protected]>:

速くなるのは:angle-vector-rawを送った時です。 rostopic echoした結果は以下のようでした。 $ rostopic echo /arm_controller/follow_joint_trajectory/goal header: seq: 1 stamp: secs: 1506566311 nsecs: 125774070 frame_id: '' goal_id: stamp: secs: 0 nsecs: 0 id: 1506566311125890734_/fetch_1506566054327641626_31807_/arm_controller/follow_joint_trajectory_12 goal: trajectory: header: seq: 0 stamp: secs: 1506566311 nsecs: 125774070 frame_id: '' joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'] points: - positions: [1.5909900852381522, 1.4277171057035565, -1.09460291370634, 1.5614453706202456, -1.6783872192308156, 0.4341607904229965, -1.5645472773593063] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 5 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 --- — You are receiving this because you commented. Reply to this email directly, view it on GitHub <#839 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3OZTA3_AzvoJuH2XagMMz5gJ2HH3ks5smwcogaJpZM4Pl4Nr> .

[708yamaguchi]

(test 100)
したあとは、+40くらいのところで手が止まりますが、その後に
(test -100)
すると、+40から-60(もしかしたら-80くらいかも)くらいまで、+100を経由することなく直接動きました。
この時の+40から-60まで動くのが速いです。

[708yamaguchi]

#841

[k-okada]

ちなみにgazeboだと再現しないんだけど、以下のファイル実機だとどうなるかな．
https://gist.github.com/k-okada/3e4cbbc8607b10bca55524122c9c1cf1

rqt_plot joint_states/position[9]

で結果がみれる．

[k-okada]

cancel したあと、sleepして、（元の軌道が終わるべき時間まで過ぎたら？）、新しい軌道を送ると再現するっぽい
ZebraDevs/robot_controllers#33

[708yamaguchi]

以下のプログラムを書いて、

(defun test (x y)
  (send *ri* :angle-vector-raw (send *fetch* :rarm :move-end-pos (float-vector x 0 0)) 5000)
  (unix:sleep 2)
  (send *ri* :cancel-angle-vector)
  (unix:sleep y)
  (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 5000))

(test 100 0)
(test 100 1)
(test 100 5)
上の3つの命令を送ったところ、
(test 100 0)
では再現せず（スムーズに動作が完了し）、
(test 100 1)
(test 100 5)
では再現しました（一旦動きが止まって急に動き出しました）。