diff --git a/.rosinstall.melodic b/.rosinstall.melodic index c97f3fbc..821cc682 100644 --- a/.rosinstall.melodic +++ b/.rosinstall.melodic @@ -15,10 +15,14 @@ local-name: kobuki_desktop uri: https://github.com/yujinrobot/kobuki_desktop.git version: 58459065087c7f9786cc2d1dc25a059280b5ca47 +# - git: +# local-name: turtlebot +# uri: https://github.com/turtlebot/turtlebot.git +# version: feb176b10f13bcdc517e7f305c39327dac6388f0 # this has not been testd on real robot, fix xtion visual? or back to 2019 version? - git: local-name: turtlebot - uri: https://github.com/turtlebot/turtlebot.git - version: feb176b10f13bcdc517e7f305c39327dac6388f0 # this has not been testd on real robot, fix xtion visual? or back to 2019 version? + uri: https://github.com/kindsenior/turtlebot.git + version: use-freenect-feb176b # fix for launching 3dsensor.launch in students' PC until https://github.com/turtlebot/turtlebot/pull/294 will be merged - git: local-name: turtlebot_simulator uri: https://github.com/turtlebot/turtlebot_simulator.git diff --git a/cart_humanoid/euslisp/cart_humanoid-interface.l b/cart_humanoid/euslisp/cart_humanoid-interface.l index 232991c8..d675d9e2 100644 --- a/cart_humanoid/euslisp/cart_humanoid-interface.l +++ b/cart_humanoid/euslisp/cart_humanoid-interface.l @@ -124,8 +124,10 @@ ) - - +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; ヒューマノイドロボットJAXON用初期化関数 +;; robot-interface (*ri*) とモデル (*cart_humanoid*)を生成する +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (defun cart_humanoid-init (&rest args) (if (not (boundp '*ri*)) (setq *ri* (instance* cart_humanoid-interface :init args))) @@ -152,3 +154,5 @@ (t (warn ";; you should load robot-file before loading this file!") )) + +(warn ";; (cart_humanoid-init) ;; for initialize and generating *cart_humanoid* and *ri*~%") diff --git a/dxl_armed_turtlebot/euslisp/keyboard-ik-sample.l b/dxl_armed_turtlebot/euslisp/keyboard-ik-sample.l new file mode 100644 index 00000000..992fcc9d --- /dev/null +++ b/dxl_armed_turtlebot/euslisp/keyboard-ik-sample.l @@ -0,0 +1,73 @@ +;;; +;;; 移動台車モデルの逆運動学を解いてarmや台車を操縦するサンプル +;;; +(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") + +(dxl-armed-turtlebot-init) + +#|********************************************* + キーボード入力によって目標座標を少しずつ動かすサンプル +***********************************************|# +(warn "(ik-demo0)~%") +(defun ik-demo0 + (&key (robot *dxl-armed-turtlebot*)) + ;;逆運動学が解きやすい初期姿勢に変更 + (warn ";; move to reset-pose~%") + (send robot :reset-pose) + (when (boundp '*ri*) + (send *ri* :angle-vector (send robot :angle-vector) 5000) + (send *ri* :wait-interpolation)) + (objects (list robot)) + + ;; + ;;'e'を押すまで続ける + (warn ";; if stop, then enter e~%") + (warn ";; h:left, j:down, k:up, l:right, f:forward, b:back~%") + (let (w goal-endcoords ll) + ;;もし腕しか使わない場合はlinklistをあらかじめ用意しておく + ;;目標座標を作成する(デフォルトは手先位置と同じにする) + (setq goal-endcoords + (make-cascoords :pos (send (send robot :arm :end-coords :copy-worldcoords) :worldpos))) + ;;ループを回す + (while t + (setq w (read-line)) ;;文字を取得 + ;;文字によって操作を変える + (cond + ((equal w "e") + (return-from nil)) ;;loopから抜けて終了 + ((equal w "h") ;;左へ動かす + (send goal-endcoords :locate #f(0 20 0) :local)) + ((equal w "j") ;;下へ動かす + (send goal-endcoords :locate #f(0 0 -20) :local)) + ((equal w "k") ;;上へ動かす + (send goal-endcoords :locate #f(0 0 20) :local)) + ((equal w "l") ;;右へ動かす + (send goal-endcoords :locate #f(0 -20 0) :local)) + ((equal w "f") ;;前へ動かす + (send goal-endcoords :locate #f( 20 0 0) :local)) + ((equal w "b") ;;後へ動かす + (send goal-endcoords :locate #f(-20 0 0) :local)) + ((not w)) ;;何も入れられなければ何もしない + (t + (warn ";; no such command~%") + (warn ";; if stop, then enter e~%") + (warn ";; h:left, j:down, k:up, l:right, f:forward, b:back~%") + )) + + ;;目標値goal-endcoordsに向かって逆運動学を解く. + ;; inverse-kinematicsという逆運動学を解くmethodを呼び出す. + (send *dxl-armed-turtlebot* :inverse-kinematics goal-endcoords :rotation-axis nil) + + (send *irtviewer* :objects (list robot goal-endcoords)) + (send *irtviewer* :draw-objects) + + ;;angle-vectorで逆運動学を解いた姿勢に移行する + (when (boundp '*ri*) + (send *ri* :angle-vector (send robot :angle-vector) 500) + (send *ri* :wait-interpolation) + ) + )) + (warn ";; finished~%") + t) + +(ik-demo0)