(send *ri* :angle-vector)した後のGOALまでの中間姿勢を知りたい

[chiwunau]

ロボットのcontrollerによって挙動が違うと思いますが、
PR2を使っていて途中の姿勢で物体との衝突の有無が知りたいので
MOVEITみたいにGoal Poseを送ったら中間の姿勢列も求めてくれる機能がほしいです。

euslisp上でこれができる機能は実装されているのでしょうか、やはりMOVEITしかないでしょうか

[furushchev]

こんな需要があると思って
jsk-ros-pkg/jsk_pr2eus#237
を作っておきました。
このPR後のものを使うと関節指令値を送るのに、control_msgsを使うようになります。
こうすると、動いている間だけfeedbackとしてdesiredとactual,
errorというプロパティが返ってきますが、そのうちerrorを見ると物や人に当たったかがわかるはずです。
feedbackをどうやって使うかは
jsk-ros-pkg/jsk_roseus#479
を参照するのがいいと思います。
まだしてなければ今後robot-interface.lに取り入れて行ってもいいと思います。

2016年10月14日(金) 10:05 Chi Wun Au [email protected]:

ロボットのcontrollerによって挙動が違うと思いますが、
PR2を使っていて途中の姿勢で物体との衝突の有無が知りたいので
MOVEITみたいにGoal Poseを送ったら中間の姿勢列も求めてくれる機能がほしいです。

euslisp上でこれができる機能は実装されているのでしょうか、やはりMOVEITしかないでしょうか

—
You are receiving this because you are subscribed to this thread.
Reply to this email directly, view it on GitHub
#686, or mute the thread
https://github.com/notifications/unsubscribe-auth/AB0B0Hsfs6QIHw15vYtNQl09rjAStIptks5qzzfKgaJpZM4KWvhI
.

-- ⌘ Yuki Furuta

[chiwunau]

@furushchev ご返信ありがとうございます。
自分が言っていることがわかりにくくてすみませでした。

動作の実行中にものと衝突が起こっているかどうかをチェックするのでではなく、（勿論これも重要だが）
(send ri :angle-vector)で姿勢を実機に送ったら実機はデフォルトで
どういうモーションで目標姿勢に到達するのかを予め知りたいという意味です。

そのモーションがEUSの世界でもしものと衝突するのなら、別のプランニングを行う
ということをしようとしています。

[furushchev]

@chiwunau なるほど、目標姿勢を与えたときの初期姿勢からの補間軌道が知りたいということでしょうか。
action serverへ目標姿勢をgoalとして与えると、おそらく
http://wiki.ros.org/ros_control

のようにControllerが補間軌道を作ったあと、hardware_interfaceを通してアクチュエータへ行くので、hardware_interfaceをdummyのものに置き換えたcontrollerを作って、切り替えられるようにすればできるように思います。
または、gazeboを同時に立ち上げて、そちらで軌道を試してからにするというのもいいかもしれません。
補間軌道のアルゴリズム自体はスプライン補間がベースだと思います。
https://github.com/ros-controls/ros_controllers/blob/8a5a6909327919eee33f4e0ee2e1522272e50660/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h

;; 別のプランニングというのをどうするのかが気になります

[mmurooka]

controllerとは別にeusで補間して確認するとしたら
euslisp/jskeus#391 が関係あるかな．

[chiwunau]

euslisp/jskeus#391 の(pos-list-interpolation)で出来ました、ありがとうございます。

補間ではなくプランニングになってしまいますが、
moveit上でもほぼ同じ挙動ができたので、コードを貼っておきます。

;;体内 or Gazebo;;
roslaunch jsk_pr2_startup start_pr2_moveit.launch
;;visualization;;
roslaunch pr2_moveit_config moveit_rviz.launch config:=true

;;euslisp;;
(load "package://pr2eus/pr2-interface.l")
(load "package://pr2eus_moveit/euslisp/pr2eus-moveit.l")
(pr2-init)
(send *ri* :set-moveit-environment (instance pr2-moveit-environment :init)
(setq ret (send *ri* :angle-vector-make-trajectory (send *pr2* :reset-pose) :move-arm :arms :use-torso t))
(setq traj (send ret :trajectory :joint_trajectory))
(send *ri* :joint-trajectory-to-angle-vector-list :arms traj :send-trajectory nil) ;;nil で実行しない