Merge pull request #430 from kindsenior/enshu2022

enshu2022

.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

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*~%")

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)