Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add initial jsk_kinova_robot package #1313

Merged
merged 46 commits into from
Jul 16, 2021

Conversation

708yamaguchi
Copy link
Member

@708yamaguchi 708yamaguchi commented Jan 2, 2021

I add jsk_kinova_robot
Current README:
https://github.com/708yamaguchi/jsk_robot/blob/kinova-gen3/jsk_kinova_robot/README.md

TODO:

@k-okada
Copy link
Member

k-okada commented Jun 16, 2021 via email

@tkmtnt7000
Copy link
Member

飴を渡すデモです
https://drive.google.com/file/d/1QcqxxwaxEmOoU1G3Zd1M2j6jl9kOWuvv/view?usp=sharing

@k-okada
Copy link
Member

k-okada commented Jun 23, 2021 via email

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jun 23, 2021

やったこと・これからやること

  • spot用launchとkinova用launchを別々に立ち上げたときに何のtopicが出るかを確認し、名前が重複しているtopicが/tf/tf_staticのみであることを確認した。(joint_statesのトピック名は、kinova: /arm_gen3/joint_states、spot: /joint_statesだった)
  • spot用launchとkinova用launchを同時に立ち上げた状態で、spot-interface以下のroseus関数を呼ぶことが出来た。これにより、tfはトピック名は重複しているものの、特に問題ないと考えた。
(send *ri* :stand)
(send *ri* :go-velocity 0.1 0 0 1000)
  • spot用launchとkinova用launchを同時に立ち上げた状態で、kinova-interface以下のroseus関数を呼ぶことが出来た
(send *ri* :angle-vector (send *kinova* :reset-pose))
(send *ri* :state :potentio-vector)
  • kinova-interfaceとspot-interfaceの両方を継承したspot-kinva-interfaceみたいなのを作り、1つのroseusプログラムからspotとkinovaを同時に動かす。

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jun 24, 2021

To use the latest kortex_ros (version 2.3.0), we may need to update the firmware of your robot.
We got similar error.
Kinovarobotics/ros_kortex#157

In order to use JSK's kinova now, we need to checkout kortex_ros (e.g. 330c55bce8c3d463cca2492b3e0c89204f235640)

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jun 24, 2021

今日出来たこと

  • spot + kinovaのEuslispモデルが完成した(URDFレベルで統合しており、:reset-poseや:inverse-kinematics、IRTViewerなどが使える状態)
  • spotのroslaunchとkinovaのroslaunchを立ち上げて、実機の状態をRVizに反映させることが出来た。
  • (send ri :state :potentio-vector)でspot+kinovaの実機の状態をeuslispに反映させることが出来た

作業ブランチ
https://github.com/708yamaguchi/jsk_robot/tree/spot-kinova

残っていること

  • (send ri :cmd-vel)で、spotに動く指令を送る(euslispのforwardがうまくできていないので、勉強する)
  • (send ri :angle-vector)でkinovaの腕を動かす指令を送る
  • spot固有の関数(:standなど)を呼べるようにする
  • kinova固有の関数(:start-graspなど)を呼べるようにする
  • .rosinstallなど、ワークスペースのビルド方法をREADMEに書く

output

@tkmtnt7000
Copy link
Member

できたこと

  • ワークスペースのビルド方法をREADMEにまとめる

以下は実機で確認したこと

  • (send ri :stand), (send ri :sit)でspotを立たせる、座らせる
  • (send ri :go-velocity) でspotに動く指令を送る
  • (send ri :start-grasp), (send ri :stop-grasp)でkinovaのグリッパを操作する

できなかったこと

  • (send ri :state :power-state-motor-power-state)でpower-onしているかどうかの確認

次に実機で確認すること

  • (send ri :angle-vector)でkinovaの腕を動かす指令を送る

残っていること

  • (send ri :cmd-vel)で、spotに動く指令を送る(euslispのforwardがうまくできていないので、勉強する)
  • (send ri :angle-vector)でkinovaの腕を動かす指令を送る
  • spot固有の関数(:standなど)を呼べるようにする
  • kinova固有の関数(:start-graspなど)を呼べるようにする
  • .rosinstallなど、ワークスペースのビルド方法をREADMEに書く

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jun 25, 2021

できたこと

  • (send ri :cmd-vel)で、spotに動く指令を送る
  • (send ri :angle-vector)でkinovaの腕を動かす指令を送る
  • spot固有の関数(:standなど)を呼べるようにする
  • kinova固有の関数(:start-graspなど)を呼べるようにする

できていないこと

  • (send ri :wait-interpolation)をする(多分、spot-kinova-interfaceとkinova-interfaceの両方で、同じネームスペースのcontroller-action-clientが立ち上がっていることが原因)
  • (send ri :state :power-state-motor-power-state)でpower-onしているかどうかの確認

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jun 28, 2021

これまでのTODOは全部解消できたと思っていて、その実験として以下のプログラムを動かしてみました。
このブランチこのcommitで実験しています。
あとから気づきましたが、(send ri :stand)は、:wait tのほうがいいかもしれなかったです

roscd spotkinovaeus

roseus spotkinova-interface.l
(spot-kinova-init)
(when (send *ri* :state :power-state-motor-power-state)
  (send *ri* :angle-vector (send *spot-kinova* :init-pose) 5000)
  (send *ri* :wait-interpolation)
  (send *ri* :stand)
  (unix:sleep 1)
  (send *ri* :go-velocity 0.2 0 0 1000)
  (unix:sleep 1)
  (send *ri* :sit)
  (send *ri* :state :potentio-vector :wait-until-update t))
->
#f(5.83414 31.0191 -82.4616 4.40702 47.659 -90.2535 5.66504 40.412 -85.7916 4.69648 47.4369 -90.7098 -0.00293 -0.165863 0.162064 -0.104492 0.045563 -0.000641) ;; :sitにwaitを入れていなかったせいで、座る途中の姿勢になっています。
spot_kinova_interface_test.mp4

今気づいているTODOは以下のとおりですが、塚本くんは番犬デモを作り始めてもらえると助かります。

  • MoveIt!用のcollision objectを追加
  • IKを解くときにkinova armの姿勢基準になっているのを、spotの/base_link基準に直す
  • (send ri :sit)するときの衝撃で、kinovaの姿勢によってはサーボの上限を超えたトルクが関節にかかり、kinovaのシステムが落ちる。:sitする際の良いkinova姿勢を探す必要あり。

@tkmtnt7000
Copy link
Member

@708yamaguchi
ありがとうございます。番犬デモとの組み合わせを始めたところです。

(send ri :sit)するときの衝撃で、kinovaの姿勢によってはサーボの上限を超えたトルクが関節にかかり、kinovaのシステムが落ちる。:sitする際の良いkinova姿勢を探す必要あり

spotと*ri*を共通化する前に、:sitする際の良さそうな姿勢の候補をいくつか実験していました。
kinova-rest-pose <= コメントアウトされていないものが現状で良さそうと考えられる姿勢で、以下の写真のようになります。

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jun 28, 2021

なるほど、ありがとう。

ではこの姿勢をspotkinovaeus/spot_gen3_lite_gen3_lite_2f.yamlに書いて、

(send *spot-kinova* :kinova-rest-pose)

から呼べるようにしておきました。

また、MoveIt!も使えるようになったはずです。

@708yamaguchi
Copy link
Member Author

spot + kinovaのIKについて相談があります。

@sktometometo
spot + kinovaのロボットクラスに対して:inverse-kinematicsを解くと、ボディ座標系(赤い座標系)を原点として、手先(緑の座標系)を動かします。spotの各脚のend-coordsも取得できて、青い座標系で表しています。
物体を操作するときはspotの足元の座標系(ボディ座標系から地面に垂線をおろしたような座標系)からIKの対象位置姿勢を指定するのがプログラム的に分かりやすいかなと思うのですが、ボディ座標系から見た足元座標系の位置姿勢を計算するような関数はありますか?
spotが体をひねらないと仮定すると、四脚を結ぶ平面とボディ座標系の原点の間の距離を計算してボディ座標系のz成分から引けば足元座標系になる気はしますが、ちょっと雑すぎるかなと思っています。

@Naoki-Hiraoka
参考にしたいのですが、脚のあるHRP2がIKを解くときはどこの座標系を基準にしていますか?
また、HRP2のボディ座標系と足元座標系の位置関係を計算しているようなプログラムがあれば教えてほしいです。

よろしくお願いします。

spot_kinova_coords

@sktometometo
Copy link
Contributor

@708yamaguchi spoteus使ってないので関数があるかどうかは知らないけど、今いるgroundを示すframe自体はgpeというのがtfで出ているはず。見ている感じbodyからgroundにおろした点にodomと同じ姿勢のtfになっているっぽい.
gpeの位置にbodyの姿勢のframe出せばfetchとかPR2でいうところのbase_linkにはなると思う.
https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html

@tkmtnt7000
Copy link
Member

spotとkinovaを両方継承した*ri*インスタンスを使って番犬デモのコードを変更せずに番犬デモを実行できました。

IMG_1124.MOV

@708yamaguchi
Copy link
Member Author

@sktometometo

遅くなりましたが、ありがとう。
足元の座標系を取るためにはgpeというtfを取る必要があって、ROSを介する必要があるっていうことだよね。
できればeuslispの中だけで完結させたかったですが、仕方ないのかなという理解をしました。

@Naoki-Hiraoka
Copy link
Contributor

参考にしたいのですが、脚のあるHRP2がIKを解くときはどこの座標系を基準にしていますか?

脚で移動するロボットは、地面に接しているリンク以外は動いてしまい、また、地面に接するリンクも変化するので、ロボットのリンクの座標を基準にするということは難しいです。同じ場所に立ったまま手先を軽く動かす程度であれば、腕以外のリンクはほとんど動かさないので腰のリンクを基準にすることができ(send *robot* :rarm :inverse-kinematics)が使えます。足元座標系が用いられることもありますが、平らな地面の上で立ってマニピュレーションしたり倒立振子制御したりしたりする場合には有効ですが、片足立ちで作業する場合・段差に片足を乗せている場合・斜面に立っている場合・足以外のリンクで体を支える場合・足の接触を切り替えながらマニピュレーションする場合などでの拡張性に難があります。

そのため、ロボット体内に基準座標系は設けずにworld座標系を基準にし、適当なリンクをrootlinkとして仮想的に6自由度の関節を取り付けてworld座標系と結びつける、:fullbody-inverse-kinematicsを用いることが多いです。

また、HRP2のボディ座標系と足元座標系の位置関係を計算しているようなプログラムがあれば教えてほしいです。

以下になります。しかし、robot-modelクラスの関数は腕と脚が2本ずつあるヒューマノイドが2本脚で立つことを前提にした関数が多いので、今回のケースではうまく動かないと思います。

; SLIME 2.20
irteusgl> (load "package://euslisp/jskeus/irteus/demo/sample-robot-model.l")
t
irteusgl> (setq *robot* (instance sample-robot :init))
;; bad stream buffer, 0x55b2a76baeb0
#<sample-robot #X55b2a778b898 sample-robot  0.0 0.0 0.0 / 0.0 0.0 0.0>
irteusgl> (objects *robot*)
;; (make-irtviewer) executed
(#<sample-robot #X55b2a778b898 sample-robot  0.0 0.0 0.0 / 0.0 0.0 0.0>)
irteusgl> (send (send *robot* :foot-midcoords) :draw-on :flush t) ;;足元座標系
1
irteusgl> (send *robot* :fix-leg-to-coords (make-coords)) ;;足元座標系の位置が(make-coords)と一致するようにロボットを移動
#<coordinates #X55b2a699dd30  0.0 0.0 712.5 / 0.0 0.0 0.0>
irteusgl> (send *irtviewer* :draw-objects)
1

@708yamaguchi
Copy link
Member Author

@Naoki-Hiraoka
忙しい中ありがとうございます。
脚ロボットは脚が色々な状態になりうるので、足元の座標系を基準にIKを解くのではなく、:fullbody-inverse-kinematicsを使うのが良いということですね。
また、:fullbody-inverse-kinematics内では仮想ジョイントを動かしているというのは初めて知りました。勉強になります。

@tkmtnt7000
spot+kinovaで:fullbody-inverse-kinematicsのサンプルコードを書いてみました。
以下の手順でspotの体も使ったリーチング動作が出来ると思います。

  1. 画像認識などで、手先の移動先を計算する
  2. :fullbody-inverse-kinematics後の(send *spot-kinova* :worldcoords)をみて、それに合うようにspotの体をひねる
    • /spot/body_poseというトピックがあるので、(send *ri* :body-pose)を拡張すればspotの体の位置姿勢の指定できるはず。
    • spotが体をひねることのできる限界に合わせて、:fullbody-inverse-kinematicsの:minや:max引数を変えると良いです。
  3. kinovaアームを動かす。

サンプルコードの結果です。
赤い矢印が世界座標系、緑の矢印が腕および各脚を動かす先の座標系です。

reset-pose ik-pose

サンプルコード

roseus fullbody-ik.l
(reset-pose)
(visualize)
(ik-pose)
(visualize)

fullbody-ik.l

(load "package://spotkinovaeus/spotkinova.l")

(unless (boundp '*spot-kinova*)
  (spot-kinova :type :gen3_lite_gen3_lite_2f)
  (objects (list *spot-kinova*)))

(setq *move-target*
      (list (send *spot-kinova* :head :end-coords)
            (send *spot-kinova* :larm :end-coords)
            (send *spot-kinova* :rarm :end-coords)
            (send *spot-kinova* :lleg :end-coords)
            (send *spot-kinova* :rleg :end-coords)))
(setq *move-coords*
      (list (send (send (send *spot-kinova* :head :end-coords) :copy-worldcoords) :translate #f(200 0 150) :world)
            (send (send (send *spot-kinova* :larm :end-coords) :copy-worldcoords) :translate #f(0 0 0) :world)
            (send (send (send *spot-kinova* :rarm :end-coords) :copy-worldcoords) :translate #f(0 0 0) :world)
            (send (send (send *spot-kinova* :lleg :end-coords) :copy-worldcoords) :translate #f(0 0 0) :world)
            (send (send (send *spot-kinova* :rleg :end-coords) :copy-worldcoords) :translate #f(0 0 0) :world)))

(defun reset-pose ()
  ;; Reset position and pose
  (send *spot-kinova* :move-to (make-coords) :world)
  (send *spot-kinova* :reset-pose))

(defun ik-pose ()
  (reset-pose)
  ;; Sample IK pose
  (send *spot-kinova*
        :fullbody-inverse-kinematics *move-coords*
        :move-target *move-target*
        :link-list (mapcar #'(lambda (limb) (send *spot-kinova* :link-list (send limb :parent))) *move-target*)
        :min (float-vector -500 -500 -500 -20 -20 -10)
        :max (float-vector 500 500 500 -20 -20 -10)
        :rotation-axis (list t nil nil nil nil)
        :target-centroid-pos nil))

(defun visualize ()
  ;; Visualize target coords and origin coords
  (send *irtviewer* :viewer :viewing :look #f(35 6280 727) #f(257 -83 58) #f(0 0 1))
  (send *irtviewer* :draw-objects :flush t)
  (mapcar #'(lambda (x) (send x :draw-on :flush t :width 3 :size 200 :color #f(0 1 0)))
          *move-coords*)
  (send (make-coords) :draw-on :flush t :width 3 :size 200 :color #f(1 0 0))
  ;; Print robot position and pose
  (format t "Robot worldcoords: ~A~%" (send *spot-kinova* :worldcoords)))

@tkmtnt7000
Copy link
Member

tkmtnt7000 commented Jun 30, 2021

番犬デモ+飴ちゃん渡すデモの動画です.
ファイルサイズが大きいのでGoogle Driveのリンクです.
https://drive.google.com/file/d/1RiAMPH24cUslglCBzB_gUnR1clsBdBlI/view?usp=sharing
人が見えた時の動作では、吠える or 飴を渡す をランダムに行うようにしています. 

spotとkinovaを両方継承したriインスタンスを使って番犬デモのコードを変更せずに番犬デモを実行できました。

@tkmtnt7000
Copy link
Member

@708yamaguchi
お忙しい中ありがとうございます。
:fullbody-inverse-kinematicsも今後実機で試してみたいと思います。

@tongtybj
Copy link

tongtybj commented Jul 1, 2021

@tkmtnt7000

面白いデモですね。
ちなみに、塚本君が首を振ってから、アームが動き出すまで、かなりの待ち時間があったけど、これは何をしているのかな?

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Jul 1, 2021

@tongtybj

ちなみに、塚本君が首を振ってから、アームが動き出すまで、かなりの待ち時間があったけど、これは何をしているのかな?

ちょっと複雑なのですが、以下が理由になります。

  1. そもそもkinovaアームは1msおきに補間されたtrajectoryしか受け付けないという仕様があります
  2. (send *ri* :angle-vector)を実行すると内部でMoveIt!のサービスを呼び、現在のロボット関節角度から目標関節角度までのtrajectoryをMoveIt!が計算してくれます。
  3. MoveIt!のデフォルト設定では1msおきのtrajectoryを返してくれるのですが、(send *ri* :angle-vector)で指定した時間でロボットが動作するようにroseus内でtrajectory間の時間を引き伸ばしているため、1msおきのtrajectoryではなくなってしまいます。
  4. この引き伸ばされたtrajectoryを1msおきのtrajectoryに変換するため、roseusの中でminjerk-interpolationを使って行っているのですが、これにすごく時間がかかっています。

例えば、(send *ri* :angle-vector angle-vector 10000)のように10秒でロボットアームを動かしたいとして、MoveIt!が計算した結果5秒・1msおき・5000点のtrajectoryが返ってきたとします。
それをroseusのなかで10秒に引き伸ばすと、10秒・2msおき・5000点のtrajectoryになります。
これをさらに1msおきになるようにroseusの中でminjerk-interpolationして、10秒・1msおき・10000点のtrajectoryにします。
このように、5000点 -> 10000点くらいの多くの点をinterpolationするとすごく長い時間がかかります。
:angle-vector-sequenceでは、この現象はより顕著に発生するので、:minjerk-interpolationの計算量が分かっていないのですが、O(n)以上なのかなぁと感じています。

対策としては、robot-moveit.lの:trajectory-filterの中で、MoveIt!から返ってきたtrajectoryを時間的に間引けるようなオプションをつけることかな、と考えています。
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/dd7305cb42fa1e8ddac4024f05ca18773b9de9c6/pr2eus_moveit/euslisp/robot-moveit.l#L645
ただ、間引くほどその後のminjerk-interpolationの計算は速くなると思うのですが、間引きすぎるとせっかくの障害物回避が無駄になってしまう可能性があるので、そこは要パラメータチューニングかなと思います。

@k-okada
Copy link
Member

k-okada commented Jul 1, 2021

@708yamaguchi せっかく @tongtybj が,これまでのJSKにはないレスポンスの良いデモにしてくれたので,そこは生かしたいですね.そうでないとインタラクションと言いづらいし...
まずは現状確認.今回のケースでよいので,

  1. MoveIt の計算に何秒かかっているか
  2. minjerk-interpolation に何秒かかっているか?
  3. 今の動作シーケンスが何秒で :angle-vector av :fastestみたいなことをしたら,何秒のシーケンスになるか?
  4. 今の動作シーケンスは何点のデータで,これが,minjerk-interpolation で何点のデータになっているのか.

で,4)の計算を再現できる最小限のサンプルプログラムを作ってPRに送ってみましょう.

@tkmtnt7000
Copy link
Member

tkmtnt7000 commented Jul 1, 2021

毎回kinova-rest-poseからreset-poseにするときで調べています。
bench関数を使って調べました。

  • :angle-vectorを20sの時間指定で送ったとき
  1. Moveitの計算にかかる時間:1.0568s
  2. minjerk-interpolationにかかる時間:24.953s
  3. :angle-vectorから抜けるためにかかる時間:27.576s
  4. 動作シーケンスのtrajectoryの数:11870(original) -> 20001(minjerk-interpolation)
[ INFO] [1625130994.532467244]: ;; Planned Trajectory Total Time  11.869 [sec]
[ INFO] [1625130994.641349262]: ;; Scaled Trajectory Total Time 20.000(20.000) [sec]
[ INFO] [1625130994.642009575]: ;; generated 11870 points for 20.0 sec using [arm] group
  • :angle-vector:fast(最短時間)で送ったとき
  1. Moveitの計算にかかる時間:1.18455s
  2. minjerk-interpolationにかかる時間:19.4994s
  3. :angle-vectorから抜けるためにかかる時間:21.7433s
  4. 動作シーケンスのtrajectoryの数:11869(original) -> 11869(minjerk-interpolation)
[ INFO] [1625131625.620980109]: ;; Planned Trajectory Total Time  11.868 [sec]
[ INFO] [1625131625.734888977]: ;; Scaled Trajectory Total Time 11.868(11.868) [sec]
[ INFO] [1625131625.735844003]: ;; generated 11869 points for 11.868 sec using [arm] group

@tkmtnt7000
Copy link
Member

jsk_pr2eus#462を使った結果がこちらです。
moveitから返ってきたtrajectoryを300msおきになるように間引いています。
条件は上と同様でkinova-rest-pose -> reset-pose, 時間はbench関数を使って調べています。

  • :angle-vectorを20sの時間指定で送ったとき
  1. Moveitの計算にかかる時間:1.11384s
  2. minjerk-interpolationにかかる時間:0.81394s
  3. :angle-vectorから抜けるためにかかる時間:3.51287s
  4. 動作シーケンスのtrajectoryの数:11871(original) -> 67 (time-step適用後) -> 19906(minjerk-interpolation)
[ INFO] [1625132102.802886164]: ;; Planned Trajectory Total Time  11.677 [sec]
[ INFO] [1625132102.935436464]: ;; Trajectory points are resized: 11678 -> 67
[ INFO] [1625132102.935495818]: ;; Scaled Trajectory Total Time 19.896(20.000) [sec]
  • :angle-vector:fast(最短時間)で送ったとき
  1. Moveitの計算にかかる時間:1.14827s
  2. minjerk-interpolationにかかる時間:0.491674s
  3. :angle-vectorから抜けるためにかかる時間:2.47714s
  4. 動作シーケンスのtrajectoryの数:11868(original) -> 40 (time-step適用後) -> 11720(minjerk-interpolation)
[ INFO] [1625132277.881351292]: ;; Planned Trajectory Total Time  11.867 [sec]
[ INFO] [1625132278.015898071]: ;; Trajectory points are resized: 11868 -> 40
[ INFO] [1625132278.015954928]: ;; Scaled Trajectory Total Time 11.719(11.867) [sec]

@k-okada k-okada merged commit 017ebb5 into jsk-ros-pkg:master Jul 16, 2021
@708yamaguchi 708yamaguchi deleted the kinova-gen3 branch November 24, 2021 07:28
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

7 participants