[2024年度ゼミ] Tsubaki's work

jsk_2024_10_semi/demo_first.l

@@ -0,0 +1,5 @@
+(require "package://pr2eus/pr2.l")
+(if (not (boundp '*pr2*)) (setq *pr2* (pr2)))
+(objects (list *pr2*))
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)

jsk_2024_10_semi/demo_solve-ik.l

@@ -0,0 +1,45 @@
+#!/usr/bin/env roseus
+
+;; PR2のモデルを読み込む
+(require "package://pr2eus/pr2.l")
+
+;; PR2のインスタンスを作成
+(if (not (boundp '*pr2*)) (setq *pr2* (pr2)))
+
+;; 1辺200mmの立方体を出現させる
+(setq *cube* (make-cube 200 200 200)) ;; NOTE 実際はハンガーなど，デモに使用するモデルを作って出現させるとよいかもしれない
+;; 立方体を(400, 0, 800)移動
+(send *cube* :translate (float-vector 400 0 800)) ;; NOTE 画像認識結果から，ハンガーの位置を取得して移動するとよいかもしれない
+
+;; 立方体の左側面に座標系を設定
+(send *cube* :put :left-coords
+      (make-cascoords
+       :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 0 100 0))
+       :parent *cube*))
+
+;; ビューワを表示
+(objects (list *pr2* *cube*))
+
+;; 左腕について，立方体の両側面に向かって逆運動学を解く
+(send *pr2* :larm :inverse-kinematics
+      (send (send *cube* :get :left-coords) :copy-worldcoords)
+      :rotation-axis :z)
+
+;; PR2の左腕で立方体を持ち上げて前に突き出す
+;; PR2の左腕と立方体の左側を連結する．
+(send *pr2* :larm :end-coords :assoc *cube*)
+;; 立方体が現在の位置から相対で(100, 0, 200)に移動するように逆運動学を解く
+(send *pr2*
+      :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords)
+                                      :translate (float-vector 100.0 0.0 100.0))
+      :move-target (send *cube* :get :left-coords)
+      :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent))
+      :rotation-axis t
+      :debug-view t)
+
+;;(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
+;;(send *ri* :wait-interpolation)
+
+
+;; NOTE 両腕で持って渡すみたいな動作は，結構難しい? ref: https://github.com/euslisp/jskeus/issues/582
+;; :assocは閉リンクを形成できないので，1つの箱に両腕を連結することはできない?

jsk_2024_10_semi/first_motion.l

@@ -0,0 +1,248 @@
+;;Okada-sensei seminar practice task ;;last editted oct 17 ;; Michitoshi TSUBAKI
+;;building specific motions using euslisp command
+
+(require "package://pr2eus/pr2.l")  ;;import pr2 package
+(require "package://pr2eus/pr2-utils.l")
+(require "package://pr2eus/pr2-interface.l") 
+(if (not (boundp '*pr2*)) (pr2-init)) ;; (setq *pr2* (pr2)))  ;; (pr2) is alternated by  *pr2*
+;;boudp 'xx : xxという変数に値が代入されているか
+;;setq xx yy : xxとう変数にyyという値を代入する
+
+(send *pr2* :reset-pose) ;; 初期姿勢
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :stop-grasp :arms)
+(send *ri* :wait-interpolation)
+
+
+(objects (list *pr2*))
+
+;;徐々に腕（肩）を広げる(Oct 17)
+(send *pr2* :larm :shoulder-p :joint-angle 74)
+(send *pr2* :larm :shoulder-p :joint-angle 74)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+(send *pr2* :larm :shoulder-p :joint-angle 54)
+(send *pr2* :rarm :shoulder-p :joint-angle 54)
+
+(send *pr2* :larm :gripper :joint-angle 30)
+(send *pr2* :rarm :gripper :joint-angle 30)
+
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :start-grasp :arms)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 34)
+(send *pr2* :rarm :shoulder-p :joint-angle 34)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 20)
+(send *pr2* :rarm :shoulder-p :joint-angle 20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle 0)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+;;徐々に型を下げる(Oct 22)
+(send *pr2* :rarm :shoulder-r :joint-angle -80)
+(send *pr2* :larm :shoulder-r :joint-angle 80)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :rarm :shoulder-r :joint-angle -95)
+(send *pr2* :larm :shoulder-r :joint-angle 95)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :rarm :shoulder-r :joint-angle -110)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+
+(send *pr2* :rarm :shoulder-r :joint-angle -130)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+
+
+(send *pr2* :larm :elbow-p :joint-angle -110)
+(send *pr2* :rarm :elbow-p :joint-angle -100)
+(send *pr2* :rarm :shoulder-r :joint-angle -130)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+(unix:sleep 1)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -130)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :elbow-p :joint-angle -100)
+(send *pr2* :rarm :elbow-p :joint-angle -70)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 30)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -100)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :elbow-p :joint-angle -100)
+(send *pr2* :rarm :elbow-p :joint-angle -70)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 30)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -80)
+(send *pr2* :larm :shoulder-r :joint-angle 80)
+(send *pr2* :larm :elbow-p :joint-angle -110)
+(send *pr2* :rarm :elbow-p :joint-angle -100)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 30)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -100)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 30)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+;;fixing now last edited Oct 22 by Michitoshi TSUBAKI
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -100)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 0)
+(send *pr2* :rarm :wrist-r :joint-angle 0)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -120)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 90)
+(send *pr2* :rarm :wrist-r :joint-angle 90)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :torso :waist-z :joint-angle 200)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -120)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 0)
+(send *pr2* :rarm :wrist-r :joint-angle 0)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :torso :waist-z :joint-angle 300)
+
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+