From d25e6f273405c9ddebbe4abfd8fcd02a331944bf Mon Sep 17 00:00:00 2001 From: Kodai Yamada Date: Wed, 16 Nov 2022 19:02:52 +0900 Subject: [PATCH 1/3] add Yamada Project --- .../euslisp/Kodai-Yamada/fishing_pose_spot.l | 65 +++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l diff --git a/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l b/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l new file mode 100644 index 0000000000..10b0950f0e --- /dev/null +++ b/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l @@ -0,0 +1,65 @@ + (load "package://spoteus/spot-interface.l") + (spot-init) + (objects (list *spot*)) + +(send *spot* :reset-pose) +(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +(send *ri* :wait-interpolation) + +;start position ver.1 +;(send *spot* :angle-vector #f(0.579579 51.0722 -90.3575 -0.196024 51.0818 -90.3335 0.430053 51.2779 -90.7333 -0.059803 51.1679 -90.4999 -7.3573 -141.649 7.83607 8.55783 -38.5174 -8.67582 -1.36665)) +;(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +;(send *ri* :wait-interpolation) + +;start position ver.2 +(send *spot* :angle-vector #f(0.900186 50.8197 -90.3914 -0.238533 50.9587 -90.6933 0.584529 51.1155 -90.9841 0.020179 50.707 -90.6455 0.534681 -143.785 18.4731 -0.344761 -36.7685 96.8463 -0.311272)) +(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +(send *ri* :wait-interpolation) + +;(send *spot* :angle-vector #f(0.225838 45.8552 -91.0167 -0.523653 45.6263 -90.5496 0.083025 45.9016 -91.0757 -0.421746 45.7121 -90.7279 -8.04395 -87.4457 10.5524 8.55208 -38.5085 -8.67215 -1.3227)) +;(send *ri* :angle-vector (send *spot* :angle-vector) 1000) +;(send *ri* :wait-interpolation) + +;end position ver.1 +;; (send *spot* :angle-vector #f(-0.001642 42.3328 -90.2934 -0.746104 41.8198 -89.2853 -0.17725 42.2826 -90.1418 -0.69892 41.8884 -89.4534 -8.08705 -59.1052 13.8305 8.54947 -38.486 -8.63814 -0.376507)) +;; (send *ri* :angle-vector (send *spot* :angle-vector) 500) +;; (send *ri* :wait-interpolation) + +;end position ver.2 +(send *spot* :angle-vector #f(0.8435 42.1846 -90.1341 -0.297538 42.1008 -90.3042 0.444329 42.1165 -90.3753 -0.015944 41.9824 -90.4953 4.08791 -61.5594 21.981 66.0059 -1.56093 24.9492 -89.512)) +(send *ri* :angle-vector (send *spot* :angle-vector) 1000) +(send *ri* :wait-interpolation) + +(dotimes (i 5) + (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) + (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) + + (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) + (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) + + (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) + (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) + + (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) + (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) + + (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) + (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) + + (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) + (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) + + (send *irtviewer* :draw-objects) + ;; (x::window-main-one) + + (send *ri* :angle-vector (send *spot* :angle-vector) 500) + (send *ri* :wait-interpolation) + ) + + From 5085236cde2c8631c6b8a168d6ea9b6217bcf890 Mon Sep 17 00:00:00 2001 From: Kodai Yamada Date: Wed, 14 Dec 2022 19:55:42 +0900 Subject: [PATCH 2/3] 2022/12/14 --- jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l b/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l index 10b0950f0e..26fdfb0c77 100644 --- a/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l +++ b/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l @@ -1,7 +1,7 @@ (load "package://spoteus/spot-interface.l") (spot-init) (objects (list *spot*)) - +;; testtest (send *spot* :reset-pose) (send *ri* :angle-vector (send *spot* :angle-vector) 5000) (send *ri* :wait-interpolation) From 13013ba065888853fb0e14e0eca8df43928f1e53 Mon Sep 17 00:00:00 2001 From: Kodai Yamada Date: Wed, 14 Dec 2022 20:01:43 +0900 Subject: [PATCH 3/3] 2022/12/14 --- .../euslisp/Kodai-Yamada/fishing_pose_spot.l | 41 +++++++++++++------ 1 file changed, 28 insertions(+), 13 deletions(-) diff --git a/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l b/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l index 26fdfb0c77..47fb3944c9 100644 --- a/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l +++ b/jsk_2022_10_semi/euslisp/Kodai-Yamada/fishing_pose_spot.l @@ -1,7 +1,7 @@ (load "package://spoteus/spot-interface.l") (spot-init) (objects (list *spot*)) -;; testtest + (send *spot* :reset-pose) (send *ri* :angle-vector (send *spot* :angle-vector) 5000) (send *ri* :wait-interpolation) @@ -12,13 +12,23 @@ ;(send *ri* :wait-interpolation) ;start position ver.2 -(send *spot* :angle-vector #f(0.900186 50.8197 -90.3914 -0.238533 50.9587 -90.6933 0.584529 51.1155 -90.9841 0.020179 50.707 -90.6455 0.534681 -143.785 18.4731 -0.344761 -36.7685 96.8463 -0.311272)) +;; (send *spot* :angle-vector #f(0.900186 50.8197 -90.3914 -0.238533 50.9587 -90.6933 0.584529 51.1155 -90.9841 0.020179 50.707 -90.6455 0.534681 -143.785 18.4731 -0.344761 -36.7685 96.8463 -0.311272)) +;; (send *ri* :angle-vector (send *spot* :angle-vector) 5000) +;; (send *ri* :wait-interpolation) + +;start position ver.3 +(send *spot* :angle-vector #f(-0.19416 47.9212 -91.3344 0.221026 53.7194 -91.2161 -3.88364 54.1232 -93.1145 3.72919 48.8215 -92.7384 -0.483659 -131.043 7.24327 -3.75731 -48.426 97.3056 -0.355218)) (send *ri* :angle-vector (send *spot* :angle-vector) 5000) (send *ri* :wait-interpolation) -;(send *spot* :angle-vector #f(0.225838 45.8552 -91.0167 -0.523653 45.6263 -90.5496 0.083025 45.9016 -91.0757 -0.421746 45.7121 -90.7279 -8.04395 -87.4457 10.5524 8.55208 -38.5085 -8.67215 -1.3227)) -;(send *ri* :angle-vector (send *spot* :angle-vector) 1000) -;(send *ri* :wait-interpolation) + +;grab fishing rod +(send *ri* :gripper-open) +(send *ri* :wait-interpolation) +(unix:sleep 5) +(send *ri* :gripper-close) +(send *ri* :wait-interpolation) + ;end position ver.1 ;; (send *spot* :angle-vector #f(-0.001642 42.3328 -90.2934 -0.746104 41.8198 -89.2853 -0.17725 42.2826 -90.1418 -0.69892 41.8884 -89.4534 -8.08705 -59.1052 13.8305 8.54947 -38.486 -8.63814 -0.376507)) @@ -26,36 +36,41 @@ ;; (send *ri* :wait-interpolation) ;end position ver.2 -(send *spot* :angle-vector #f(0.8435 42.1846 -90.1341 -0.297538 42.1008 -90.3042 0.444329 42.1165 -90.3753 -0.015944 41.9824 -90.4953 4.08791 -61.5594 21.981 66.0059 -1.56093 24.9492 -89.512)) +;; (send *spot* :angle-vector #f(0.8435 42.1846 -90.1341 -0.297538 42.1008 -90.3042 0.444329 42.1165 -90.3753 -0.015944 41.9824 -90.4953 4.08791 -61.5594 21.981 66.0059 -1.56093 24.9492 -89.512)) +;; (send *ri* :angle-vector (send *spot* :angle-vector) 1000) +;; (send *ri* :wait-interpolation) + +;end position ver.3 +(send *spot* :angle-vector #f(-0.219186 43.0658 -90.6477 0.221758 49.2979 -91.2719 -3.85436 49.5271 -92.9653 3.72034 43.9016 -91.9354 -7.38435 -97.7657 30.7545 10.1184 -52.493 89.5932 -0.356591)) (send *ri* :angle-vector (send *spot* :angle-vector) 1000) (send *ri* :wait-interpolation) (dotimes (i 5) (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) - (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send hand-coord-copy :translate (float-vector 0 (* 50 (expt -1 i)) 0) :world) (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) - (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send hand-coord-copy :translate (float-vector 0 (* 50 (expt -1 i)) 0) :world) (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) - (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send hand-coord-copy :translate (float-vector 0 (* 50 (expt -1 i)) 0) :world) (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) - (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send hand-coord-copy :translate (float-vector 0 (* 50 (expt -1 i)) 0) :world) (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) - (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send hand-coord-copy :translate (float-vector 0 (* 50 (expt -1 i)) 0) :world) (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) (setq hand-coord-copy (send (send *spot* :arm :end-coords) :copy-worldcoords)) - (send hand-coord-copy :translate (float-vector 0 (* 75 (expt -1 i)) 0) :world) + (send hand-coord-copy :translate (float-vector 0 (* 50 (expt -1 i)) 0) :world) (send *spot* :arm :inverse-kinematics hand-coord-copy :rotation-axis :z) - (send *irtviewer* :draw-objects) + ;; (send *irtviewer* :draw-objects) ;; (x::window-main-one) (send *ri* :angle-vector (send *spot* :angle-vector) 500)