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)