diff --git a/jsk_2024_10_semi/README.md b/jsk_2024_10_semi/README.md index acd4388fa..9e2355b5b 100644 --- a/jsk_2024_10_semi/README.md +++ b/jsk_2024_10_semi/README.md @@ -1,3 +1,7 @@ # jsk_2024_10_semi +## 目標 +手術ロボット。縫合タスク。 +## サブ目標 +スポンジを縫う \ No newline at end of file diff --git a/jsk_2024_10_semi/detect_test/niku.jpg b/jsk_2024_10_semi/detect_test/niku.jpg new file mode 100644 index 000000000..5e6be5661 Binary files /dev/null and b/jsk_2024_10_semi/detect_test/niku.jpg differ diff --git a/jsk_2024_10_semi/detect_test/niku_detect_1.py b/jsk_2024_10_semi/detect_test/niku_detect_1.py new file mode 100755 index 000000000..c4c431c97 --- /dev/null +++ b/jsk_2024_10_semi/detect_test/niku_detect_1.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +##切れ目のsegmentation + +##参考: https://qiita.com/ysdyt/items/5972c9520acf6a094d90 +## https://pystyle.info/opencv-distance-transform/#google_vignette + + +#import common pkg in Python +import cv2 as cv +import numpy as np +import matplotlib.pyplot as plt + +#import pkg for connecting to ROS +import rospy +from sensor_msgs.msg import Image, CompressedImage, CameraInfo + + +#define call back function +def callback(msg): + global niku + niku = msg[0] + + + +rospy.init_node('client') +rospy.Subscriber("/kinect_head/rgb/image_raw/compressed", CompressedImage, callback) + +niku = cv.imread("./niku.jpg") +niku = niku[1000: 3200, 500 : 2700] + + +niku_gray = cv.cvtColor(niku, cv.COLOR_BGR2GRAY) + + + +plt.imshow(niku_gray,cmap='gray') +plt.show() + +thresh,bin_img = cv.threshold(niku_gray,0,255,cv.THRESH_BINARY_INV+cv.THRESH_OTSU) +plt.imshow(bin_img,cmap='gray') +plt.show() +print('大津法の二値化によって自動で決定された閾値:',thresh) + +## print(bin_img) #for debug + +kernel = np.ones((3,3),np.uint8) +opening = cv.morphologyEx(bin_img,cv.MORPH_OPEN,kernel,iterations = 2) +plt.imshow(opening,cmap='gray') +plt.show() + +#モルフォロジー演算のDilationを使う +sure_bg = cv.dilate(opening,kernel,iterations=2) +plt.imshow(sure_bg,cmap='gray') +plt.show() + +dist_transform = cv.distanceTransform(opening,cv.DIST_L2,5) +plt.imshow(dist_transform) +plt.show() + +ret, sure_fg = cv.threshold(dist_transform,0.05*dist_transform.max(),255,0) +print('閾値（距離変換で得られた値の最大値×0.5）:',ret) +plt.imshow(sure_fg,cmap='gray') +plt.show() + +sure_fg = np.uint8(sure_fg) +unknown = cv.subtract(sure_bg,sure_fg) +plt.imshow(unknown,cmap='gray') + +# foregroundの1オブジェクトごとにラベル（番号）を振っていく +ret, markers = cv.connectedComponents(sure_fg) +plt.imshow(markers) +plt.show() diff --git a/jsk_2024_10_semi/first_motion.l b/jsk_2024_10_semi/first_motion.l new file mode 100644 index 000000000..122275c07 --- /dev/null +++ b/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) + + + diff --git a/jsk_2024_10_semi/first_task/demo_first.l b/jsk_2024_10_semi/first_task/demo_first.l new file mode 100644 index 000000000..eaad0d502 --- /dev/null +++ b/jsk_2024_10_semi/first_task/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) diff --git a/jsk_2024_10_semi/first_task/homework_uml_1.act b/jsk_2024_10_semi/first_task/homework_uml_1.act new file mode 100644 index 000000000..0f0deb623 --- /dev/null +++ b/jsk_2024_10_semi/first_task/homework_uml_1.act @@ -0,0 +1,42 @@ +@startuml +|医者役| + start + :傷口を早く閉じて，休憩しないと死にそうだ〜: + + :針を通して，ロボットに渡す; + +|手術ロボット| + + :針を受け取る; + +|医者役| + + :「自動縫合モード」スタートさせる; + +|手術ロボット| + + :傷口を認識する（画像認識 segmantation? detection?）; + + repeat + :アームを目標（傷口）に近づける; + :1針縫う; + + + repeat while (傷口が塞がれているか確認する（画像認識）) + :傷口が塞がれているなら，プログラムを停止する; + + + + +|医者役| + :休憩から帰ってきてちゃんと縫えているか確認する; + +|手術ロボット| + :お医者さんに褒められながらCtl+Cを押される（嬉しい？）; + +|医者役| + :糸を結ぶ; + :手術終了; + +end +@enduml \ No newline at end of file diff --git a/jsk_2024_10_semi/first_task/homework_uml_2.act b/jsk_2024_10_semi/first_task/homework_uml_2.act new file mode 100644 index 000000000..0f0deb623 --- /dev/null +++ b/jsk_2024_10_semi/first_task/homework_uml_2.act @@ -0,0 +1,42 @@ +@startuml +|医者役| + start + :傷口を早く閉じて，休憩しないと死にそうだ〜: + + :針を通して，ロボットに渡す; + +|手術ロボット| + + :針を受け取る; + +|医者役| + + :「自動縫合モード」スタートさせる; + +|手術ロボット| + + :傷口を認識する（画像認識 segmantation? detection?）; + + repeat + :アームを目標（傷口）に近づける; + :1針縫う; + + + repeat while (傷口が塞がれているか確認する（画像認識）) + :傷口が塞がれているなら，プログラムを停止する; + + + + +|医者役| + :休憩から帰ってきてちゃんと縫えているか確認する; + +|手術ロボット| + :お医者さんに褒められながらCtl+Cを押される（嬉しい？）; + +|医者役| + :糸を結ぶ; + :手術終了; + +end +@enduml \ No newline at end of file diff --git a/jsk_2024_10_semi/first_task/memo1023.txt b/jsk_2024_10_semi/first_task/memo1023.txt new file mode 100644 index 000000000..d00d0e89d --- /dev/null +++ b/jsk_2024_10_semi/first_task/memo1023.txt @@ -0,0 +1,3783 @@ +mech-user@ki00119:~/semi_ws/src$ roseus +configuring by "/opt/ros/noetic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph gnuplotlib ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x55e070c10680[16374] --> 0x55e0710a0c90[32748] top=3d4b +irtgl irtglc irtviewer +EusLisp 9.29( 1.2.5) for Linux64 created on ip-10-0-1-5(Tue Jun 20 10:34:21 PST 2023) +roseus ;; loading roseus("1.7.5") on euslisp((9.29 ip-10-0-1-5 Tue Jun 20 10:34:21 PST 2023 1.2.5)) +eustf roseus_c_util 1.irteusgl$ (load "first_motion.l") +;; (make-irtviewer) executed +t +2.irteusgl$ (require "package://pr2eus/pr2-util.l") ;;import pr2 package +:larm :start-grasp) +;; error: cannot find method :start-grasp +nilp +3.irteusgl$ (require "package://pr2eus/pr2-util.l") ;;import pr2 package +:larm :start-grasp) +Call Stack (max depth: 20): + 0: at (apply #'ros::load-org-for-ros ros::fullname args) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 3: at (require "package://pr2eus/pr2-util.l") + 4: at (require "package://pr2eus/pr2-util.l") + 5: at # +/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: file #P"/opt/ros/noetic/share/pr2eus/pr2-util.l" not found in (apply #'ros::load-org-for-ros ros::fullname args) +4.E1-irteusgl$ nil +5.E1-irteusgl$ sh: 1: Syntax error: ")" unexpected +512 +6.E1-irteusgl$ (require "package://pr2eus/pr2-util.l") +:larm :start-grasp) +Call Stack (max depth: 20): + 0: at (apply #'ros::load-org-for-ros ros::fullname args) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 3: at (require "package://pr2eus/pr2-util.l") + 4: at (require "package://pr2eus/pr2-util.l") + 5: at euserror + 6: at euserror + 7: at (apply #'ros::load-org-for-ros ros::fullname args) + 8: at (apply #'ros::load-org-for-ros ros::fullname args) + 9: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 10: at (require "package://pr2eus/pr2-util.l") + 11: at (require "package://pr2eus/pr2-util.l") + 12: at # +/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: file #P"/opt/ros/noetic/share/pr2eus/pr2-util.l" not found in (apply #'ros::load-org-for-ros ros::fullname args) +7.E2-irteusgl$ nil +7.E2-irteusgl$ sh: 1: Syntax error: ")" unexpected +512 +8.E2-irteusgl$ (require "package://pr2eus/pr2-util.l") + +Call Stack (max depth: 20): + 0: at (apply #'ros::load-org-for-ros ros::fullname args) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 3: at (require "package://pr2eus/pr2-util.l") + 4: at (require "package://pr2eus/pr2-util.l") + 5: at euserror + 6: at euserror + 7: at (apply #'ros::load-org-for-ros ros::fullname args) + 8: at (apply #'ros::load-org-for-ros ros::fullname args) + 9: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 10: at (require "package://pr2eus/pr2-util.l") + 11: at (require "package://pr2eus/pr2-util.l") + 12: at euserror + 13: at euserror + 14: at (apply #'ros::load-org-for-ros ros::fullname args) + 15: at (apply #'ros::load-org-for-ros ros::fullname args) + 16: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 17: at (require "package://pr2eus/pr2-util.l") + 18: at (require "package://pr2eus/pr2-util.l") + 19: at # +/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: file #P"/opt/ros/noetic/share/pr2eus/pr2-util.l" not found in (apply #'ros::load-org-for-ros ros::fullname args) +9.E3-irteusgl$ nil +9.E3-irteusgl$ nil +9.E3-irteusgl$ (require "package://pr2eus/pr2-util.l") +Call Stack (max depth: 20): + 0: at (apply #'ros::load-org-for-ros ros::fullname args) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 3: at (require "package://pr2eus/pr2-util.l") + 4: at (require "package://pr2eus/pr2-util.l") + 5: at euserror + 6: at euserror + 7: at (apply #'ros::load-org-for-ros ros::fullname args) + 8: at (apply #'ros::load-org-for-ros ros::fullname args) + 9: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 10: at (require "package://pr2eus/pr2-util.l") + 11: at (require "package://pr2eus/pr2-util.l") + 12: at euserror + 13: at euserror + 14: at (apply #'ros::load-org-for-ros ros::fullname args) + 15: at (apply #'ros::load-org-for-ros ros::fullname args) + 16: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 17: at (require "package://pr2eus/pr2-util.l") + 18: at (require "package://pr2eus/pr2-util.l") + 19: at euserror + And more... +/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: file #P"/opt/ros/noetic/share/pr2eus/pr2-util.l" not found in (apply #'ros::load-org-for-ros ros::fullname args) +10.E4-irteusgl$ (require "package://pr2eus/pr2-utils.l") +:|PACKAGE://PR2EUS/PR2-UTILS.L| +11.E4-irteusgl$ (send *pr2* :larm :start-grasp) +;; error: cannot find method :start-grasp +nil +12.E4-irteusgl$ (send *pr2* :start-grasp :larm) +nil +13.E4-irteusgl$ (send *pr2* :stop-grasp :larm) +Call Stack (max depth: 20): + 0: at (if (eq #:case3317 ':larm) larm-grasping-obj nil nil) + 1: at (if (eq #:case3317 ':rarm) rarm-grasping-obj (if (eq #:case3317 ':larm) larm-grasping-obj nil nil) nil) + 2: at (let ((#:case3317 arm)) (if (eq #:case3317 ':rarm) rarm-grasping-obj (if (eq #:case3317 ':larm) larm-grasping-obj nil nil) nil)) + 3: at (case arm (:rarm rarm-grasping-obj) (:larm larm-grasping-obj)) + 4: at (cond ((or (derivedp target body) (derivedp target cascaded-coords)) (case arm (:rarm (setq rarm-grasping-obj target)) (:larm (setq larm-grasping-obj target)))) (target (case arm (:rarm rarm-grasping-obj) (:larm larm-grasping-obj))) ((null target) (case arm (:rarm (setq rarm-grasping-obj nil)) (:larm (setq larm-grasping-obj nil)))) (t)) + 5: at (send self :grasping-obj am) + 6: at (setq target (send self :grasping-obj am)) + 7: at (progn (setq target (send self :grasping-obj am))) + 8: at (if (null target) (progn (setq target (send self :grasping-obj am)))) + 9: at (when (null target) (setq target (send self :grasping-obj am))) + 10: at (while #:dolist3315 (setq am (pop #:dolist3315)) (when (null target) (setq target (send self :grasping-obj am))) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target) (send self :gripper am :joint-angle (+ (send (send self :gripper_finger_joint am) :max-angle) 15))) + 11: at (let ((am nil) (#:dolist3315 arm-lst)) nil (while #:dolist3315 (setq am (pop #:dolist3315)) (when (null target) (setq target (send self :grasping-obj am))) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target) (send self :gripper am :joint-angle (+ (send (send self :gripper_finger_joint am) :max-angle) 15))) nil) + 12: at (dolist (am arm-lst) (when (null target) (setq target (send self :grasping-obj am))) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target) (send self :gripper am :joint-angle (+ (send (send self :gripper_finger_joint am) :max-angle) 15))) + 13: at (cond ((and (numberp arg1) (derivedp arg2 body)) (setq angle arg1 target arg2) (dolist (am arm-lst) (when (eq (send self :grasping-obj am) target) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target)) (send self :gripper am :joint-angle angle))) ((numberp arg1) (setq angle arg1) (dolist (am arm-lst) (send self :gripper am :joint-angle angle))) (t (when (derivedp arg1 body) (setq target arg1)) (dolist (am arm-lst) (when (null target) (setq target (send self :grasping-obj am))) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target) (send self :gripper am :joint-angle (+ (send (send self :gripper_finger_joint am) :max-angle) 15))))) + 14: at (let (angle target finger-joint (arm-lst (case arm ((:rarm :larm) (list arm)) (:arms (list :rarm :larm))))) (cond ((and (numberp arg1) (derivedp arg2 body)) (setq angle arg1 target arg2) (dolist (am arm-lst) (when (eq (send self :grasping-obj am) target) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target)) (send self :gripper am :joint-angle angle))) ((numberp arg1) (setq angle arg1) (dolist (am arm-lst) (send self :gripper am :joint-angle angle))) (t (when (derivedp arg1 body) (setq target arg1)) (dolist (am arm-lst) (when (null target) (setq target (send self :grasping-obj am))) (send self :grasping-obj am nil) (send (send self :gripper_finger_joint am) :parent-link :dissoc target) (send self :gripper am :joint-angle (+ (send (send self :gripper_finger_joint am) :max-angle) 15)))))) + 15: at (send *pr2* :stop-grasp :larm) + 16: at euserror + 17: at euserror + 18: at (apply #'ros::load-org-for-ros ros::fullname args) + 19: at (apply #'ros::load-org-for-ros ros::fullname args) + And more... +/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable larm-grasping-obj in (if (eq #:case3317 ':larm) larm-grasping-obj nil nil) +14.E5-irteusgl$ (send *pr2* :larm :gripper :joint-angle 30) +30 +15.E5-irteusgl$ (send *pr2* :larm :gripper :joint-angle -30) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +-30 +16.E5-irteusgl$ (send *pr2* :larm :gripper :joint-angle -0) +0 +17.E5-irteusgl$ (load "first_motion.l") +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +t +18.E5-irteusgl$ o(send *pr2* :larm :gripper :joint-angle -30) +(send *pr2* :rarm :gripper :joint-angle -30) + +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +-30 +19.E5-irteusgl$ ;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +;; # :joint-angle(-15) violate min-angle(0.0) +-30 +20.E5-irteusgl$ nil +20.E5-irteusgl$ (load "first_motion.l") +t +21.E5-irteusgl$ (exit) +[ INFO] [1729664760.034245223]: cell* ROSEUS_EXIT(context*, int, cell**) +mech-user@ki00119:~/semi_ws/src$ rossetmaster pr1040 +set ROS_MASTER_URI to http://pr1040:11311 +[http://pr1040:11311][] mech-user@ki00119:~/semi_ws/src$ rossetip +Could not resolve ip from address. Subnet may be different +set ROS_IP and ROS_HOSTNAME to 10.100.196.111 +[http://pr1040:11311][10.100.196.111] mech-user@ki00119:~/semi_ws/src$ rossetip +Could not resolve ip from address. Subnet may be different +set ROS_IP and ROS_HOSTNAME to 133.11.216.23 +[http://pr1040:11311][133.11.216.23] mech-user@ki00119:~/semi_ws/src$ rostopic list +/accelerometer/l_gripper_motor +/accelerometer/r_gripper_motor +/amcl/parameter_descriptions +/amcl/parameter_updates +/amcl_pose +/app_scheduler/app_schedules +/audible_warning/output/original_text +/audible_warning/output/text +/audible_warning/parameter_descriptions +/audible_warning/parameter_updates +/audio +/audio_play/audio +/base_controller/bl_caster_l_wheel_joint/parameter_descriptions +/base_controller/bl_caster_l_wheel_joint/parameter_updates +/base_controller/bl_caster_r_wheel_joint/parameter_descriptions +/base_controller/bl_caster_r_wheel_joint/parameter_updates +/base_controller/bl_caster_rotation_joint/position_controller/parameter_descriptions +/base_controller/bl_caster_rotation_joint/position_controller/parameter_updates +/base_controller/br_caster_l_wheel_joint/parameter_descriptions +/base_controller/br_caster_l_wheel_joint/parameter_updates +/base_controller/br_caster_r_wheel_joint/parameter_descriptions +/base_controller/br_caster_r_wheel_joint/parameter_updates +/base_controller/br_caster_rotation_joint/position_controller/parameter_descriptions +/base_controller/br_caster_rotation_joint/position_controller/parameter_updates +/base_controller/cmd_vel +/base_controller/command +/base_controller/command_unchecked +/base_controller/fl_caster_l_wheel_joint/parameter_descriptions +/base_controller/fl_caster_l_wheel_joint/parameter_updates +/base_controller/fl_caster_r_wheel_joint/parameter_descriptions +/base_controller/fl_caster_r_wheel_joint/parameter_updates +/base_controller/fl_caster_rotation_joint/position_controller/parameter_descriptions +/base_controller/fl_caster_rotation_joint/position_controller/parameter_updates +/base_controller/follow_joint_trajectory/cancel +/base_controller/follow_joint_trajectory/feedback +/base_controller/follow_joint_trajectory/goal +/base_controller/follow_joint_trajectory/result +/base_controller/follow_joint_trajectory/status +/base_controller/fr_caster_l_wheel_joint/parameter_descriptions +/base_controller/fr_caster_l_wheel_joint/parameter_updates +/base_controller/fr_caster_r_wheel_joint/parameter_descriptions +/base_controller/fr_caster_r_wheel_joint/parameter_updates +/base_controller/fr_caster_rotation_joint/position_controller/parameter_descriptions +/base_controller/fr_caster_rotation_joint/position_controller/parameter_updates +/base_controller/state +/base_hokuyo_node/parameter_descriptions +/base_hokuyo_node/parameter_updates +/base_odometry/odom +/base_odometry/odometer +/base_odometry/state +/base_scan +/base_scan_filtered +/base_scan_filtered/scan +/base_scan_shadow_filtered +/base_scan_throttled +/base_shadow_filter/dark_shadows/parameter_descriptions +/base_shadow_filter/dark_shadows/parameter_updates +/base_shadow_filter/shadows/parameter_descriptions +/base_shadow_filter/shadows/parameter_updates +/battery/server +/battery/server2 +/calibrated +/calibration_status +/camera_synchronizer_node/parameter_descriptions +/camera_synchronizer_node/parameter_updates +/check_room_light/output +/client_count +/cmd_vel_smoother/parameter_descriptions +/cmd_vel_smoother/parameter_updates +/connected_clients +/dashboard_agg +/ddwrt/accesspoint +/diagnostics +/diagnostics_agg +/diagnostics_toplevel_state +/dialog_response +/dialogflow_client/text_action/cancel +/dialogflow_client/text_action/feedback +/dialogflow_client/text_action/goal +/dialogflow_client/text_action/result +/dialogflow_client/text_action/status +/edgetpu_human_pose_estimator/output/class +/edgetpu_human_pose_estimator/output/image +/edgetpu_human_pose_estimator/output/image/compressed +/edgetpu_human_pose_estimator/output/poses +/edgetpu_human_pose_estimator/output/rects +/edgetpu_human_pose_estimator/output/skeletons +/edgetpu_human_pose_estimator/parameter_descriptions +/edgetpu_human_pose_estimator/parameter_updates +/email +/empty_cloud +/eng2/1f +/eng2/1f_tf +/eng2/2f +/eng2/2f_tf +/eng2/3f +/eng2/3f_tf +/eng2/7f +/eng2/7f_tf +/eng2/8f +/eng2/8f_tf +/femto_mega/accel/imu_info +/femto_mega/color/camera_info +/femto_mega/color/image_raw +/femto_mega/depth/camera_info +/femto_mega/depth/image_raw +/femto_mega/depth/points +/femto_mega/depth_to_accel +/femto_mega/depth_to_color +/femto_mega/depth_to_gyro +/femto_mega/depth_to_ir +/femto_mega/filter_status +/femto_mega/gyro/imu_info +/femto_mega/gyro_accel/sample +/femto_mega/ir/camera_info +/femto_mega/ir/image_raw +/femto_mega/sdk_version +/google_chat_ros/card_activity +/google_chat_ros/message_activity +/google_chat_ros/send/cancel +/google_chat_ros/send/feedback +/google_chat_ros/send/goal +/google_chat_ros/send/result +/google_chat_ros/send/status +/google_chat_ros/space_activity +/head_camera_trigger/trigger +/head_camera_trigger/waveform +/head_traj_controller/command +/head_traj_controller/command_deadman +/head_traj_controller/command_dummy +/head_traj_controller/follow_joint_trajectory/cancel +/head_traj_controller/follow_joint_trajectory/feedback +/head_traj_controller/follow_joint_trajectory/goal +/head_traj_controller/follow_joint_trajectory/result +/head_traj_controller/follow_joint_trajectory/status +/head_traj_controller/gains/head_pan_joint/parameter_descriptions +/head_traj_controller/gains/head_pan_joint/parameter_updates +/head_traj_controller/gains/head_tilt_joint/parameter_descriptions +/head_traj_controller/gains/head_tilt_joint/parameter_updates +/head_traj_controller/joint_trajectory_action/cancel +/head_traj_controller/joint_trajectory_action/feedback +/head_traj_controller/joint_trajectory_action/goal +/head_traj_controller/joint_trajectory_action/result +/head_traj_controller/joint_trajectory_action/status +/head_traj_controller/mux/selected +/head_traj_controller/point_head_action/cancel +/head_traj_controller/point_head_action/feedback +/head_traj_controller/point_head_action/goal +/head_traj_controller/point_head_action/result +/head_traj_controller/point_head_action/status +/head_traj_controller/state +/head_traj_controller_loose/command +/head_traj_controller_loose/state +/initialpose +/initialpose3d +/input_vel +/input_vel_mux/selected +/joint_states +/joy +/joy/set_feedback +/joy_org +/joy_org_ds3 +/joy_other +/joy_vel +/kinect_head/depth/camera_info +/kinect_head/depth/disparity +/kinect_head/depth/image +/kinect_head/depth/image/compressed +/kinect_head/depth/image/compressed/parameter_descriptions +/kinect_head/depth/image/compressed/parameter_updates +/kinect_head/depth/image/compressedDepth +/kinect_head/depth/image/compressedDepth/parameter_descriptions +/kinect_head/depth/image/compressedDepth/parameter_updates +/kinect_head/depth/image/theora +/kinect_head/depth/image/theora/parameter_descriptions +/kinect_head/depth/image/theora/parameter_updates +/kinect_head/depth/image/zdepth +/kinect_head/depth/image_raw +/kinect_head/depth/image_raw/compressed +/kinect_head/depth/image_raw/compressed/parameter_descriptions +/kinect_head/depth/image_raw/compressed/parameter_updates +/kinect_head/depth/image_raw/compressedDepth +/kinect_head/depth/image_raw/compressedDepth/parameter_descriptions +/kinect_head/depth/image_raw/compressedDepth/parameter_updates +/kinect_head/depth/image_raw/theora +/kinect_head/depth/image_raw/theora/parameter_descriptions +/kinect_head/depth/image_raw/theora/parameter_updates +/kinect_head/depth/image_raw/zdepth +/kinect_head/depth/image_rect +/kinect_head/depth/image_rect/compressed +/kinect_head/depth/image_rect/compressed/parameter_descriptions +/kinect_head/depth/image_rect/compressed/parameter_updates +/kinect_head/depth/image_rect/compressedDepth +/kinect_head/depth/image_rect/compressedDepth/parameter_descriptions +/kinect_head/depth/image_rect/compressedDepth/parameter_updates +/kinect_head/depth/image_rect/theora +/kinect_head/depth/image_rect/theora/parameter_descriptions +/kinect_head/depth/image_rect/theora/parameter_updates +/kinect_head/depth/image_rect/zdepth +/kinect_head/depth/image_rect_raw +/kinect_head/depth/image_rect_raw/compressed +/kinect_head/depth/image_rect_raw/compressed/parameter_descriptions +/kinect_head/depth/image_rect_raw/compressed/parameter_updates +/kinect_head/depth/image_rect_raw/compressedDepth +/kinect_head/depth/image_rect_raw/compressedDepth/parameter_descriptions +/kinect_head/depth/image_rect_raw/compressedDepth/parameter_updates +/kinect_head/depth/image_rect_raw/theora +/kinect_head/depth/image_rect_raw/theora/parameter_descriptions +/kinect_head/depth/image_rect_raw/theora/parameter_updates +/kinect_head/depth/image_rect_raw/zdepth +/kinect_head/depth/points +/kinect_head/depth_rectify_depth/parameter_descriptions +/kinect_head/depth_rectify_depth/parameter_updates +/kinect_head/depth_registered/camera_info +/kinect_head/depth_registered/disparity +/kinect_head/depth_registered/downsample_cloud_half/input/mask +/kinect_head/depth_registered/downsample_cloud_half/parameter_descriptions +/kinect_head/depth_registered/downsample_cloud_half/parameter_updates +/kinect_head/depth_registered/downsample_cloud_quater/input/mask +/kinect_head/depth_registered/downsample_cloud_quater/parameter_descriptions +/kinect_head/depth_registered/downsample_cloud_quater/parameter_updates +/kinect_head/depth_registered/half/points +/kinect_head/depth_registered/half/throttled/points +/kinect_head/depth_registered/hw_registered/image_rect +/kinect_head/depth_registered/hw_registered/image_rect/compressed +/kinect_head/depth_registered/hw_registered/image_rect/compressed/parameter_descriptions +/kinect_head/depth_registered/hw_registered/image_rect/compressed/parameter_updates +/kinect_head/depth_registered/hw_registered/image_rect/compressedDepth +/kinect_head/depth_registered/hw_registered/image_rect/compressedDepth/parameter_descriptions +/kinect_head/depth_registered/hw_registered/image_rect/compressedDepth/parameter_updates +/kinect_head/depth_registered/hw_registered/image_rect/theora +/kinect_head/depth_registered/hw_registered/image_rect/theora/parameter_descriptions +/kinect_head/depth_registered/hw_registered/image_rect/theora/parameter_updates +/kinect_head/depth_registered/hw_registered/image_rect/zdepth +/kinect_head/depth_registered/hw_registered/image_rect_raw +/kinect_head/depth_registered/hw_registered/image_rect_raw/compressed +/kinect_head/depth_registered/hw_registered/image_rect_raw/compressed/parameter_descriptions +/kinect_head/depth_registered/hw_registered/image_rect_raw/compressed/parameter_updates +/kinect_head/depth_registered/hw_registered/image_rect_raw/compressedDepth +/kinect_head/depth_registered/hw_registered/image_rect_raw/compressedDepth/parameter_descriptions +/kinect_head/depth_registered/hw_registered/image_rect_raw/compressedDepth/parameter_updates +/kinect_head/depth_registered/hw_registered/image_rect_raw/theora +/kinect_head/depth_registered/hw_registered/image_rect_raw/theora/parameter_descriptions +/kinect_head/depth_registered/hw_registered/image_rect_raw/theora/parameter_updates +/kinect_head/depth_registered/hw_registered/image_rect_raw/zdepth +/kinect_head/depth_registered/image +/kinect_head/depth_registered/image/compressed +/kinect_head/depth_registered/image/compressed/parameter_descriptions +/kinect_head/depth_registered/image/compressed/parameter_updates +/kinect_head/depth_registered/image/compressedDepth +/kinect_head/depth_registered/image/compressedDepth/parameter_descriptions +/kinect_head/depth_registered/image/compressedDepth/parameter_updates +/kinect_head/depth_registered/image/theora +/kinect_head/depth_registered/image/theora/parameter_descriptions +/kinect_head/depth_registered/image/theora/parameter_updates +/kinect_head/depth_registered/image/zdepth +/kinect_head/depth_registered/image_raw +/kinect_head/depth_registered/image_raw/compressed +/kinect_head/depth_registered/image_raw/compressed/parameter_descriptions +/kinect_head/depth_registered/image_raw/compressed/parameter_updates +/kinect_head/depth_registered/image_raw/compressedDepth +/kinect_head/depth_registered/image_raw/compressedDepth/parameter_descriptions +/kinect_head/depth_registered/image_raw/compressedDepth/parameter_updates +/kinect_head/depth_registered/image_raw/theora +/kinect_head/depth_registered/image_raw/theora/parameter_descriptions +/kinect_head/depth_registered/image_raw/theora/parameter_updates +/kinect_head/depth_registered/image_raw/zdepth +/kinect_head/depth_registered/image_rect +/kinect_head/depth_registered/image_rect/compressedDepth +/kinect_head/depth_registered/points +/kinect_head/depth_registered/points_self_filtered +/kinect_head/depth_registered/quater/points +/kinect_head/depth_registered/quater/throttled/points +/kinect_head/depth_registered/throttle_camera_info/parameter_descriptions +/kinect_head/depth_registered/throttle_camera_info/parameter_updates +/kinect_head/depth_registered/throttle_cloud/parameter_descriptions +/kinect_head/depth_registered/throttle_cloud/parameter_updates +/kinect_head/depth_registered/throttle_cloud_half/parameter_descriptions +/kinect_head/depth_registered/throttle_cloud_half/parameter_updates +/kinect_head/depth_registered/throttle_cloud_quater/parameter_descriptions +/kinect_head/depth_registered/throttle_cloud_quater/parameter_updates +/kinect_head/depth_registered/throttle_image/parameter_descriptions +/kinect_head/depth_registered/throttle_image/parameter_updates +/kinect_head/depth_registered/throttle_image_compressed/parameter_descriptions +/kinect_head/depth_registered/throttle_image_compressed/parameter_updates +/kinect_head/depth_registered/throttled/camera_info +/kinect_head/depth_registered/throttled/image_rect +/kinect_head/depth_registered/throttled/image_rect/compressedDepth +/kinect_head/depth_registered/throttled/points +/kinect_head/depth_registered_rectify_depth/parameter_descriptions +/kinect_head/depth_registered_rectify_depth/parameter_updates +/kinect_head/driver/parameter_descriptions +/kinect_head/driver/parameter_updates +/kinect_head/ir/camera_info +/kinect_head/ir/image_raw +/kinect_head/ir/image_raw/compressed +/kinect_head/ir/image_raw/compressed/parameter_descriptions +/kinect_head/ir/image_raw/compressed/parameter_updates +/kinect_head/ir/image_raw/compressedDepth +/kinect_head/ir/image_raw/compressedDepth/parameter_descriptions +/kinect_head/ir/image_raw/compressedDepth/parameter_updates +/kinect_head/ir/image_raw/theora +/kinect_head/ir/image_raw/theora/parameter_descriptions +/kinect_head/ir/image_raw/theora/parameter_updates +/kinect_head/ir/image_raw/zdepth +/kinect_head/ir/image_rect_ir +/kinect_head/ir/image_rect_ir/compressed +/kinect_head/ir/image_rect_ir/compressed/parameter_descriptions +/kinect_head/ir/image_rect_ir/compressed/parameter_updates +/kinect_head/ir/image_rect_ir/compressedDepth +/kinect_head/ir/image_rect_ir/compressedDepth/parameter_descriptions +/kinect_head/ir/image_rect_ir/compressedDepth/parameter_updates +/kinect_head/ir/image_rect_ir/theora +/kinect_head/ir/image_rect_ir/theora/parameter_descriptions +/kinect_head/ir/image_rect_ir/theora/parameter_updates +/kinect_head/ir/image_rect_ir/zdepth +/kinect_head/ir_rectify_ir/parameter_descriptions +/kinect_head/ir_rectify_ir/parameter_updates +/kinect_head/kinect_head_c2_nodelet_manager/bond +/kinect_head/kinect_head_nodelet_manager/bond +/kinect_head/projector/camera_info +/kinect_head/rgb/camera_info +/kinect_head/rgb/downsample_half/parameter_descriptions +/kinect_head/rgb/downsample_half/parameter_updates +/kinect_head/rgb/downsample_quater/parameter_descriptions +/kinect_head/rgb/downsample_quater/parameter_updates +/kinect_head/rgb/half/camera_info +/kinect_head/rgb/half/image_rect_color +/kinect_head/rgb/half/image_rect_color/compressed +/kinect_head/rgb/half/image_rect_color/compressed/parameter_descriptions +/kinect_head/rgb/half/image_rect_color/compressed/parameter_updates +/kinect_head/rgb/half/image_rect_color/compressedDepth +/kinect_head/rgb/half/image_rect_color/compressedDepth/parameter_descriptions +/kinect_head/rgb/half/image_rect_color/compressedDepth/parameter_updates +/kinect_head/rgb/half/image_rect_color/theora +/kinect_head/rgb/half/image_rect_color/theora/parameter_descriptions +/kinect_head/rgb/half/image_rect_color/theora/parameter_updates +/kinect_head/rgb/half/image_rect_color/zdepth +/kinect_head/rgb/image_color +/kinect_head/rgb/image_color/compressed +/kinect_head/rgb/image_color/compressed/parameter_descriptions +/kinect_head/rgb/image_color/compressed/parameter_updates +/kinect_head/rgb/image_color/compressedDepth +/kinect_head/rgb/image_color/compressedDepth/parameter_descriptions +/kinect_head/rgb/image_color/compressedDepth/parameter_updates +/kinect_head/rgb/image_color/theora +/kinect_head/rgb/image_color/theora/parameter_descriptions +/kinect_head/rgb/image_color/theora/parameter_updates +/kinect_head/rgb/image_color/zdepth +/kinect_head/rgb/image_mono +/kinect_head/rgb/image_mono/compressed +/kinect_head/rgb/image_mono/compressed/parameter_descriptions +/kinect_head/rgb/image_mono/compressed/parameter_updates +/kinect_head/rgb/image_mono/compressedDepth +/kinect_head/rgb/image_mono/compressedDepth/parameter_descriptions +/kinect_head/rgb/image_mono/compressedDepth/parameter_updates +/kinect_head/rgb/image_mono/theora +/kinect_head/rgb/image_mono/theora/parameter_descriptions +/kinect_head/rgb/image_mono/theora/parameter_updates +/kinect_head/rgb/image_mono/zdepth +/kinect_head/rgb/image_raw +/kinect_head/rgb/image_raw/compressed +/kinect_head/rgb/image_raw/compressed/parameter_descriptions +/kinect_head/rgb/image_raw/compressed/parameter_updates +/kinect_head/rgb/image_raw/compressedDepth +/kinect_head/rgb/image_raw/compressedDepth/parameter_descriptions +/kinect_head/rgb/image_raw/compressedDepth/parameter_updates +/kinect_head/rgb/image_raw/theora +/kinect_head/rgb/image_raw/theora/parameter_descriptions +/kinect_head/rgb/image_raw/theora/parameter_updates +/kinect_head/rgb/image_raw/zdepth +/kinect_head/rgb/image_rect_color +/kinect_head/rgb/image_rect_color/compressed +/kinect_head/rgb/image_rect_color/compressed/parameter_descriptions +/kinect_head/rgb/image_rect_color/compressed/parameter_updates +/kinect_head/rgb/image_rect_color/compressedDepth +/kinect_head/rgb/image_rect_color/compressedDepth/parameter_descriptions +/kinect_head/rgb/image_rect_color/compressedDepth/parameter_updates +/kinect_head/rgb/image_rect_color/theora +/kinect_head/rgb/image_rect_color/theora/parameter_descriptions +/kinect_head/rgb/image_rect_color/theora/parameter_updates +/kinect_head/rgb/image_rect_color/zdepth +/kinect_head/rgb/image_rect_mono +/kinect_head/rgb/image_rect_mono/compressed +/kinect_head/rgb/image_rect_mono/compressed/parameter_descriptions +/kinect_head/rgb/image_rect_mono/compressed/parameter_updates +/kinect_head/rgb/image_rect_mono/compressedDepth +/kinect_head/rgb/image_rect_mono/compressedDepth/parameter_descriptions +/kinect_head/rgb/image_rect_mono/compressedDepth/parameter_updates +/kinect_head/rgb/image_rect_mono/theora +/kinect_head/rgb/image_rect_mono/theora/parameter_descriptions +/kinect_head/rgb/image_rect_mono/theora/parameter_updates +/kinect_head/rgb/image_rect_mono/zdepth +/kinect_head/rgb/quater/camera_info +/kinect_head/rgb/quater/image_rect_color +/kinect_head/rgb/quater/image_rect_color/compressed +/kinect_head/rgb/quater/image_rect_color/compressed/parameter_descriptions +/kinect_head/rgb/quater/image_rect_color/compressed/parameter_updates +/kinect_head/rgb/quater/image_rect_color/compressedDepth +/kinect_head/rgb/quater/image_rect_color/compressedDepth/parameter_descriptions +/kinect_head/rgb/quater/image_rect_color/compressedDepth/parameter_updates +/kinect_head/rgb/quater/image_rect_color/theora +/kinect_head/rgb/quater/image_rect_color/theora/parameter_descriptions +/kinect_head/rgb/quater/image_rect_color/theora/parameter_updates +/kinect_head/rgb/quater/image_rect_color/zdepth +/kinect_head/rgb/throttle_camera_info/parameter_descriptions +/kinect_head/rgb/throttle_camera_info/parameter_updates +/kinect_head/rgb/throttle_rgb/parameter_descriptions +/kinect_head/rgb/throttle_rgb/parameter_updates +/kinect_head/rgb/throttle_rgb_compressed/parameter_descriptions +/kinect_head/rgb/throttle_rgb_compressed/parameter_updates +/kinect_head/rgb/throttled/camera_info +/kinect_head/rgb/throttled/image_rect_color +/kinect_head/rgb/throttled/image_rect_color/compressed +/kinect_head/rgb_debayer/parameter_descriptions +/kinect_head/rgb_debayer/parameter_updates +/kinect_head/rgb_rectify_color/parameter_descriptions +/kinect_head/rgb_rectify_color/parameter_updates +/kinect_head/rgb_rectify_mono/parameter_descriptions +/kinect_head/rgb_rectify_mono/parameter_updates +/kinect_head_c2/depth_registered/camera_info +/kinect_head_c2/depth_registered/points +/kinect_head_c2/rgb/camera_info +/kinect_head_c2/rgb/image_rect_color +/l_arm_controller/command +/l_arm_controller/follow_joint_trajectory/cancel +/l_arm_controller/follow_joint_trajectory/feedback +/l_arm_controller/follow_joint_trajectory/goal +/l_arm_controller/follow_joint_trajectory/result +/l_arm_controller/follow_joint_trajectory/status +/l_arm_controller/gains/l_elbow_flex_joint/parameter_descriptions +/l_arm_controller/gains/l_elbow_flex_joint/parameter_updates +/l_arm_controller/gains/l_forearm_roll_joint/parameter_descriptions +/l_arm_controller/gains/l_forearm_roll_joint/parameter_updates +/l_arm_controller/gains/l_shoulder_lift_joint/parameter_descriptions +/l_arm_controller/gains/l_shoulder_lift_joint/parameter_updates +/l_arm_controller/gains/l_shoulder_pan_joint/parameter_descriptions +/l_arm_controller/gains/l_shoulder_pan_joint/parameter_updates +/l_arm_controller/gains/l_upper_arm_roll_joint/parameter_descriptions +/l_arm_controller/gains/l_upper_arm_roll_joint/parameter_updates +/l_arm_controller/gains/l_wrist_flex_joint/parameter_descriptions +/l_arm_controller/gains/l_wrist_flex_joint/parameter_updates +/l_arm_controller/gains/l_wrist_roll_joint/parameter_descriptions +/l_arm_controller/gains/l_wrist_roll_joint/parameter_updates +/l_arm_controller/joint_trajectory_action/cancel +/l_arm_controller/joint_trajectory_action/feedback +/l_arm_controller/joint_trajectory_action/goal +/l_arm_controller/joint_trajectory_action/result +/l_arm_controller/joint_trajectory_action/status +/l_arm_controller/state +/l_arm_controller_loose/command +/l_arm_controller_loose/state +/l_forearm_cam/camera_info +/l_forearm_cam/image_color +/l_forearm_cam/image_color/compressed +/l_forearm_cam/image_color/compressed/parameter_descriptions +/l_forearm_cam/image_color/compressed/parameter_updates +/l_forearm_cam/image_color/compressedDepth +/l_forearm_cam/image_color/compressedDepth/parameter_descriptions +/l_forearm_cam/image_color/compressedDepth/parameter_updates +/l_forearm_cam/image_color/theora +/l_forearm_cam/image_color/theora/parameter_descriptions +/l_forearm_cam/image_color/theora/parameter_updates +/l_forearm_cam/image_color/zdepth +/l_forearm_cam/image_mono +/l_forearm_cam/image_mono/compressed +/l_forearm_cam/image_mono/compressed/parameter_descriptions +/l_forearm_cam/image_mono/compressed/parameter_updates +/l_forearm_cam/image_mono/compressedDepth +/l_forearm_cam/image_mono/compressedDepth/parameter_descriptions +/l_forearm_cam/image_mono/compressedDepth/parameter_updates +/l_forearm_cam/image_mono/theora +/l_forearm_cam/image_mono/theora/parameter_descriptions +/l_forearm_cam/image_mono/theora/parameter_updates +/l_forearm_cam/image_mono/zdepth +/l_forearm_cam/image_proc_debayer/parameter_descriptions +/l_forearm_cam/image_proc_debayer/parameter_updates +/l_forearm_cam/image_proc_rectify_color/parameter_descriptions +/l_forearm_cam/image_proc_rectify_color/parameter_updates +/l_forearm_cam/image_proc_rectify_mono/parameter_descriptions +/l_forearm_cam/image_proc_rectify_mono/parameter_updates +/l_forearm_cam/image_raw +/l_forearm_cam/image_raw/compressed +/l_forearm_cam/image_raw/compressed/parameter_descriptions +/l_forearm_cam/image_raw/compressed/parameter_updates +/l_forearm_cam/image_raw/compressedDepth +/l_forearm_cam/image_raw/compressedDepth/parameter_descriptions +/l_forearm_cam/image_raw/compressedDepth/parameter_updates +/l_forearm_cam/image_raw/theora +/l_forearm_cam/image_raw/theora/parameter_descriptions +/l_forearm_cam/image_raw/theora/parameter_updates +/l_forearm_cam/image_raw/zdepth +/l_forearm_cam/image_rect +/l_forearm_cam/image_rect/compressed +/l_forearm_cam/image_rect/compressed/parameter_descriptions +/l_forearm_cam/image_rect/compressed/parameter_updates +/l_forearm_cam/image_rect/compressedDepth +/l_forearm_cam/image_rect/compressedDepth/parameter_descriptions +/l_forearm_cam/image_rect/compressedDepth/parameter_updates +/l_forearm_cam/image_rect/theora +/l_forearm_cam/image_rect/theora/parameter_descriptions +/l_forearm_cam/image_rect/theora/parameter_updates +/l_forearm_cam/image_rect/zdepth +/l_forearm_cam/image_rect_color +/l_forearm_cam/image_rect_color/compressed +/l_forearm_cam/image_rect_color/compressed/parameter_descriptions +/l_forearm_cam/image_rect_color/compressed/parameter_updates +/l_forearm_cam/image_rect_color/compressedDepth +/l_forearm_cam/image_rect_color/compressedDepth/parameter_descriptions +/l_forearm_cam/image_rect_color/compressedDepth/parameter_updates +/l_forearm_cam/image_rect_color/theora +/l_forearm_cam/image_rect_color/theora/parameter_descriptions +/l_forearm_cam/image_rect_color/theora/parameter_updates +/l_forearm_cam/image_rect_color/zdepth +/l_forearm_cam/parameter_descriptions +/l_forearm_cam/parameter_updates +/l_forearm_cam_trigger/waveform +/l_gripper_controller/command +/l_gripper_controller/gripper_action/cancel +/l_gripper_controller/gripper_action/feedback +/l_gripper_controller/gripper_action/goal +/l_gripper_controller/gripper_action/result +/l_gripper_controller/gripper_action/status +/l_gripper_controller/pid/parameter_descriptions +/l_gripper_controller/pid/parameter_updates +/l_gripper_controller/state +/l_gripper_sensor_controller/command +/l_gripper_sensor_controller/contact_state +/l_gripper_sensor_controller/event_detector +/l_gripper_sensor_controller/event_detector/cancel +/l_gripper_sensor_controller/event_detector/feedback +/l_gripper_sensor_controller/event_detector/goal +/l_gripper_sensor_controller/event_detector/result +/l_gripper_sensor_controller/event_detector/status +/l_gripper_sensor_controller/event_detector_state +/l_gripper_sensor_controller/find_contact +/l_gripper_sensor_controller/find_contact/cancel +/l_gripper_sensor_controller/find_contact/feedback +/l_gripper_sensor_controller/find_contact/goal +/l_gripper_sensor_controller/find_contact/result +/l_gripper_sensor_controller/find_contact/status +/l_gripper_sensor_controller/force_servo +/l_gripper_sensor_controller/force_servo/cancel +/l_gripper_sensor_controller/force_servo/feedback +/l_gripper_sensor_controller/force_servo/goal +/l_gripper_sensor_controller/force_servo/result +/l_gripper_sensor_controller/force_servo/status +/l_gripper_sensor_controller/force_servo_state +/l_gripper_sensor_controller/grab/cancel +/l_gripper_sensor_controller/grab/feedback +/l_gripper_sensor_controller/grab/goal +/l_gripper_sensor_controller/grab/result +/l_gripper_sensor_controller/grab/status +/l_gripper_sensor_controller/gripper_action/cancel +/l_gripper_sensor_controller/gripper_action/feedback +/l_gripper_sensor_controller/gripper_action/goal +/l_gripper_sensor_controller/gripper_action/result +/l_gripper_sensor_controller/gripper_action/status +/l_gripper_sensor_controller/release/cancel +/l_gripper_sensor_controller/release/feedback +/l_gripper_sensor_controller/release/goal +/l_gripper_sensor_controller/release/result +/l_gripper_sensor_controller/release/status +/l_gripper_sensor_controller/slip_servo +/l_gripper_sensor_controller/slip_servo/cancel +/l_gripper_sensor_controller/slip_servo/feedback +/l_gripper_sensor_controller/slip_servo/goal +/l_gripper_sensor_controller/slip_servo/result +/l_gripper_sensor_controller/slip_servo/status +/l_gripper_sensor_controller/slip_servo_state +/l_gripper_sensor_controller/state +/laser_tilt_controller/gains/parameter_descriptions +/laser_tilt_controller/gains/parameter_updates +/laser_tilt_controller/laser_scanner_signal +/laser_tilt_controller/set_periodic_cmd +/laser_tilt_controller/set_traj_cmd +/left_endeffector/wrench +/lifelog/joint_states_logger/output +/lifelog/joint_states_logger/parameter_descriptions +/lifelog/joint_states_logger/parameter_updates +/lifelog/joint_states_throttle/output +/lifelog/joint_states_throttle/parameter_descriptions +/lifelog/joint_states_throttle/parameter_updates +/look_at_human/enabled +/map +/map_keepout +/map_metadata +/map_reload +/map_tf_mux/selected +/map_updates +/mechanism_statistics +/message_store/insert +/motor_trace/bl_caster_l_wheel_motor +/motor_trace/bl_caster_r_wheel_motor +/motor_trace/bl_caster_rotation_motor +/motor_trace/br_caster_l_wheel_motor +/motor_trace/br_caster_r_wheel_motor +/motor_trace/br_caster_rotation_motor +/motor_trace/fl_caster_l_wheel_motor +/motor_trace/fl_caster_r_wheel_motor +/motor_trace/fl_caster_rotation_motor +/motor_trace/fr_caster_l_wheel_motor +/motor_trace/fr_caster_r_wheel_motor +/motor_trace/fr_caster_rotation_motor +/motor_trace/head_pan_motor +/motor_trace/head_tilt_motor +/motor_trace/l_elbow_flex_motor +/motor_trace/l_forearm_roll_motor +/motor_trace/l_gripper_motor +/motor_trace/l_shoulder_lift_motor +/motor_trace/l_shoulder_pan_motor +/motor_trace/l_upper_arm_roll_motor +/motor_trace/l_wrist_l_motor +/motor_trace/l_wrist_r_motor +/motor_trace/laser_tilt_mount_motor +/motor_trace/r_elbow_flex_motor +/motor_trace/r_forearm_roll_motor +/motor_trace/r_gripper_motor +/motor_trace/r_shoulder_lift_motor +/motor_trace/r_shoulder_pan_motor +/motor_trace/r_upper_arm_roll_motor +/motor_trace/r_wrist_l_motor +/motor_trace/r_wrist_r_motor +/motor_trace/torso_lift_motor +/move_base/cancel +/move_base/feedback +/move_base/goal +/move_base/recovery_status +/move_base/result +/move_base/status +/move_base_node/DWAPlannerROS/cost_cloud +/move_base_node/DWAPlannerROS/global_plan +/move_base_node/DWAPlannerROS/local_plan +/move_base_node/DWAPlannerROS/parameter_descriptions +/move_base_node/DWAPlannerROS/parameter_updates +/move_base_node/DWAPlannerROS/trajectory_cloud +/move_base_node/NavfnROS/plan +/move_base_node/current_goal +/move_base_node/global_costmap/costmap +/move_base_node/global_costmap/costmap_updates +/move_base_node/global_costmap/footprint +/move_base_node/global_costmap/inflation_layer/parameter_descriptions +/move_base_node/global_costmap/inflation_layer/parameter_updates +/move_base_node/global_costmap/obstacle_layer/parameter_descriptions +/move_base_node/global_costmap/obstacle_layer/parameter_updates +/move_base_node/global_costmap/parameter_descriptions +/move_base_node/global_costmap/parameter_updates +/move_base_node/global_costmap/static_layer/parameter_descriptions +/move_base_node/global_costmap/static_layer/parameter_updates +/move_base_node/local_costmap/costmap +/move_base_node/local_costmap/costmap_updates +/move_base_node/local_costmap/footprint +/move_base_node/local_costmap/inflation_layer/parameter_descriptions +/move_base_node/local_costmap/inflation_layer/parameter_updates +/move_base_node/local_costmap/obstacle_layer/parameter_descriptions +/move_base_node/local_costmap/obstacle_layer/parameter_updates +/move_base_node/local_costmap/parameter_descriptions +/move_base_node/local_costmap/parameter_updates +/move_base_node/parameter_descriptions +/move_base_node/parameter_updates +/move_base_simple/goal +/multiple_joystick_mux/selected +/narrow_stereo/disparity +/narrow_stereo/left/camera_info +/narrow_stereo/left/image_color +/narrow_stereo/left/image_color/compressed +/narrow_stereo/left/image_color/compressed/parameter_descriptions +/narrow_stereo/left/image_color/compressed/parameter_updates +/narrow_stereo/left/image_color/compressedDepth +/narrow_stereo/left/image_color/compressedDepth/parameter_descriptions +/narrow_stereo/left/image_color/compressedDepth/parameter_updates +/narrow_stereo/left/image_color/theora +/narrow_stereo/left/image_color/theora/parameter_descriptions +/narrow_stereo/left/image_color/theora/parameter_updates +/narrow_stereo/left/image_color/zdepth +/narrow_stereo/left/image_mono +/narrow_stereo/left/image_mono/compressed +/narrow_stereo/left/image_mono/compressed/parameter_descriptions +/narrow_stereo/left/image_mono/compressed/parameter_updates +/narrow_stereo/left/image_mono/compressedDepth +/narrow_stereo/left/image_mono/compressedDepth/parameter_descriptions +/narrow_stereo/left/image_mono/compressedDepth/parameter_updates +/narrow_stereo/left/image_mono/theora +/narrow_stereo/left/image_mono/theora/parameter_descriptions +/narrow_stereo/left/image_mono/theora/parameter_updates +/narrow_stereo/left/image_mono/zdepth +/narrow_stereo/left/image_raw +/narrow_stereo/left/image_raw/compressed +/narrow_stereo/left/image_raw/compressed/parameter_descriptions +/narrow_stereo/left/image_raw/compressed/parameter_updates +/narrow_stereo/left/image_raw/compressedDepth +/narrow_stereo/left/image_raw/compressedDepth/parameter_descriptions +/narrow_stereo/left/image_raw/compressedDepth/parameter_updates +/narrow_stereo/left/image_raw/theora +/narrow_stereo/left/image_raw/theora/parameter_descriptions +/narrow_stereo/left/image_raw/theora/parameter_updates +/narrow_stereo/left/image_raw/zdepth +/narrow_stereo/left/image_rect +/narrow_stereo/left/image_rect/compressed +/narrow_stereo/left/image_rect/compressed/parameter_descriptions +/narrow_stereo/left/image_rect/compressed/parameter_updates +/narrow_stereo/left/image_rect/compressedDepth +/narrow_stereo/left/image_rect/compressedDepth/parameter_descriptions +/narrow_stereo/left/image_rect/compressedDepth/parameter_updates +/narrow_stereo/left/image_rect/theora +/narrow_stereo/left/image_rect/theora/parameter_descriptions +/narrow_stereo/left/image_rect/theora/parameter_updates +/narrow_stereo/left/image_rect/zdepth +/narrow_stereo/left/image_rect_color +/narrow_stereo/left/image_rect_color/compressed +/narrow_stereo/left/image_rect_color/compressed/parameter_descriptions +/narrow_stereo/left/image_rect_color/compressed/parameter_updates +/narrow_stereo/left/image_rect_color/compressedDepth +/narrow_stereo/left/image_rect_color/compressedDepth/parameter_descriptions +/narrow_stereo/left/image_rect_color/compressedDepth/parameter_updates +/narrow_stereo/left/image_rect_color/theora +/narrow_stereo/left/image_rect_color/theora/parameter_descriptions +/narrow_stereo/left/image_rect_color/theora/parameter_updates +/narrow_stereo/left/image_rect_color/zdepth +/narrow_stereo/narrow_stereo_proc/parameter_descriptions +/narrow_stereo/narrow_stereo_proc/parameter_updates +/narrow_stereo/narrow_stereo_proc_debayer_left/parameter_descriptions +/narrow_stereo/narrow_stereo_proc_debayer_left/parameter_updates +/narrow_stereo/narrow_stereo_proc_debayer_right/parameter_descriptions +/narrow_stereo/narrow_stereo_proc_debayer_right/parameter_updates +/narrow_stereo/narrow_stereo_proc_rectify_color_left/parameter_descriptions +/narrow_stereo/narrow_stereo_proc_rectify_color_left/parameter_updates +/narrow_stereo/narrow_stereo_proc_rectify_color_right/parameter_descriptions +/narrow_stereo/narrow_stereo_proc_rectify_color_right/parameter_updates +/narrow_stereo/narrow_stereo_proc_rectify_mono_left/parameter_descriptions +/narrow_stereo/narrow_stereo_proc_rectify_mono_left/parameter_updates +/narrow_stereo/narrow_stereo_proc_rectify_mono_right/parameter_descriptions +/narrow_stereo/narrow_stereo_proc_rectify_mono_right/parameter_updates +/narrow_stereo/points2 +/narrow_stereo/right/camera_info +/narrow_stereo/right/image_color +/narrow_stereo/right/image_color/compressed +/narrow_stereo/right/image_color/compressed/parameter_descriptions +/narrow_stereo/right/image_color/compressed/parameter_updates +/narrow_stereo/right/image_color/compressedDepth +/narrow_stereo/right/image_color/compressedDepth/parameter_descriptions +/narrow_stereo/right/image_color/compressedDepth/parameter_updates +/narrow_stereo/right/image_color/theora +/narrow_stereo/right/image_color/theora/parameter_descriptions +/narrow_stereo/right/image_color/theora/parameter_updates +/narrow_stereo/right/image_color/zdepth +/narrow_stereo/right/image_mono +/narrow_stereo/right/image_mono/compressed +/narrow_stereo/right/image_mono/compressed/parameter_descriptions +/narrow_stereo/right/image_mono/compressed/parameter_updates +/narrow_stereo/right/image_mono/compressedDepth +/narrow_stereo/right/image_mono/compressedDepth/parameter_descriptions +/narrow_stereo/right/image_mono/compressedDepth/parameter_updates +/narrow_stereo/right/image_mono/theora +/narrow_stereo/right/image_mono/theora/parameter_descriptions +/narrow_stereo/right/image_mono/theora/parameter_updates +/narrow_stereo/right/image_mono/zdepth +/narrow_stereo/right/image_raw +/narrow_stereo/right/image_raw/compressed +/narrow_stereo/right/image_raw/compressed/parameter_descriptions +/narrow_stereo/right/image_raw/compressed/parameter_updates +/narrow_stereo/right/image_raw/compressedDepth +/narrow_stereo/right/image_raw/compressedDepth/parameter_descriptions +/narrow_stereo/right/image_raw/compressedDepth/parameter_updates +/narrow_stereo/right/image_raw/theora +/narrow_stereo/right/image_raw/theora/parameter_descriptions +/narrow_stereo/right/image_raw/theora/parameter_updates +/narrow_stereo/right/image_raw/zdepth +/narrow_stereo/right/image_rect +/narrow_stereo/right/image_rect/compressed +/narrow_stereo/right/image_rect/compressed/parameter_descriptions +/narrow_stereo/right/image_rect/compressed/parameter_updates +/narrow_stereo/right/image_rect/compressedDepth +/narrow_stereo/right/image_rect/compressedDepth/parameter_descriptions +/narrow_stereo/right/image_rect/compressedDepth/parameter_updates +/narrow_stereo/right/image_rect/theora +/narrow_stereo/right/image_rect/theora/parameter_descriptions +/narrow_stereo/right/image_rect/theora/parameter_updates +/narrow_stereo/right/image_rect/zdepth +/narrow_stereo/right/image_rect_color +/narrow_stereo/right/image_rect_color/compressed +/narrow_stereo/right/image_rect_color/compressed/parameter_descriptions +/narrow_stereo/right/image_rect_color/compressed/parameter_updates +/narrow_stereo/right/image_rect_color/compressedDepth +/narrow_stereo/right/image_rect_color/compressedDepth/parameter_descriptions +/narrow_stereo/right/image_rect_color/compressedDepth/parameter_updates +/narrow_stereo/right/image_rect_color/theora +/narrow_stereo/right/image_rect_color/theora/parameter_descriptions +/narrow_stereo/right/image_rect_color/theora/parameter_updates +/narrow_stereo/right/image_rect_color/zdepth +/narrow_stereo_both/parameter_descriptions +/narrow_stereo_both/parameter_updates +/narrow_stereo_left/parameter_descriptions +/narrow_stereo_left/parameter_updates +/narrow_stereo_right/parameter_descriptions +/narrow_stereo_right/parameter_updates +/narrow_stereo_textured/disparity +/narrow_stereo_textured/left/camera_info +/narrow_stereo_textured/left/image_color +/narrow_stereo_textured/left/image_color/compressed +/narrow_stereo_textured/left/image_color/compressed/parameter_descriptions +/narrow_stereo_textured/left/image_color/compressed/parameter_updates +/narrow_stereo_textured/left/image_color/compressedDepth +/narrow_stereo_textured/left/image_color/compressedDepth/parameter_descriptions +/narrow_stereo_textured/left/image_color/compressedDepth/parameter_updates +/narrow_stereo_textured/left/image_color/theora +/narrow_stereo_textured/left/image_color/theora/parameter_descriptions +/narrow_stereo_textured/left/image_color/theora/parameter_updates +/narrow_stereo_textured/left/image_color/zdepth +/narrow_stereo_textured/left/image_mono +/narrow_stereo_textured/left/image_mono/compressed +/narrow_stereo_textured/left/image_mono/compressed/parameter_descriptions +/narrow_stereo_textured/left/image_mono/compressed/parameter_updates +/narrow_stereo_textured/left/image_mono/compressedDepth +/narrow_stereo_textured/left/image_mono/compressedDepth/parameter_descriptions +/narrow_stereo_textured/left/image_mono/compressedDepth/parameter_updates +/narrow_stereo_textured/left/image_mono/theora +/narrow_stereo_textured/left/image_mono/theora/parameter_descriptions +/narrow_stereo_textured/left/image_mono/theora/parameter_updates +/narrow_stereo_textured/left/image_mono/zdepth +/narrow_stereo_textured/left/image_raw +/narrow_stereo_textured/left/image_raw/compressed +/narrow_stereo_textured/left/image_raw/compressed/parameter_descriptions +/narrow_stereo_textured/left/image_raw/compressed/parameter_updates +/narrow_stereo_textured/left/image_raw/compressedDepth +/narrow_stereo_textured/left/image_raw/compressedDepth/parameter_descriptions +/narrow_stereo_textured/left/image_raw/compressedDepth/parameter_updates +/narrow_stereo_textured/left/image_raw/theora +/narrow_stereo_textured/left/image_raw/theora/parameter_descriptions +/narrow_stereo_textured/left/image_raw/theora/parameter_updates +/narrow_stereo_textured/left/image_raw/zdepth +/narrow_stereo_textured/left/image_rect +/narrow_stereo_textured/left/image_rect/compressed +/narrow_stereo_textured/left/image_rect/compressed/parameter_descriptions +/narrow_stereo_textured/left/image_rect/compressed/parameter_updates +/narrow_stereo_textured/left/image_rect/compressedDepth +/narrow_stereo_textured/left/image_rect/compressedDepth/parameter_descriptions +/narrow_stereo_textured/left/image_rect/compressedDepth/parameter_updates +/narrow_stereo_textured/left/image_rect/theora +/narrow_stereo_textured/left/image_rect/theora/parameter_descriptions +/narrow_stereo_textured/left/image_rect/theora/parameter_updates +/narrow_stereo_textured/left/image_rect/zdepth +/narrow_stereo_textured/left/image_rect_color +/narrow_stereo_textured/left/image_rect_color/compressed +/narrow_stereo_textured/left/image_rect_color/compressed/parameter_descriptions +/narrow_stereo_textured/left/image_rect_color/compressed/parameter_updates +/narrow_stereo_textured/left/image_rect_color/compressedDepth +/narrow_stereo_textured/left/image_rect_color/compressedDepth/parameter_descriptions +/narrow_stereo_textured/left/image_rect_color/compressedDepth/parameter_updates +/narrow_stereo_textured/left/image_rect_color/theora +/narrow_stereo_textured/left/image_rect_color/theora/parameter_descriptions +/narrow_stereo_textured/left/image_rect_color/theora/parameter_updates +/narrow_stereo_textured/left/image_rect_color/zdepth +/narrow_stereo_textured/narrow_stereo_textured_proc/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc/parameter_updates +/narrow_stereo_textured/narrow_stereo_textured_proc_debayer_left/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc_debayer_left/parameter_updates +/narrow_stereo_textured/narrow_stereo_textured_proc_debayer_right/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc_debayer_right/parameter_updates +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_color_left/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_color_left/parameter_updates +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_color_right/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_color_right/parameter_updates +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_mono_left/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_mono_left/parameter_updates +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_mono_right/parameter_descriptions +/narrow_stereo_textured/narrow_stereo_textured_proc_rectify_mono_right/parameter_updates +/narrow_stereo_textured/points2 +/narrow_stereo_textured/right/camera_info +/narrow_stereo_textured/right/image_color +/narrow_stereo_textured/right/image_color/compressed +/narrow_stereo_textured/right/image_color/compressed/parameter_descriptions +/narrow_stereo_textured/right/image_color/compressed/parameter_updates +/narrow_stereo_textured/right/image_color/compressedDepth +/narrow_stereo_textured/right/image_color/compressedDepth/parameter_descriptions +/narrow_stereo_textured/right/image_color/compressedDepth/parameter_updates +/narrow_stereo_textured/right/image_color/theora +/narrow_stereo_textured/right/image_color/theora/parameter_descriptions +/narrow_stereo_textured/right/image_color/theora/parameter_updates +/narrow_stereo_textured/right/image_color/zdepth +/narrow_stereo_textured/right/image_mono +/narrow_stereo_textured/right/image_mono/compressed +/narrow_stereo_textured/right/image_mono/compressed/parameter_descriptions +/narrow_stereo_textured/right/image_mono/compressed/parameter_updates +/narrow_stereo_textured/right/image_mono/compressedDepth +/narrow_stereo_textured/right/image_mono/compressedDepth/parameter_descriptions +/narrow_stereo_textured/right/image_mono/compressedDepth/parameter_updates +/narrow_stereo_textured/right/image_mono/theora +/narrow_stereo_textured/right/image_mono/theora/parameter_descriptions +/narrow_stereo_textured/right/image_mono/theora/parameter_updates +/narrow_stereo_textured/right/image_mono/zdepth +/narrow_stereo_textured/right/image_raw +/narrow_stereo_textured/right/image_raw/compressed +/narrow_stereo_textured/right/image_raw/compressed/parameter_descriptions +/narrow_stereo_textured/right/image_raw/compressed/parameter_updates +/narrow_stereo_textured/right/image_raw/compressedDepth +/narrow_stereo_textured/right/image_raw/compressedDepth/parameter_descriptions +/narrow_stereo_textured/right/image_raw/compressedDepth/parameter_updates +/narrow_stereo_textured/right/image_raw/theora +/narrow_stereo_textured/right/image_raw/theora/parameter_descriptions +/narrow_stereo_textured/right/image_raw/theora/parameter_updates +/narrow_stereo_textured/right/image_raw/zdepth +/narrow_stereo_textured/right/image_rect +/narrow_stereo_textured/right/image_rect/compressed +/narrow_stereo_textured/right/image_rect/compressed/parameter_descriptions +/narrow_stereo_textured/right/image_rect/compressed/parameter_updates +/narrow_stereo_textured/right/image_rect/compressedDepth +/narrow_stereo_textured/right/image_rect/compressedDepth/parameter_descriptions +/narrow_stereo_textured/right/image_rect/compressedDepth/parameter_updates +/narrow_stereo_textured/right/image_rect/theora +/narrow_stereo_textured/right/image_rect/theora/parameter_descriptions +/narrow_stereo_textured/right/image_rect/theora/parameter_updates +/narrow_stereo_textured/right/image_rect/zdepth +/narrow_stereo_textured/right/image_rect_color +/narrow_stereo_textured/right/image_rect_color/compressed +/narrow_stereo_textured/right/image_rect_color/compressed/parameter_descriptions +/narrow_stereo_textured/right/image_rect_color/compressed/parameter_updates +/narrow_stereo_textured/right/image_rect_color/compressedDepth +/narrow_stereo_textured/right/image_rect_color/compressedDepth/parameter_descriptions +/narrow_stereo_textured/right/image_rect_color/compressedDepth/parameter_updates +/narrow_stereo_textured/right/image_rect_color/theora +/narrow_stereo_textured/right/image_rect_color/theora/parameter_descriptions +/narrow_stereo_textured/right/image_rect_color/theora/parameter_updates +/narrow_stereo_textured/right/image_rect_color/zdepth +/navigation/cmd_vel +/navigation/cmd_vel/raw +/network/connected +/odom_teleop +/particlecloud +/power_board/state +/power_monitor/parameter_descriptions +/power_monitor/parameter_updates +/power_state +/pr1040/app_list +/pr1040/application/app_status +/pr1040/bond0/receive +/pr1040/bond0/receive_kbps +/pr1040/bond0/receive_mbps +/pr1040/bond0/transmit +/pr1040/bond0/transmit_kbps +/pr1040/bond0/transmit_mbps +/pr1040/lan0/receive +/pr1040/lan0/receive_kbps +/pr1040/lan0/receive_mbps +/pr1040/lan0/transmit +/pr1040/lan0/transmit_kbps +/pr1040/lan0/transmit_mbps +/pr1040/lan1/receive +/pr1040/lan1/receive_kbps +/pr1040/lan1/receive_mbps +/pr1040/lan1/transmit +/pr1040/lan1/transmit_kbps +/pr1040/lan1/transmit_mbps +/pr1040/lan2/receive +/pr1040/lan2/receive_kbps +/pr1040/lan2/receive_mbps +/pr1040/lan2/transmit +/pr1040/lan2/transmit_kbps +/pr1040/lan2/transmit_mbps +/pr1040/lan3/receive +/pr1040/lan3/receive_kbps +/pr1040/lan3/receive_mbps +/pr1040/lan3/transmit +/pr1040/lan3/transmit_kbps +/pr1040/lan3/transmit_mbps +/pr1040/lo/receive +/pr1040/lo/receive_kbps +/pr1040/lo/receive_mbps +/pr1040/lo/transmit +/pr1040/lo/transmit_kbps +/pr1040/lo/transmit_mbps +/pr1040/nonlocal/receive +/pr1040/nonlocal/receive_kbps +/pr1040/nonlocal/receive_mbps +/pr1040/nonlocal/transmit +/pr1040/nonlocal/transmit_kbps +/pr1040/nonlocal/transmit_mbps +/pr1040/wan0/receive +/pr1040/wan0/receive_kbps +/pr1040/wan0/receive_mbps +/pr1040/wan0/transmit +/pr1040/wan0/transmit_kbps +/pr1040/wan0/transmit_mbps +/pr2_ethercat/motors_halted +/pr2_move_base/cancel +/pr2_move_base/feedback +/pr2_move_base/goal +/pr2_move_base/result +/pr2_move_base/status +/pr2twit_from_tablet +/pressure/l_gripper_motor +/pressure/r_gripper_motor +/projector_controller/falling_edge_timestamps +/projector_controller/rising_edge_timestamps +/projector_trigger/off_time +/projector_trigger/on_time +/projector_trigger/waveform +/prosilica_inhibit_projector_controller/waveform +/r_arm_controller/command +/r_arm_controller/follow_joint_trajectory/cancel +/r_arm_controller/follow_joint_trajectory/feedback +/r_arm_controller/follow_joint_trajectory/goal +/r_arm_controller/follow_joint_trajectory/result +/r_arm_controller/follow_joint_trajectory/status +/r_arm_controller/gains/r_elbow_flex_joint/parameter_descriptions +/r_arm_controller/gains/r_elbow_flex_joint/parameter_updates +/r_arm_controller/gains/r_forearm_roll_joint/parameter_descriptions +/r_arm_controller/gains/r_forearm_roll_joint/parameter_updates +/r_arm_controller/gains/r_shoulder_lift_joint/parameter_descriptions +/r_arm_controller/gains/r_shoulder_lift_joint/parameter_updates +/r_arm_controller/gains/r_shoulder_pan_joint/parameter_descriptions +/r_arm_controller/gains/r_shoulder_pan_joint/parameter_updates +/r_arm_controller/gains/r_upper_arm_roll_joint/parameter_descriptions +/r_arm_controller/gains/r_upper_arm_roll_joint/parameter_updates +/r_arm_controller/gains/r_wrist_flex_joint/parameter_descriptions +/r_arm_controller/gains/r_wrist_flex_joint/parameter_updates +/r_arm_controller/gains/r_wrist_roll_joint/parameter_descriptions +/r_arm_controller/gains/r_wrist_roll_joint/parameter_updates +/r_arm_controller/joint_trajectory_action/cancel +/r_arm_controller/joint_trajectory_action/feedback +/r_arm_controller/joint_trajectory_action/goal +/r_arm_controller/joint_trajectory_action/result +/r_arm_controller/joint_trajectory_action/status +/r_arm_controller/state +/r_arm_controller_loose/command +/r_arm_controller_loose/state +/r_forearm_cam/camera_info +/r_forearm_cam/image_color +/r_forearm_cam/image_color/compressed +/r_forearm_cam/image_color/compressed/parameter_descriptions +/r_forearm_cam/image_color/compressed/parameter_updates +/r_forearm_cam/image_color/compressedDepth +/r_forearm_cam/image_color/compressedDepth/parameter_descriptions +/r_forearm_cam/image_color/compressedDepth/parameter_updates +/r_forearm_cam/image_color/theora +/r_forearm_cam/image_color/theora/parameter_descriptions +/r_forearm_cam/image_color/theora/parameter_updates +/r_forearm_cam/image_color/zdepth +/r_forearm_cam/image_mono +/r_forearm_cam/image_mono/compressed +/r_forearm_cam/image_mono/compressed/parameter_descriptions +/r_forearm_cam/image_mono/compressed/parameter_updates +/r_forearm_cam/image_mono/compressedDepth +/r_forearm_cam/image_mono/compressedDepth/parameter_descriptions +/r_forearm_cam/image_mono/compressedDepth/parameter_updates +/r_forearm_cam/image_mono/theora +/r_forearm_cam/image_mono/theora/parameter_descriptions +/r_forearm_cam/image_mono/theora/parameter_updates +/r_forearm_cam/image_mono/zdepth +/r_forearm_cam/image_proc_debayer/parameter_descriptions +/r_forearm_cam/image_proc_debayer/parameter_updates +/r_forearm_cam/image_proc_rectify_color/parameter_descriptions +/r_forearm_cam/image_proc_rectify_color/parameter_updates +/r_forearm_cam/image_proc_rectify_mono/parameter_descriptions +/r_forearm_cam/image_proc_rectify_mono/parameter_updates +/r_forearm_cam/image_raw +/r_forearm_cam/image_raw/compressed +/r_forearm_cam/image_raw/compressed/parameter_descriptions +/r_forearm_cam/image_raw/compressed/parameter_updates +/r_forearm_cam/image_raw/compressedDepth +/r_forearm_cam/image_raw/compressedDepth/parameter_descriptions +/r_forearm_cam/image_raw/compressedDepth/parameter_updates +/r_forearm_cam/image_raw/theora +/r_forearm_cam/image_raw/theora/parameter_descriptions +/r_forearm_cam/image_raw/theora/parameter_updates +/r_forearm_cam/image_raw/zdepth +/r_forearm_cam/image_rect +/r_forearm_cam/image_rect/compressed +/r_forearm_cam/image_rect/compressed/parameter_descriptions +/r_forearm_cam/image_rect/compressed/parameter_updates +/r_forearm_cam/image_rect/compressedDepth +/r_forearm_cam/image_rect/compressedDepth/parameter_descriptions +/r_forearm_cam/image_rect/compressedDepth/parameter_updates +/r_forearm_cam/image_rect/theora +/r_forearm_cam/image_rect/theora/parameter_descriptions +/r_forearm_cam/image_rect/theora/parameter_updates +/r_forearm_cam/image_rect/zdepth +/r_forearm_cam/image_rect_color +/r_forearm_cam/image_rect_color/compressed +/r_forearm_cam/image_rect_color/compressed/parameter_descriptions +/r_forearm_cam/image_rect_color/compressed/parameter_updates +/r_forearm_cam/image_rect_color/compressedDepth +/r_forearm_cam/image_rect_color/compressedDepth/parameter_descriptions +/r_forearm_cam/image_rect_color/compressedDepth/parameter_updates +/r_forearm_cam/image_rect_color/theora +/r_forearm_cam/image_rect_color/theora/parameter_descriptions +/r_forearm_cam/image_rect_color/theora/parameter_updates +/r_forearm_cam/image_rect_color/zdepth +/r_forearm_cam/parameter_descriptions +/r_forearm_cam/parameter_updates +/r_forearm_cam_trigger/waveform +/r_gripper_controller/command +/r_gripper_controller/gripper_action/cancel +/r_gripper_controller/gripper_action/feedback +/r_gripper_controller/gripper_action/goal +/r_gripper_controller/gripper_action/result +/r_gripper_controller/gripper_action/status +/r_gripper_controller/pid/parameter_descriptions +/r_gripper_controller/pid/parameter_updates +/r_gripper_controller/state +/r_gripper_sensor_controller/command +/r_gripper_sensor_controller/contact_state +/r_gripper_sensor_controller/event_detector +/r_gripper_sensor_controller/event_detector/cancel +/r_gripper_sensor_controller/event_detector/feedback +/r_gripper_sensor_controller/event_detector/goal +/r_gripper_sensor_controller/event_detector/result +/r_gripper_sensor_controller/event_detector/status +/r_gripper_sensor_controller/event_detector_state +/r_gripper_sensor_controller/find_contact +/r_gripper_sensor_controller/find_contact/cancel +/r_gripper_sensor_controller/find_contact/feedback +/r_gripper_sensor_controller/find_contact/goal +/r_gripper_sensor_controller/find_contact/result +/r_gripper_sensor_controller/find_contact/status +/r_gripper_sensor_controller/force_servo +/r_gripper_sensor_controller/force_servo/cancel +/r_gripper_sensor_controller/force_servo/feedback +/r_gripper_sensor_controller/force_servo/goal +/r_gripper_sensor_controller/force_servo/result +/r_gripper_sensor_controller/force_servo/status +/r_gripper_sensor_controller/force_servo_state +/r_gripper_sensor_controller/grab/cancel +/r_gripper_sensor_controller/grab/feedback +/r_gripper_sensor_controller/grab/goal +/r_gripper_sensor_controller/grab/result +/r_gripper_sensor_controller/grab/status +/r_gripper_sensor_controller/gripper_action/cancel +/r_gripper_sensor_controller/gripper_action/feedback +/r_gripper_sensor_controller/gripper_action/goal +/r_gripper_sensor_controller/gripper_action/result +/r_gripper_sensor_controller/gripper_action/status +/r_gripper_sensor_controller/release/cancel +/r_gripper_sensor_controller/release/feedback +/r_gripper_sensor_controller/release/goal +/r_gripper_sensor_controller/release/result +/r_gripper_sensor_controller/release/status +/r_gripper_sensor_controller/slip_servo +/r_gripper_sensor_controller/slip_servo/cancel +/r_gripper_sensor_controller/slip_servo/feedback +/r_gripper_sensor_controller/slip_servo/goal +/r_gripper_sensor_controller/slip_servo/result +/r_gripper_sensor_controller/slip_servo/status +/r_gripper_sensor_controller/slip_servo_state +/r_gripper_sensor_controller/state +/right_endeffector/wrench +/robot_interface_marker_array +/robot_pose_ekf/odom_combined +/robotsound +/robotsound/cancel +/robotsound/feedback +/robotsound/goal +/robotsound/result +/robotsound/status +/robotsound_jp +/robotsound_jp/cancel +/robotsound_jp/feedback +/robotsound_jp/goal +/robotsound_jp/result +/robotsound_jp/status +/rosout +/rosout_agg +/safe_teleop_base/local_costmap/costmap +/safe_teleop_base/local_costmap/costmap_updates +/safe_teleop_base/local_costmap/footprint +/safe_teleop_base/local_costmap/inflation_layer/parameter_descriptions +/safe_teleop_base/local_costmap/inflation_layer/parameter_updates +/safe_teleop_base/local_costmap/obstacle_layer/parameter_descriptions +/safe_teleop_base/local_costmap/obstacle_layer/parameter_updates +/safe_teleop_base/local_costmap/parameter_descriptions +/safe_teleop_base/local_costmap/parameter_updates +/safe_teleop_base/local_plan +/safe_teleop_base/user_plan +/scene_marker_array +/server_name/smach/container_status +/server_name/smach/container_structure +/smach_image_publisher/image +/smach_image_publisher/image/compressed +/smach_to_mail/parameter_descriptions +/smach_to_mail/parameter_updates +/sound_play/cancel +/sound_play/feedback +/sound_play/goal +/sound_play/result +/sound_play/status +/speech_audio +/speech_to_text +/speech_to_text_google +/speech_to_text_julius +/speech_to_text_mux/selected +/speech_to_text_other +/spots_marker_array +/teleop/cmd_vel +/teleop/cmd_vel/safe +/teleop/cmd_vel/unsafe +/teleop/joy_vel +/tf +/tf2_buffer_server/cancel +/tf2_buffer_server/feedback +/tf2_buffer_server/goal +/tf2_buffer_server/result +/tf2_buffer_server/status +/tf_static +/tilt_hokuyo_node/parameter_descriptions +/tilt_hokuyo_node/parameter_updates +/tilt_laser_mux/selected +/tilt_scan +/tilt_scan_filtered +/tilt_scan_filtered/navigation +/tilt_scan_filtered/scan +/tilt_scan_interpolated +/tilt_scan_shadow_filtered +/tilt_scan_throttled +/tilt_shadow_filter/dark_shadows/parameter_descriptions +/tilt_shadow_filter/dark_shadows/parameter_updates +/tilt_shadow_filter/shadows/parameter_descriptions +/tilt_shadow_filter/shadows/parameter_updates +/torso_controller/command +/torso_controller/command_deadman +/torso_controller/command_dummy +/torso_controller/follow_joint_trajectory/cancel +/torso_controller/follow_joint_trajectory/feedback +/torso_controller/follow_joint_trajectory/goal +/torso_controller/follow_joint_trajectory/result +/torso_controller/follow_joint_trajectory/status +/torso_controller/gains/torso_lift_joint/parameter_descriptions +/torso_controller/gains/torso_lift_joint/parameter_updates +/torso_controller/joint_trajectory_action/cancel +/torso_controller/joint_trajectory_action/feedback +/torso_controller/joint_trajectory_action/goal +/torso_controller/joint_trajectory_action/result +/torso_controller/joint_trajectory_action/status +/torso_controller/mux/selected +/torso_controller/position_joint_action/cancel +/torso_controller/position_joint_action/feedback +/torso_controller/position_joint_action/goal +/torso_controller/position_joint_action/result +/torso_controller/position_joint_action/status +/torso_controller/state +/torso_lift_imu/data +/torso_lift_imu/is_calibrated +/tuck_arms/cancel +/tuck_arms/feedback +/tuck_arms/goal +/tuck_arms/result +/tuck_arms/status +/tweet +/tweet_client_tablet/parameter_descriptions +/tweet_client_tablet/parameter_updates +/tweet_client_uptime/parameter_descriptions +/tweet_client_uptime/parameter_updates +/tweet_client_warning/parameter_descriptions +/tweet_client_warning/parameter_updates +/tweet_client_worktime/parameter_descriptions +/tweet_client_worktime/parameter_updates +/tweet_compressed_image_mux/selected +/tweet_image +/tweet_image/compressed +/tweet_image_mux/selected +/tweet_image_server/parameter_descriptions +/tweet_image_server/parameter_updates +/tweet_image_server/tweet/cancel +/tweet_image_server/tweet/feedback +/tweet_image_server/tweet/goal +/tweet_image_server/tweet/result +/tweet_image_server/tweet/status +/unsafe_vel_mux/selected +/vel_type_mux/selected +/visualization/battery/value0 +/visualization/battery/value1 +/visualization/battery/value2 +/visualization/battery/value3 +/voice_text/parameter_descriptions +/voice_text/parameter_updates +/wide_stereo/disparity +/wide_stereo/left/camera_info +/wide_stereo/left/image_color +/wide_stereo/left/image_color/compressed +/wide_stereo/left/image_color/compressed/parameter_descriptions +/wide_stereo/left/image_color/compressed/parameter_updates +/wide_stereo/left/image_color/compressedDepth +/wide_stereo/left/image_color/compressedDepth/parameter_descriptions +/wide_stereo/left/image_color/compressedDepth/parameter_updates +/wide_stereo/left/image_color/theora +/wide_stereo/left/image_color/theora/parameter_descriptions +/wide_stereo/left/image_color/theora/parameter_updates +/wide_stereo/left/image_color/zdepth +/wide_stereo/left/image_mono +/wide_stereo/left/image_mono/compressed +/wide_stereo/left/image_mono/compressed/parameter_descriptions +/wide_stereo/left/image_mono/compressed/parameter_updates +/wide_stereo/left/image_mono/compressedDepth +/wide_stereo/left/image_mono/compressedDepth/parameter_descriptions +/wide_stereo/left/image_mono/compressedDepth/parameter_updates +/wide_stereo/left/image_mono/theora +/wide_stereo/left/image_mono/theora/parameter_descriptions +/wide_stereo/left/image_mono/theora/parameter_updates +/wide_stereo/left/image_mono/zdepth +/wide_stereo/left/image_raw +/wide_stereo/left/image_raw/compressed +/wide_stereo/left/image_raw/compressed/parameter_descriptions +/wide_stereo/left/image_raw/compressed/parameter_updates +/wide_stereo/left/image_raw/compressedDepth +/wide_stereo/left/image_raw/compressedDepth/parameter_descriptions +/wide_stereo/left/image_raw/compressedDepth/parameter_updates +/wide_stereo/left/image_raw/theora +/wide_stereo/left/image_raw/theora/parameter_descriptions +/wide_stereo/left/image_raw/theora/parameter_updates +/wide_stereo/left/image_raw/zdepth +/wide_stereo/left/image_rect +/wide_stereo/left/image_rect/compressed +/wide_stereo/left/image_rect/compressed/parameter_descriptions +/wide_stereo/left/image_rect/compressed/parameter_updates +/wide_stereo/left/image_rect/compressedDepth +/wide_stereo/left/image_rect/compressedDepth/parameter_descriptions +/wide_stereo/left/image_rect/compressedDepth/parameter_updates +/wide_stereo/left/image_rect/theora +/wide_stereo/left/image_rect/theora/parameter_descriptions +/wide_stereo/left/image_rect/theora/parameter_updates +/wide_stereo/left/image_rect/zdepth +/wide_stereo/left/image_rect_color +/wide_stereo/left/image_rect_color/compressed +/wide_stereo/left/image_rect_color/compressed/parameter_descriptions +/wide_stereo/left/image_rect_color/compressed/parameter_updates +/wide_stereo/left/image_rect_color/compressedDepth +/wide_stereo/left/image_rect_color/compressedDepth/parameter_descriptions +/wide_stereo/left/image_rect_color/compressedDepth/parameter_updates +/wide_stereo/left/image_rect_color/theora +/wide_stereo/left/image_rect_color/theora/parameter_descriptions +/wide_stereo/left/image_rect_color/theora/parameter_updates +/wide_stereo/left/image_rect_color/zdepth +/wide_stereo/points2 +/wide_stereo/right/camera_info +/wide_stereo/right/image_color +/wide_stereo/right/image_color/compressed +/wide_stereo/right/image_color/compressed/parameter_descriptions +/wide_stereo/right/image_color/compressed/parameter_updates +/wide_stereo/right/image_color/compressedDepth +/wide_stereo/right/image_color/compressedDepth/parameter_descriptions +/wide_stereo/right/image_color/compressedDepth/parameter_updates +/wide_stereo/right/image_color/theora +/wide_stereo/right/image_color/theora/parameter_descriptions +/wide_stereo/right/image_color/theora/parameter_updates +/wide_stereo/right/image_color/zdepth +/wide_stereo/right/image_mono +/wide_stereo/right/image_mono/compressed +/wide_stereo/right/image_mono/compressed/parameter_descriptions +/wide_stereo/right/image_mono/compressed/parameter_updates +/wide_stereo/right/image_mono/compressedDepth +/wide_stereo/right/image_mono/compressedDepth/parameter_descriptions +/wide_stereo/right/image_mono/compressedDepth/parameter_updates +/wide_stereo/right/image_mono/theora +/wide_stereo/right/image_mono/theora/parameter_descriptions +/wide_stereo/right/image_mono/theora/parameter_updates +/wide_stereo/right/image_mono/zdepth +/wide_stereo/right/image_raw +/wide_stereo/right/image_raw/compressed +/wide_stereo/right/image_raw/compressed/parameter_descriptions +/wide_stereo/right/image_raw/compressed/parameter_updates +/wide_stereo/right/image_raw/compressedDepth +/wide_stereo/right/image_raw/compressedDepth/parameter_descriptions +/wide_stereo/right/image_raw/compressedDepth/parameter_updates +/wide_stereo/right/image_raw/theora +/wide_stereo/right/image_raw/theora/parameter_descriptions +/wide_stereo/right/image_raw/theora/parameter_updates +/wide_stereo/right/image_raw/zdepth +/wide_stereo/right/image_rect +/wide_stereo/right/image_rect/compressed +/wide_stereo/right/image_rect/compressed/parameter_descriptions +/wide_stereo/right/image_rect/compressed/parameter_updates +/wide_stereo/right/image_rect/compressedDepth +/wide_stereo/right/image_rect/compressedDepth/parameter_descriptions +/wide_stereo/right/image_rect/compressedDepth/parameter_updates +/wide_stereo/right/image_rect/theora +/wide_stereo/right/image_rect/theora/parameter_descriptions +/wide_stereo/right/image_rect/theora/parameter_updates +/wide_stereo/right/image_rect/zdepth +/wide_stereo/right/image_rect_color +/wide_stereo/right/image_rect_color/compressed +/wide_stereo/right/image_rect_color/compressed/parameter_descriptions +/wide_stereo/right/image_rect_color/compressed/parameter_updates +/wide_stereo/right/image_rect_color/compressedDepth +/wide_stereo/right/image_rect_color/compressedDepth/parameter_descriptions +/wide_stereo/right/image_rect_color/compressedDepth/parameter_updates +/wide_stereo/right/image_rect_color/theora +/wide_stereo/right/image_rect_color/theora/parameter_descriptions +/wide_stereo/right/image_rect_color/theora/parameter_updates +/wide_stereo/right/image_rect_color/zdepth +/wide_stereo/wide_stereo_proc/parameter_descriptions +/wide_stereo/wide_stereo_proc/parameter_updates +/wide_stereo/wide_stereo_proc_debayer_left/parameter_descriptions +/wide_stereo/wide_stereo_proc_debayer_left/parameter_updates +/wide_stereo/wide_stereo_proc_debayer_right/parameter_descriptions +/wide_stereo/wide_stereo_proc_debayer_right/parameter_updates +/wide_stereo/wide_stereo_proc_rectify_color_left/parameter_descriptions +/wide_stereo/wide_stereo_proc_rectify_color_left/parameter_updates +/wide_stereo/wide_stereo_proc_rectify_color_right/parameter_descriptions +/wide_stereo/wide_stereo_proc_rectify_color_right/parameter_updates +/wide_stereo/wide_stereo_proc_rectify_mono_left/parameter_descriptions +/wide_stereo/wide_stereo_proc_rectify_mono_left/parameter_updates +/wide_stereo/wide_stereo_proc_rectify_mono_right/parameter_descriptions +/wide_stereo/wide_stereo_proc_rectify_mono_right/parameter_updates +/wide_stereo_both/parameter_descriptions +/wide_stereo_both/parameter_updates +/wide_stereo_left/parameter_descriptions +/wide_stereo_left/parameter_updates +/wide_stereo_right/parameter_descriptions +/wide_stereo_right/parameter_updates +[http://pr1040:11311][133.11.216.23] mech-user@ki00119:~/semi_ws/src$ rostopic echo cmd_vel +WARNING: topic [/cmd_vel] does not appear to be published yet +WARNING: topic [/cmd_vel] does not appear to be published yet C-c C-c[http://pr1040:11311][133.11.216.23] mech-user@ki00119:~/semi_ws/src$ rostopic echo /cmd_vel +WARNING: topic [/cmd_vel] does not appear to be published yet +WARNING: topic [/cmd_vel] does not appear to be published yet C-c C-c[http://pr1040:11311][133.11.216.23] mech-user@ki00119:~/semi_ws/src$ rostopic echo /teleop/cmd_vel +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.08775347471237183 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.08775347471237183 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.12523576021194457 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.12523576021194457 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.11585824489593506 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.1064807415008545 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.09710323810577393 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.09710323810577393 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.7156579494476318 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.7156579494476318 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.6594206333160401 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.38766720294952395 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.0 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.06897072196006775 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.06897072196006775 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.6031833648681642 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.6031833648681642 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: -0.0 + y: -0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: -0.8 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- +linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0 +--- + C-c C-c[http://pr1040:11311][133.11.216.23] mech-user@ki00119:~/semi_ws/src$ roseus +configuring by "/opt/ros/noetic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph gnuplotlib ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x55e5a09ab680[16374] --> 0x55e5a0e3bc90[32748] top=3d4b +irtgl irtglc irtviewer +EusLisp 9.29( 1.2.5) for Linux64 created on ip-10-0-1-5(Tue Jun 20 10:34:21 PST 2023) +roseus ;; loading roseus("1.7.5") on euslisp((9.29 ip-10-0-1-5 Tue Jun 20 10:34:21 PST 2023 1.2.5)) +eustf roseus_c_util 1.irteusgl$ (load "first_motion.l") +;; (make-irtviewer) executed +t +2.irteusgl$ +(require "package://pr2eus/pr2-interface.l") ;;import pr2 package + +nil +2.irteusgl$ ;; roseus_resume is disabled. +:|PACKAGE://PR2EUS/PR2-INTERFACE.L| +3.irteusgl$ nil +4.irteusgl$ nil +4.irteusgl$ pr2-init + +;; extending gcstack 0x55e5a0e3bc90[32738] --> 0x55e5a57c6540[65476] top=7d95 +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +nil +5.irteusgl$ (send *ri* :angle-vector (send *pr2* :angle-vector)) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +;; # :joint-angle(-5.34142) violate max-angle(-5.72958) +;; # :joint-angle(-123.607) violate min-angle(-121.542) +;; # :joint-angle(-23.16) violate min-angle(-21.2682) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.512) violate min-angle(-121.542) +;; # :joint-angle(-5.38629) violate max-angle(-5.72958) +#f(182.874 3.88879 68.4486 104.412 -81.4114 -98.4558 -5.72958 -4.6688 0.452866 63.2517 -79.7933 -121.542 -100.615 -105.832 12.949 5.53153 -21.2682) +6.irteusgl$ (load "first_motion.l") +t +7.irteusgl$ (send *ri* :angle-vector (send *pr2* :angle-vector)) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.47) violate min-angle(-121.542) +;; # :joint-angle(-5.72782) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.47) violate min-angle(-121.542) +;; # :joint-angle(-5.72782) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.47) violate min-angle(-121.542) +;; # :joint-angle(-5.72782) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.47) violate min-angle(-121.542) +;; # :joint-angle(-5.72782) violate max-angle(-5.72958) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.47) violate min-angle(-121.542) +;; # :joint-angle(-5.72782) violate max-angle(-5.72958) +;; # :joint-angle(-21.2682) violate min-angle(-21.2682) +;; # :joint-angle(-22.158) violate min-angle(-21.2682) +;; # :joint-angle(-122.47) violate min-angle(-121.542) +;; # :joint-angle(-5.72782) violate max-angle(-5.72958) +#f(300.0 60.0 10.0 120.0 -120.0 20.0 -30.0 0.0 -60.0 -20.0 -120.0 -120.0 0.0 -40.0 0.0 0.0 0.0) +8.irteusgl$ (send *ri* :wait-interpolation) +[ INFO] [1729665278.625315427]: wait-interpolation debug: start +[ERROR] [1729665278.658195900]: joint trajectory status: (4 . actionlib_msgs::goalstatus::*aborted*) + +[ERROR] [1729665278.658251240]: error_code: (-5 . GOAL_TOLERANCE_VIOLATED) + +[ INFO] [1729665278.662522301]: wait-interpolation debug: end +(nil nil nil nil) +9.irteusgl$ (load "first_motion.l") +[ INFO] [1729665371.924329834]: wait-interpolation debug: start +[ERROR] [1729665391.152011698]: joint trajectory status: (4 . actionlib_msgs::goalstatus::*aborted*) + +[ERROR] [1729665391.152091968]: error_code: (-5 . GOAL_TOLERANCE_VIOLATED) + +[ INFO] [1729665391.152622378]: wait-interpolation debug: end +Call Stack (max depth: 20): + 0: at (send *irtv0) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (apply #'ros::load-org-for-ros ros::fullname args) + 3: at (let ((ros::fullname fname)) (when (and (stringp fname) (substringp "package://" fname)) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 4: at (load "first_motion.l") + 5: at # +/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *irtv0 in (send *irtv0) +10.E1-irteusgl$ reset +11.irteusgl$ (load "first_motion.l") +[ INFO] [1729665412.405603378]: wait-interpolation debug: start +[ INFO] [1729665413.445186124]: wait-interpolation debug: end +[ INFO] [1729665413.457923364]: wait-interpolation debug: start +[ INFO] [1729665414.496236238]: wait-interpolation debug: end +t +12.irteusgl$ (send *ri* :stop-grasp :arms) +(t t) +13.irteusgl$ (load "first_motion.l") +[ INFO] [1729665532.834773457]: wait-interpolation debug: start +[ INFO] [1729665533.934082201]: wait-interpolation debug: end +[ INFO] [1729665533.953203205]: wait-interpolation debug: start +[ INFO] [1729665535.016466058]: wait-interpolation debug: end +[ WARN] [1729665535.057218743]: [l_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665535.057659665]: [r_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665535.058899420]: [torso_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665535.061647748]: [head_traj_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665535.061957258]: [head_traj_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665535.062237995]: [head_traj_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ INFO] [1729665537.552695448]: wait-interpolation debug: start +[ INFO] [1729665537.553253665]: wait-interpolation debug: end +[ INFO] [1729665537.564546634]: wait-interpolation debug: start +[ INFO] [1729665538.724101798]: wait-interpolation debug: end +[ INFO] [1729665538.737843726]: wait-interpolation debug: start +[ WARN] [1729665538.821487506]: [torso_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665538.822414806]: [torso_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665538.828988142]: [r_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665538.829903044]: [l_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ INFO] [1729665539.849467686]: wait-interpolation debug: end +[ INFO] [1729665539.861754963]: wait-interpolation debug: start +[ INFO] [1729665540.953058540]: wait-interpolation debug: end +[ INFO] [1729665540.969119606]: wait-interpolation debug: start +[ INFO] [1729665542.035728532]: wait-interpolation debug: end +[ INFO] [1729665542.045338513]: wait-interpolation debug: start +[ INFO] [1729665543.169270450]: wait-interpolation debug: end +[ INFO] [1729665543.182459409]: wait-interpolation debug: start +[ WARN] [1729665543.244370062]: [head_traj_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665543.245664202]: [l_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665543.250024477]: [torso_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665543.259581768]: [r_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ INFO] [1729665544.229971396]: wait-interpolation debug: end +[ INFO] [1729665544.240476576]: wait-interpolation debug: start +[ WARN] [1729665544.258518085]: [r_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665544.260639341]: [l_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ INFO] [1729665545.290826686]: wait-interpolation debug: end +[ INFO] [1729665545.300205633]: wait-interpolation debug: start +[ INFO] [1729665546.343070104]: wait-interpolation debug: end +[ INFO] [1729665547.401782224]: wait-interpolation debug: start +[ INFO] [1729665548.449819092]: wait-interpolation debug: end +[ INFO] [1729665548.471123723]: wait-interpolation debug: start +[ INFO] [1729665549.540332440]: wait-interpolation debug: end +[ INFO] [1729665549.558752173]: wait-interpolation debug: start +[ INFO] [1729665550.593625320]: wait-interpolation debug: end +[ WARN] [1729665550.618664274]: [l_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665550.619075040]: [r_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ INFO] [1729665550.620444971]: wait-interpolation debug: start +[ INFO] [1729665551.662678868]: wait-interpolation debug: end +[ INFO] [1729665551.674839178]: wait-interpolation debug: start +[ WARN] [1729665551.690175755]: [l_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1729665551.690608557]: [r_arm_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ INFO] [1729665552.819662941]: wait-interpolation debug: end +[ INFO] [1729665552.849406243]: wait-interpolation debug: start +[ERROR] [1729665564.452784076]: joint trajectory status: (4 . actionlib_msgs::goalstatus::*aborted*) + +[ERROR] [1729665564.452839911]: error_code: (-5 . GOAL_TOLERANCE_VIOLATED) + +[ INFO] [1729665564.454964335]: wait-interpolation debug: end +[ INFO] [1729665567.835068546]: wait-interpolation debug: start +[ERROR] [1729665575.595436467]: joint trajectory status: (4 . actionlib_msgs::goalstatus::*aborted*) + +[ERROR] [1729665575.595495803]: error_code: (-5 . GOAL_TOLERANCE_VIOLATED) + +[ INFO] [1729665575.597102187]: wait-interpolation debug: end +t +14.irteusgl$ + + + +mech-user@ki00119:~/semi_ws/src$ git clone https://github.com/jsk-ros-pkg/jsk_robot +Cloning into 'jsk_robot'... +remote: Enumerating objects: 38978, done. +remote: Counting objects: 100% (5743/5743), done. +remote: Compressing objects: 100% (1842/1842), done. +remote: Total 38978 (delta 4058), reused 5421 (delta 3884), pack-reused 33235 (from 1) +Receiving objects: 100% (38978/38978), 179.66 MiB | 24.21 MiB/s, done. +Resolving deltas: 100% (25707/25707), done. +mech-user@ki00119:~/semi_ws/src$ ls +jsk_robot +mech-user@ki00119:~/semi_ws/src$ ls +jsk_robot +mech-user@ki00119:~/semi_ws/src$ rm -fr jsk_robot/ +mech-user@ki00119:~/semi_ws/src$ cd .. +mech-user@ki00119:~/semi_ws$ git clone https://github.com/jsk-ros-pkg/jsk_demos -b jsk_2024_semi +Cloning into 'jsk_demos'... +fatal: Remote branch jsk_2024_semi not found in upstream origin +mech-user@ki00119:~/semi_ws$ git clone https://github.com/jsk-ros-pkg/jsk_demos +Cloning into 'jsk_demos'... +remote: Enumerating objects: 32410, done. +remote: Counting objects: 100% (6/6), done. +remote: Compressing objects: 100% (5/5), done. +remote: Total 32410 (delta 0), reused 5 (delta 0), pack-reused 32404 (from 1) +Receiving objects: 100% (32410/32410), 177.20 MiB | 23.39 MiB/s, done. +Resolving deltas: 100% (23469/23469), done. +cd j mech-user@ki00119:~/semi_ws$ cd jsk_demos/ +mech-user@ki00119:~/semi_ws/jsk_demos$ ls +2010_05_pr2_workshop jsk_2013_04_pr2_610 jsk_demo_common +README.md jsk_2013_05_pr2_tatu jsk_maps +detect_cans_in_fridge_201202 jsk_2014_06_pr2_drcbox jsk_semantic_maps +diabolo_pr2_201806 jsk_2015_06_hrp_drc rwt_teleop +elevator_move_base_pr2 jsk_2017_10_semi welcome_to_jsk_fetch +interactive_behavior_201409 jsk_2019_10_semi +jsk_2011_07_pr2_semantic jsk_2022_08_miraikan_demo +mech-user@ki00119:~/semi_ws/jsk_demos$ git checkout jsk_2024_10_semi +Branch 'jsk_2024_10_semi' set up to track remote branch 'jsk_2024_10_semi' from 'origin'. +Switched to a new branch 'jsk_2024_10_semi' +mech-user@ki00119:~/semi_ws/jsk_demos$ ls +2010_05_pr2_workshop jsk_2013_04_pr2_610 jsk_2024_10_semi +README.md jsk_2013_05_pr2_tatu jsk_demo_common +detect_cans_in_fridge_201202 jsk_2014_06_pr2_drcbox jsk_maps +diabolo_pr2_201806 jsk_2015_06_hrp_drc jsk_semantic_maps +elevator_move_base_pr2 jsk_2017_10_semi rwt_teleop +interactive_behavior_201409 jsk_2019_10_semi welcome_to_jsk_fetch +jsk_2011_07_pr2_semantic jsk_2022_08_miraikan_demo +mech-user@ki00119:~/semi_ws/jsk_demos$ cd ../src/ +mech-user@ki00119:~/semi_ws/src$ ls +mech-user@ki00119:~/semi_ws/src$ ln -sf ../jsk_demos/jsk_2024_10_semi/ +mech-user@ki00119:~/semi_ws/src$ ls +jsk_2024_10_semi +mech-user@ki00119:~/semi_ws/src$ cd jsk_2024_10_semi +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt README.md package.xml +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ catkin b t-vi +ca==> Expanding alias 'b' from 'catkin b t-vi' to 'catkin build t-vi' +Initialized new catkin workspace in `/home/mech-user/semi_ws/jsk_demos/jsk_2024_10_semi` +[build] Error: Unable to find source space `/home/mech-user/semi_ws/jsk_demos/jsk_2024_10_semi/src` +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ catkin bt -vi +==> Expanding alias 'bt' from 'catkin bt -vi' to 'catkin b --this -vi' +==> Expanding alias 'b' from 'catkin b --this -vi' to 'catkin build --this -vi' +[build] Error: In order to use --this, the current directory must be part of a catkin package. +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ cd ../../ +mech-user@ki00119:~/semi_ws$ catkin b +==> Expanding alias 'b' from 'catkin b' to 'catkin build' +Initialized new catkin workspace in `/home/mech-user/semi_ws` +------------------------------------------------------------ +Profile: default +Extending: [env] /opt/ros/noetic +Workspace: /home/mech-user/semi_ws +------------------------------------------------------------ +Build Space: [exists] /home/mech-user/semi_ws/build +Devel Space: [exists] /home/mech-user/semi_ws/devel +Install Space: [unused] /home/mech-user/semi_ws/install +Log Space: [missing] /home/mech-user/semi_ws/logs +Source Space: [exists] /home/mech-user/semi_ws/src +DESTDIR: [unused] None +------------------------------------------------------------ +Devel Space Layout: linked +Install Space Layout: None +------------------------------------------------------------ +Additional CMake Args: None +Additional Make Args: None +Additional catkin Make Args: None +Internal Make Job Server: True +Cache Job Environments: False +------------------------------------------------------------ +Buildlisted Packages: None +Skiplisted Packages: None +------------------------------------------------------------ +Workspace configuration appears valid. + +NOTE: Forcing CMake to run for each package. +------------------------------------------------------------ +[build] Found 1 packages in 0.0 seconds. +[build] Updating package table. +Starting >>> catkin_tools_prebuild +Finished <<< catkin_tools_prebuild [ 2.2 seconds ] +Starting >>> jsk_2024_10_semi +Finished <<< jsk_2024_10_semi [ 1.8 seconds ] +[build] Summary: All 2 packages succeeded! +[build] Ignored: None. +[build] Warnings: None. +[build] Abandoned: None. +[build] Failed: None. +[build] Runtime: 4.1 seconds total. +[build] Note: Workspace packages have changed, please re-source setup files to use them. +mech-user@ki00119:~/semi_ws$ source devel/setup.bash +mech-user@ki00119:~/semi_ws$ roscd jsk_2024_10_semi/ +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt README.md package.xml +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote -v +origin https://github.com/jsk-ros-pkg/jsk_demos (fetch) +origin https://github.com/jsk-ros-pkg/jsk_demos (push) +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ cp ~/semi_ws.bak/ +.catkin_tools/ devel/ src/ +.git/ homework241016.webm task.txt +README.md homework_uml.png +build/ logs/ +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ cp ~/semi_ws.bak/src/ +demo.l first_motion.l~ homework241016.l +first_motion.l hampen.mov homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ cp ~/semi_ws.bak/src/* . +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt demo.l first_motion.l~ homework241016.l package.xml +README.md first_motion.l hampen.mov homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git diff +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt demo.l first_motion.l~ homework241016.l package.xml +README.md first_motion.l hampen.mov homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git add --all +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git commit -m"add 1023" +[jsk_2024_10_semi 295d7a47] add 1023 + 9 files changed, 496 insertions(+) + create mode 100644 jsk_2024_10_semi/.catkin_tools/CATKIN_IGNORE + create mode 100644 jsk_2024_10_semi/.catkin_tools/README + create mode 100644 jsk_2024_10_semi/.catkin_tools/VERSION + create mode 100644 jsk_2024_10_semi/.catkin_tools/profiles/profiles.yaml + create mode 100644 jsk_2024_10_semi/demo.l + create mode 100644 jsk_2024_10_semi/first_motion.l + create mode 100644 jsk_2024_10_semi/hampen.mov + create mode 100644 jsk_2024_10_semi/homework241016.l + create mode 100644 jsk_2024_10_semi/homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git push +Username for 'https://github.com': ^C +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ rm -fr .catkin_tools/ +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git rm .catkin_tools/ +fatal: not removing '.catkin_tools/' recursively without -r +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git rm -r .catkin_tools/ +rm 'jsk_2024_10_semi/.catkin_tools/CATKIN_IGNORE' +rm 'jsk_2024_10_semi/.catkin_tools/README' +rm 'jsk_2024_10_semi/.catkin_tools/VERSION' +rm 'jsk_2024_10_semi/.catkin_tools/profiles/profiles.yaml' +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git commit --amend +[jsk_2024_10_semi a0d98162] add 1023 + Date: Wed Oct 23 15:56:46 2024 +0900 + 5 files changed, 481 insertions(+) + create mode 100644 jsk_2024_10_semi/demo.l + create mode 100644 jsk_2024_10_semi/first_motion.l + create mode 100644 jsk_2024_10_semi/hampen.mov + create mode 100644 jsk_2024_10_semi/homework241016.l + create mode 100644 jsk_2024_10_semi/homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt demo.l first_motion.l~ homework241016.l package.xml +README.md first_motion.l hampen.mov homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls -al +total 195140 +drwxrwxr-x 2 mech-user mech-user 4096 10月 23 15:57 . +drwxrwxr-x 24 mech-user mech-user 4096 10月 23 15:55 .. +-rw-rw-r-- 1 mech-user mech-user 206 10月 23 15:55 CMakeLists.txt +-rw-rw-r-- 1 mech-user mech-user 21 10月 23 15:55 README.md +-rw-rw-r-- 1 mech-user mech-user 184 10月 23 15:55 demo.l +-rw-rw-r-- 1 mech-user mech-user 8480 10月 23 15:55 first_motion.l +-rw-rw-r-- 1 mech-user mech-user 449 10月 23 15:55 first_motion.l~ +-rw-rw-r-- 1 mech-user mech-user 199762796 10月 23 15:55 hampen.mov +-rw-rw-r-- 1 mech-user mech-user 6555 10月 23 15:55 homework241016.l +-rw-rw-r-- 1 mech-user mech-user 843 10月 23 15:55 homework_uml.act +-rw-rw-r-- 1 mech-user mech-user 438 10月 23 15:55 package.xml +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git push +Username for 'https://github.com': ^C +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt demo.l first_motion.l~ homework241016.l package.xml +README.md first_motion.l hampen.mov homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote add Michi-Tsubaki https://github.com//Michi-Tsubaki/jsk_demos.git +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git push Michi-Tsubaki jsk_2024_10_semi +Username for 'https://github.com': ^C +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote -v +Michi-Tsubaki https://github.com//Michi-Tsubaki/jsk_demos.git (fetch) +Michi-Tsubaki https://github.com//Michi-Tsubaki/jsk_demos.git (push) +origin https://github.com/jsk-ros-pkg/jsk_demos (fetch) +origin https://github.com/jsk-ros-pkg/jsk_demos (push) +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote add Michi-Tsubaki git@github.com:Michi-Tsubaki/jsk_demos.git +fatal: remote Michi-Tsubaki already exists. +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote del Michi-Tsubaki +error: Unknown subcommand: del +usage: git remote [-v | --verbose] + or: git remote add [-t ] [-m ] [-f] [--tags | --no-tags] [--mirror=] + or: git remote rename + or: git remote remove + or: git remote set-head (-a | --auto | -d | --delete | ) + or: git remote [-v | --verbose] show [-n] + or: git remote prune [-n | --dry-run] + or: git remote [-v | --verbose] update [-p | --prune] [( | )...] + or: git remote set-branches [--add] ... + or: git remote get-url [--push] [--all] + or: git remote set-url [--push] [] + or: git remote set-url --add + or: git remote set-url --delete + + -v, --verbose be verbose; must be placed before a subcommand + +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote remove Michi-Tsubaki +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git remote add Michi-Tsubaki git@github.com:Michi-Tsubaki/jsk_demos.git +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git push Michi-Tsubaki jsk_2024_10_semi +Enumerating objects: 10, done. +Counting objects: 100% (10/10), done. +Delta compression using up to 8 threads +Compressing objects: 100% (8/8), done. +Writing objects: 100% (8/8), 190.28 MiB | 3.34 MiB/s, done. +Total 8 (delta 2), reused 0 (delta 0) +remote: Resolving deltas: 100% (2/2), completed with 1 local object. +remote: error: Trace: 9a75dffa0e412693f5af2d6fce134396be0f6820799246e55dcd8d745f26056c +remote: error: See https://gh.io/lfs for more information. +remote: error: File jsk_2024_10_semi/hampen.mov is 190.51 MB; this exceeds GitHub's file size limit of 100.00 MB +remote: error: GH001: Large files detected. You may want to try Git Large File Storage - https://git-lfs.github.com. +To github.com:Michi-Tsubaki/jsk_demos.git + ! [remote rejected] jsk_2024_10_semi -> jsk_2024_10_semi (pre-receive hook declined) +error: failed to push some refs to 'git@github.com:Michi-Tsubaki/jsk_demos.git' +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt demo.l first_motion.l~ homework241016.l package.xml +README.md first_motion.l hampen.mov homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ cp hampen.mov .. +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git rm hampen.mov +rm 'jsk_2024_10_semi/hampen.mov' +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git commit --amend +[jsk_2024_10_semi d0dedc45] add 1023 + Date: Wed Oct 23 15:56:46 2024 +0900 + 4 files changed, 481 insertions(+) + create mode 100644 jsk_2024_10_semi/demo.l + create mode 100644 jsk_2024_10_semi/first_motion.l + create mode 100644 jsk_2024_10_semi/homework241016.l + create mode 100644 jsk_2024_10_semi/homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ git push Michi-Tsubaki jsk_2024_10_semi +Enumerating objects: 9, done. +Counting objects: 100% (9/9), done. +Delta compression using up to 8 threads +Compressing objects: 100% (7/7), done. +Writing objects: 100% (7/7), 2.23 KiB | 2.23 MiB/s, done. +Total 7 (delta 2), reused 0 (delta 0) +remote: Resolving deltas: 100% (2/2), completed with 1 local object. +To github.com:Michi-Tsubaki/jsk_demos.git + a6024b89..d0dedc45 jsk_2024_10_semi -> jsk_2024_10_semi +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ ls +CMakeLists.txt demo.l first_motion.l~ homework_uml.act +README.md first_motion.l homework241016.l package.xml +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ emacs homework.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ emacs homework_uml.act +^Z +[1]+ Stopped emacs homework_uml.act +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ emacs memo1023.txt +mech-user@ki00119:~/semi_ws/src/jsk_2024_10_semi$ fg +emacs homework_uml.act diff --git a/jsk_2024_10_semi/first_task/uml1.png b/jsk_2024_10_semi/first_task/uml1.png new file mode 100644 index 000000000..b03c62adf Binary files /dev/null and b/jsk_2024_10_semi/first_task/uml1.png differ diff --git a/jsk_2024_10_semi/homework241016.l b/jsk_2024_10_semi/homework241016.l new file mode 100644 index 000000000..178181037 --- /dev/null +++ b/jsk_2024_10_semi/homework241016.l @@ -0,0 +1,186 @@ +;;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 +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* +;;boudp 'xx : xxという変数に値が代入されているか +;;setq xx yy : xxとう変数にyyという値を代入する + +(send *pr2* :reset-pose) ;; 初期姿勢 + +(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 *pr2* :larm :shoulder-p :joint-angle 54) +(send *pr2* :rarm :shoulder-p :joint-angle 54) +(send *irtviewer* :draw-objects) +(unix:sleep 1) + +(send *pr2* :larm :shoulder-p :joint-angle 34) +(send *pr2* :rarm :shoulder-p :joint-angle 34) +(send *irtviewer* :draw-objects) +(unix:sleep 1) + +(send *pr2* :larm :shoulder-p :joint-angle 20) +(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 0) +(send *irtviewer* :draw-objects) +(unix:sleep 1) + +(send *pr2* :larm :shoulder-p :joint-angle 10) +(send *pr2* :rarm :shoulder-p :joint-angle -20) +(send *irtviewer* :draw-objects) +(unix:sleep 1) + +;;徐々に型を下げる(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 *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 *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 *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 :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 *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 *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 *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) + +;;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 *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 *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) + + diff --git a/jsk_2024_10_semi/homework241023.l b/jsk_2024_10_semi/homework241023.l new file mode 100755 index 000000000..488940f25 --- /dev/null +++ b/jsk_2024_10_semi/homework241023.l @@ -0,0 +1,202 @@ +#!/usr/bin/env roseus + +;chmod u+x ./surgery_motion_1.l --> ./surgery_motion_1.l ;; change from ki00119 Michi-Tsubaki + +;;Okada-seisei seminar homework achievement ;;last editted oct 29 ;; Michi-Tsubaki + +;; +;;Thiscode is for trying to make unique motion for sewing something. Still Trying + + +;;Load packages for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") + + +;;Making PR2 object +(if (not (boundp '*pr2*)) (pr2-init)) + + +;;Setting cube as a desk for surgery. +(setq *desk* (make-cube 500 500 600)) +(send *desk* :translate (float-vector 700 0 350)) ;; 質問：世界座標の原点がわからないので，机を床に設置(接地)できない． +(send *desk* :set-color :brown) +;[memo] (send *desk* :translate (float-vector 500 0 80)) ;;CAUTION!!(by Michi-Tsubaki)<-parameter that make robot motion NOT converge + + +;;Setting cylinder as needle. +(setq *needle* (make-cylinder 2 40)) +(send *needle* :translate (float-vector 700 0 700)) +(send *needle* :set-color :yellow) +(send *needle* :rotate #d90 :x) + + +;;Setting hampen +(setq *hampen* (make-cube 60 60 20)) +(send *hampen* :translate (float-vector 700 0 660)) ; 質問：座標じゃなくてdeskの上面に設置させたい +(send *hampen* :set-color :white) + +;;Setting coordination. +(send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *desk*)) +;;Setting coordination. +(send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) ;;質問：parent座標をpr2に設定したら収束しなくなった + +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +;;Show all objects in viewer. +(objects (list *pr2* *desk* *needle* *hampen*)) ;;Dont forget to add all items + + +;Setting Initial Pose +(send *pr2* :reset-pose) +(send *ri* :stop-grasp :arms) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +;Putting arms above each side of the desk +;;Setting the right arm and the left arm to the initial position. +;;USING INVERSE KINEMATICS +(send *pr2* :larm :inverse-kinematics + (send (send *desk* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *desk* :get :left-coords) :copy-worldcoords) +; :rotation-axis :z) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +;;Grasping needle +;;1. Reach +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +(send *pr2* :larm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + +;;2. Graspe +(send *ri* :start-grasp :arms) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + + +;;Stitching with the left hand +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 -10.0 -10.0)) + :move-target (send *needle* :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) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* :larm :wrist-p :joint-angle -100) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 50.0 20.0 0.0)) + :move-target (send *needle* :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) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 -12.0 20.0)) + :move-target (send *needle* :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) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* :larm :wrist-p :joint-angle -80) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector -35.0 -10.0 5.0)) + :move-target (send *needle* :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) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* :larm :elbow-r :joint-angle 95) +(send *pr2* :larm :shoulder-r :joint-angle 95) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 2.0 2.0)) + :move-target (send *needle* :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) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +;;Switching hands +(send *pr2* :rarm :shoulder-p :joint-angle -20) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + +(send *pr2* :rarm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector -10.0 0 0) + :move-target (send *needle* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis :x)) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + + + + +;;質問：ここでikが上手く行かなくなった。左では上手く行っているのに、右では下からアプローチする。assocの切り替えもわからない。あと，オフセット +;;Grasping needle with right hand +(send *pr2* :rarm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) diff --git a/jsk_2024_10_semi/object.l b/jsk_2024_10_semi/object.l new file mode 100644 index 000000000..1d9d1a8d8 --- /dev/null +++ b/jsk_2024_10_semi/object.l @@ -0,0 +1,151 @@ +#!/usr/bin/env irteusgl + +(defclass trajectory + :super cascaded-link + :slots (end-coords tp1 tp2 tp3 tp4 tp5 start-coords)) + +(defmethod trajectory + (:init () + (let (b) + (send-super :init) + ;; tp5 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 50)) + (send b :set-color :black) + (setq tp5 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp5)) + (setq end-coords (make-cascoords :pos #f(0 0 20))) + (send tp5 :assoc end-coords) + (send tp5 :locate #f(0 0 100)) + ;; tp4 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 4)) + (send b :set-color :red) + (setq tp4 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp4)) + (send tp4 :assoc tp5) + (send tp4 :locate #f(0 0 4)) + ;; tp3 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 3)) + (send b :set-color :blue) + (setq tp3 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp3)) + (send tp3 :assoc tp4) + (send tp3 :locate #f(0 0 4)) + ;; tp2 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 2)) + (send b :set-color :red) + (setq tp2 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp2)) + (send tp2 :assoc tp3) + (send tp2 :locate #f(0 0 20)) + ;; tp1 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 1)) + (send b :set-color :white) + (setq tp1 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp1)) + (send tp1 :assoc tp2) + + (setq links (list tp1 tp2 tp3 tp4 tp5)) + (send self :init-ending) + self)) + + (:end-coords (&rest args) (forward-message-to end-coords args)) + + ;; 等間隔にポイントを配置するメソッド + (:set-equal-intervals (start-coord end-coord) + (let* ((num-points 5) ;; tp1 から tp5 までの5点 + (start-pos (send start-coord :pos)) + (end-pos (send end-coord :pos)) + (delta-x (/ (- (aref end-pos 0) (aref start-pos 0)) (float (1- num-points)))) + (delta-y (/ (- (aref end-pos 1) (aref start-pos 1)) (float (1- num-points)))) + (delta-z (/ (- (aref end-pos 2) (aref start-pos 2)) (float (1- num-points))))) + + ;; 各ポイントを計算して配置 + (setq tp1 (send self :update-point tp1 start-pos delta-x delta-y delta-z 0)) + (setq tp2 (send self :update-point tp2 start-pos delta-x delta-y delta-z 1)) + (setq tp3 (send self :update-point tp3 start-pos delta-x delta-y delta-z 2)) + (setq tp4 (send self :update-point tp4 start-pos delta-x delta-y delta-z 3)) + (setq tp5 (send self :update-point tp5 start-pos delta-x delta-y delta-z 4)) + + ;; 更新されたターゲットポイントをリンクに再設定 + (send tp1 :assoc tp2) + (send tp2 :assoc tp3) + (send tp3 :assoc tp4) + (send tp4 :assoc tp5) + self)) + + ;; 各ターゲットポイントを更新するサブルーチン + (:update-point (point start-pos delta-x delta-y delta-z index) + (let* ((new-pos (list (+ (aref start-pos 0) (* delta-x index)) + (+ (aref start-pos 1) (* delta-y index)) + (+ (aref start-pos 2) (* delta-z index))))) + ;; new-pos をベクターとして変換 + (setq new-pos (vector (aref new-pos 0) (aref new-pos 1) (aref new-pos 2))) ;; ベクターに変換 + (send point :locate new-pos) + point)) + ) + +;; 使用例 +(setq r (instance trajectory :init)) +(setq start-coord (make-cascoords :pos #f(0 0 0))) +(setq end-coord (make-cascoords :pos #f(0 0 100))) +;(send r :set-equal-intervals start-coord end-coord) +(objects (list r (send r :end-coords))) +(send *irtviewer* :draw-objects) + + + + +#| +(defclass trajectory + :super cascaded-link + :slots (end-coords tp1 tp2 tp3 tp4 tp5)) + +(defmethod trajectory + (:init () + (let (b) + (send-super :init) + ;; tp5 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 50)) + (send b :set-color :black) + (setq tp5 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp5)) + (setq end-coords (make-cascoords :pos #f(0 0 20))) + (send tp5 :assoc end-coords) + (send tp5 :locate #f(0 0 100)) + ;; tp4 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 4)) + (send b :set-color :red) + (setq tp4 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp4)) + (send tp4 :assoc tp5) + (send tp4 :locate #f(0 0 4)) + ;; tp3 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 3)) + (send b :set-color :blue) + (setq tp3 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp3)) + (send tp3 :assoc tp4) + (send tp3 :locate #f(0 0 4)) + ;; tp2 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 2)) + (send b :set-color :red) + (setq tp2 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp2)) + (send tp2 :assoc tp3) + (send tp2 :locate #f(0 0 20)) + ;; tp1 + (setq b (make-cube 1 1 1)) + (send b :locate #f(0 0 1)) + (send b :set-color :white) + (setq tp1 (instance bodyset-link :init (make-cascoords) :bodies (list b) :name 'tp1)) + (send tp1 :assoc tp2) + + (setq links (list tp1 tp2 tp3 tp4 tp5)) + (send self :init-ending) + self)) + (:end-coords (&rest args) (forward-message-to end-coords args)) + ) + +(setq r (instance trajectory :init)) +(objects (list r (send r :end-coords))) +(send *irtviewer* :draw-objects) diff --git a/jsk_2024_10_semi/pr2_surgery/.#*shell* b/jsk_2024_10_semi/pr2_surgery/.#*shell* new file mode 120000 index 000000000..45f967377 --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery/.#*shell* @@ -0,0 +1 @@ +mech-user@ki00119.4627:1731969929 \ No newline at end of file diff --git a/jsk_2024_10_semi/pr2_surgery/.#main.l b/jsk_2024_10_semi/pr2_surgery/.#main.l new file mode 120000 index 000000000..a92333f49 --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery/.#main.l @@ -0,0 +1 @@ +mech-user@ki00119.186446:1732011910 \ No newline at end of file diff --git a/jsk_2024_10_semi/pr2_surgery/main.l b/jsk_2024_10_semi/pr2_surgery/main.l new file mode 100644 index 000000000..49af1dc7b --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery/main.l @@ -0,0 +1,306 @@ +#!/usr/bin/env roseus + +#| DESCRIPTION +Okada-sensei seminar +Surgery-method dev2 +Last editted Nov 23 by Michi-Tsubaki +GOAL: Trying to make unique motion for sewing sponge. +今後やること +・実際のスポンジを縫う +・音声認識で手術開始(detect-voice) +・画像認識で位置決め(Perception) +・手術ロボットでにしてみる +・曲がった経路にする(make-trajectory) +|# + +;;Load pkgs around PR2 Interface +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") ;;*ri* +(require "package://pr2eus/speak.l") ;;pkg for speaking + + +#| UNDER CONSTRUCTION +;; For perception +(ros::load-ros-manifest "jsk_recognition_msgs") + +;; For listening +(ros::roseus-add-msgs "speech_recognition_msgs") + +;;set node +(ros::roseus "surgery") +|# + + +;;Load class related +(load "models/arrow-object.l") ;;*arrow* + + +#| UNDER CONSTRUCTION +;; Voice Detection +(defun speech-cb(msg) + (setq *ans* (elt (send msg :transcript) 0)) + (if (equalp *ans* "start") + (setq *start-flag* 1) + ;; 1回フラグが立てばそれでいい + ) + +(defun detect-voice () + (ros::ros-info "start waiting for call ~%") + (ros::rate 10) + (ros::subscribe "speech_to_text" speech_recognition_msgs::SpeechRecognitionCandidates #'speech-cb) + ) + +|# + + +;;Global Param +(setq *start-flag* 0) +(setq *centerx* 700) +(setq *centery* 0) +(setq *centerz* 800) +(setq *deskw* 500) +(setq *needle_len* 70) +(setq *interval* 10) +(setq *traj_num* 10) +(setq *traj_len* (* *interval* *traj_num*)) + +;; Set remain +(setq *remain* 300) ;;how long remaining thread? +(setq *diff_remain* 30) + +;; Offset +(setq *rarm_offset* 2) + + +;; Define Trajectory Class +(defclass traj + :super cascaded-coords + :slots (points)) + +(defmethod traj + (:init (&rest args) + (send-super* :init args) + (dotimes (i *traj_num*) + (push + (make-cube 10 10 10 :pos (float-vector (* (- i 5) *interval*) 0 0)) + points)) + (dotimes (i (- (length points) 1)) + (send (elt points i) :assoc (elt points (+ 1 i)))) + (send self :assoc (car points)) + self) ;; :init + (:points () points) + ) ;; defmethod + + +;; Define set-environment method +(defun set-env() + ;;Set center + (setq *deskh* *centerz*) + + (setq *center* (make-cube 10 10 10)) + (send *center* :translate (float-vector *centerx* *centery* *centerz*)) + (send *center* :set-color :black) + (setq *o* (send *center* :copy-worldcoords)) + + ;;Set desk + (setq *desk* (make-cube *deskw* *deskw* *deskh*)) + (send *desk* :translate (float-vector *centerx* *centery* (/ *centerz* 2))) + (send *desk* :set-color :brown) + + ;;Set needle + (setq *needle* (make-cylinder 0.5 *needle_len*)) + (send *needle* :translate (v+ (float-vector 0 20 300) (send *center* :pos))) + (send *needle* :set-color :yellow) + (send *needle* :rotate #d90 :y) + + ;;Set hampen + (setq *hampen* (make-cube 50 50 50)) + (send *hampen* :translate (v+ (float-vector 0 0 10) (send *center* :pos))) + (send *hampen* :set-color :white) + (send *hampen* :rotate #d45 :x) + + (send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *pr2*)) + (send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) +) + + +;;Define preparation task. +(defun preparation() + (require "package://pr2eus/pr2.l") + (require "package://pr2eus/pr2-utils.l") + (require "package://pr2eus/pr2-interface.l") + (require "package://pr2eus/speak.l") + + (send *ri* :speak-jp "初期姿勢に戻ります．注意してください．" :wait t) + (send *pr2* :reset-pose) ;;Set Initial Pose + (send *ri* :start-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* :larm :inverse-kinematics + (send (send (send *needle* :get :left-coords) + :copy-worldcoords) + :rotate (deg2rad 90) :z) + :rotation-axis :z) ;;IK to needle pose + (send *pr2* :larm :end-coords :assoc *needle*) ;; + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;;Grasp needle (real) + (send *ri* :speak-jp "左手を開きます．針を持たせてください．" :wait t) + (send *ri* :stop-grasp :larm :wait t) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ) + +;;Define initial task. +(defun init-task() + (send *ri* :speak-jp "手を閉じます．挟まれないように気をつけてください" :wait t) + (send *ri* :start-grasp :larm :wait t) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *ri* :speak-jp "これから針を持って作業します．離れてください．" :wait t) + (unix:sleep 2) + + ;;Grasp needle (dummy) + (send *pr2* :larm :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) :rotate (deg2rad 90) :z) :rotation-axis :z) + (send *pr2* :larm :end-coords :assoc *needle*) ;;針を固定 + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ) + + +;;;;main.l;;;;; + +;;Set arrow (for debug) +(setq *arrow* (arrow)) +(send *arrow* :copy-worldcoords) + +;;Set PR2 +(if (not (boundp '*pr2*)) (pr2-init)) + +;; Set the experimental environments. +(set-env) + +;; Show all objects in irtviewer. +(objects (list *pr2* *center* *arrow* *desk* *needle* *hampen*)) + +;; Human help Robot to grab needle. +(preparation) + +;; The Doctore declare the start of the surgery. +;;音声認識******************************************************************************************************** +;;(detect-voice) +(unix:sleep 3) ;;dummy + +;; Do initial task. +(init-task) + +;; Perception (dummy) +(setq *target-xpos* 0) +(setq *target-zpos* 30) + +;; Stitch +(dotimes (i 5) + ;; Make trajectory points. + (setq r (instance traj :init)) + (send r :rotate pi/2 :z) + (setq *target* (v+ (send *center* :pos) (float-vector (+ *target-xpos* (* i 3)) (* *traj_len* 0.2) *target-zpos*))) ;; 0.2は実験則 ;; Trajectoryの中心座標をHampenにあわせている + (send r :locate *target* :world) + (objects (append (list *pr2* *center* *arrow* *desk* *needle* *hampen*) (send r :points))) + + + ;; Set start position. + ;; Rarm + (setq *r-pos* (send (elt (send r :points) 9) :copy-worldcoords)) + (send *r-pos* :rotate pi/2 :y :local) + (send *r-pos* :rotate -1 :z :local) ;; if -pi/2, rarm state is parallel to desk + (send *r-pos* :translate #f(-10 0 0) :local) + (send *pr2* :rarm :inverse-kinematics *r-pos* :rotation-axis t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :stop-grasp :rarm) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + + ;; Follow the trajectory points. Stitch. + (dolist (e (send r :points)) + (let (ee) + (setq ee (send e :copy-worldcoords)) + (send ee :translate (float-vector (- *needle_len* *rarm_offset*) 0 0) :local) + (send ee :rotate pi :z :local) + (send ee :rotate pi/2 :y :local) + (send ee :draw-on :flush t :size 100) + (send *pr2* :larm :inverse-kinematics ee :rotation-axis t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ) + ) + + ;; Pass on the needle. (左手から右手に針を戻す) + (send *ri* :start-grasp :rarm :wait t) + (send *ri* :wait-interpolation) + (unix:sleep 1) + (send *ri* :stop-grasp :larm :wait t) + (send *ri* :wait-interpolation) + + ;; Fix the right arm and neadle (針の固定を左から右に移す) + (send *pr2* :larm :end-coords :dissoc *needle*) + (send *pr2* :rarm :end-coords :assoc *needle*) + + ;; Draw the needle from the object. (対象物から針を抜き出) + (send *pr2* :rarm :move-end-pos (float-vector 0 0 (- 0 *remain*)) :local) + (- *remain* *diff_remain*) ;; オリジナリティポイント: 1回縫うと針を引ける量が減るのを再現 + (format t "remaining ~A ~%" *remain*) ;; for debug! (*remain*が0になったらbreakするようにしたい)******************************************************************************************** + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; Move left hand to the delivery point. + (setq *pass-larm* (send (send (send (send (send *center* :copy-worldcoords) + :translate (float-vector 0 (* *needle_len* 0.5) 200)) + :rotate pi/2 :z) + :rotate pi/2 :y) + :rotate pi :x)) + (send *pass-larm* :draw-on :flush t :size 100) ;; for debug! + (send *pr2* :larm :inverse-kinematics *pass-larm* :rotation-axis t :use-torso t) ;; Use torso! (腰を使わないとIKが解けないので注意) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; Move right hand to the delivery point. + (setq *pass-rarm* (send (send (send (send *center* :copy-worldcoords) + :translate (float-vector 0 (- 0 (* *needle_len* 0.5)) 200)) + :translate #f(-10 0 0)) + :rotate pi/2 :x)) + (send *pass-rarm* :draw-on :flush t :size 100) ;; for debug! + (send *pr2* :rarm :inverse-kinematics *pass-rarm* :rotation-axis t) ;; Do not use torso! (左手との相対座標が狂う) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; Pass on the needle. (右手から左手に針を戻す) + (send *ri* :start-grasp :larm) + (send *ri* :wait-interpolation) + (unix:sleep 1) + (send *ri* :stop-grasp :rarm) + (send *ri* :wait-interpolation) + + ;; Fix the left arm and needle again (針の固定を右から左に移す) + (send *pr2* :rarm :end-coords :dissoc *needle*) + (send *pr2* :larm :end-coords :assoc *needle*) + (send *ri* :speak-jp "繰り返し次の経路を縫います．注意してください．" :wait t) + (unix:sleep 1) + ) diff --git a/jsk_2024_10_semi/pr2_surgery/removable/env.l b/jsk_2024_10_semi/pr2_surgery/removable/env.l new file mode 100644 index 000000000..151f4831b --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery/removable/env.l @@ -0,0 +1,33 @@ +(defun set-env() + ;;cet center + (setq *center* (make-cube 10 10 10)) + (send *center* :translate (float-vector *centerx* *centery* *centerz*)) + (send *center* :set-color :black) + (setq *o* (make-cascoords :pos (send *center* :pos))) + + ;;Set desk + (setq *desk* (make-cube *deskw* *deskw* *deskh*)) + (send *desk* :translate (float-vector *centerx* *centery* (/ *centerz* 2))) + (send *desk* :set-color :brown) + + ;;Set needle + (setq *needle* (make-cylinder 0.5 70)) + (send *needle* :translate (v+ (float-vector 0 20 300) (send *center* :pos))) + (send *needle* :set-color :yellow) + (send *needle* :rotate #d90 :y) + + ;;Set hampen + (setq *hampen* (make-cube 50 50 50)) + (send *hampen* :translate (v+ (float-vector 0 0 10) (send *center* :pos))) + (send *hampen* :set-color :white) + (send *hampen* :rotate #d45 :x) + + (send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *pr2*)) + (send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) +) diff --git a/jsk_2024_10_semi/pr2_surgery/removable/initial-task.l b/jsk_2024_10_semi/pr2_surgery/removable/initial-task.l new file mode 100644 index 000000000..2c738d791 --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery/removable/initial-task.l @@ -0,0 +1,36 @@ +(defun initial-task() + (require "package://pr2eus/pr2.l") + (require "package://pr2eus/pr2-utils.l") + (require "package://pr2eus/pr2-interface.l") + (require "package://pr2eus/speak.l") + + (send *ri* :speak-jp "初期姿勢に戻ります。" :wait t) + (send *pr2* :reset-pose) + (send *ri* :start-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* :larm :inverse-kinematics + (send (send (send *needle* :get :left-coords) + :copy-worldcoords) + :rotate (deg2rad 90) :z) + :rotation-axis :z + :debug-view t) + (send *pr2* :larm :end-coords :assoc *needle*) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *ri* :speak-jp "左手を開きます。針を持たせてください。") + (send *ri* :stop-grasp :larm :wait t) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *ri* :speak-jp "閉じます。" :wait t) + (send *ri* :start-grasp :larm :wait t) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *ri* :speak-jp "これから針を持って作業します．離れてください．" :wait t) + (unix:sleep 2) +) diff --git a/jsk_2024_10_semi/pr2_surgery/removable/trajectory.l b/jsk_2024_10_semi/pr2_surgery/removable/trajectory.l new file mode 100644 index 000000000..e48645d80 --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery/removable/trajectory.l @@ -0,0 +1,16 @@ +(defclass traj + :super cascaded-coords + :slots (points)) +(defmethod traj + (:init (&rest args) + (send-super* :init args) + (dotimes (i 10) + (push + (make-cube 10 10 10 :pos (float-vector (* (- i 5) 20) 0 0)) + points)) + (dotimes (i (- (length points) 1)) + (send (elt points i) :assoc (elt points (+ 1 i)))) + (send self :assoc (car points)) + self) ;; :init + (:points () points) + ) ;; defmethod diff --git a/jsk_2024_10_semi/pr2_surgery_dev1/surgery_method_2.l b/jsk_2024_10_semi/pr2_surgery_dev1/surgery_method_2.l new file mode 100755 index 000000000..0e54a4d6d --- /dev/null +++ b/jsk_2024_10_semi/pr2_surgery_dev1/surgery_method_2.l @@ -0,0 +1,357 @@ +#!/usr/bin/env roseus + +;;Okada-seisei seminar +;;Surgery-method dev1 +;;last editted Nov 12 ;; Michi-Tsubaki + +;; +;;This program is for trying to make unique motion for sewing sponge. Still Trying + +;;Load pkgs for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") ;;pkg for speaking +(load "models/arrow-object.l") + +;;Define Trajectory Point (7) +(defvar *tp1* nil) +(defvar *tp2* nil) +(defvar *tp3* nil) +(defvar *tp4* nil) +(defvar *tp5* nil) +(defvar *tp6* nil) +(defvar *tp7* nil) + +;;Set PR2 +(if (not (boundp '*pr2*)) (pr2-init)) + +;;Set Center of a Table +(setq *center* (make-cube 10 10 10)) +(send *center* :translate (float-vector 700 0 750)) +(send *center* :set-color :black) + +;;Set cube as a desk for surgery. +(setq *desk* (make-cube 500 500 750)) +(send *desk* :translate (float-vector 700 0 750/2)) +(send *desk* :set-color :brown) + +;;Set cylinder as needle. +(setq *needle* (make-cylinder 0.5 70)) +(send *needle* :translate (v+ (float-vector 0 20 100) (send *center* :pos))) +(send *needle* :set-color :yellow) +(send *needle* :rotate #d270 :x) + +;;Set hampen +(setq *hampen* (make-cube 50 50 50)) +(send *hampen* :translate (v+ (float-vector 0 0 10) (send *center* :pos))) +(send *hampen* :set-color :white) +(send *hampen* :rotate #d45 :x) + + + +;;Define set-trajectory function +(defun set-trajectory (x) + (let ((prev-pos *center*)) ; 最初の座標を原点に設定 + ;; tp1 + (setq *tp1* (make-cube 2 2 2)) + (send *tp1* :translate (v+ (float-vector (- x 15) 30 40) (send *center* :pos))) + (send *tp1* :rotate #d0 :z) + (send *tp1* :set-color :red) + (send *tp1* :put :left-coords + (make-cascoords + :coords (send (send *tp1* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp1*)) + (setq prev-pos (send *tp1* :pos)) + ;; tp2 + (setq *tp2* (make-cube 2 2 2)) + (send *tp2* :translate (v+ (float-vector 0 -10 0) prev-pos)) + (send *tp2* :rotate #d90 :z) + (send *tp2* :set-color :red) + (send *tp2* :put :left-coords + (make-cascoords + :coords (send (send *tp2* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp1*)) + (setq prev-pos (send *tp2* :pos)) + ;; tp3 + (setq *tp3* (make-cube 2 2 2)) + (send *tp3* :translate (v+ (float-vector 0 -10 0) prev-pos)) + (send *tp3* :rotate #d270 :z) + (send *tp3* :set-color :red) + (send *tp3* :put :left-coords + (make-cascoords + :coords (send (send *tp3* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp2*)) + (setq prev-pos (send *tp3* :pos)) + ;; tp4 + (setq *tp4* (make-cube 2 2 2)) + (send *tp4* :translate (v+ (float-vector 0 -10 0) prev-pos)) + (send *tp4* :rotate #d90 :x) + (send *tp4* :set-color :red) + (send *tp4* :put :left-coords + (make-cascoords + :coords (send (send *tp3* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp3*)) + (setq prev-pos (send *tp4* :pos)) + ;; tp5 + (setq *tp5* (make-cube 2 2 2)) + (send *tp5* :translate (v+ (float-vector 0 10 0) prev-pos)) + (send *tp5* :rotate #d90 :x) + (send *tp5* :set-color :red) + (send *tp5* :put :left-coords + (make-cascoords + :coords (send (send *tp3* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp4*)) + (setq prev-pos (send *tp5* :pos)) + ;; tp6 + (setq *tp6* (make-cube 2 2 2)) + (send *tp6* :translate (v+ (float-vector 0 -10 0) prev-pos)) + (send *tp6* :rotate #d90 :x) + (send *tp6* :set-color :red) + (send *tp6* :put :left-coords + (make-cascoords + :coords (send (send *tp6* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp5*)) + (setq prev-pos (send *tp6* :pos)) + ;; tp7 + (setq *tp7* (make-cube 2 2 2)) + (send *tp7* :translate (v+ (float-vector 0 -70 0) prev-pos)) + (send *tp7* :rotate #d90 :x) + (send *tp7* :set-color :red) + (send *tp7* :put :left-coords + (make-cascoords + :coords (send (send *tp6* :copy-worldcoords) + :translate (float-vector 1 0 0)) + :rot #2f((0 0 1) + (-1 0 0) + (0 -1 0)) + :parent *tp6*)) + (setq prev-pos (send *tp7* :pos)))) + + +(set-trajectory 0) + +;;Set coordination. +(send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *pr2*)) +(send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) + +;;Show Worldrecord +(setq *arrow* (arrow)) +(send *arrow* :copy-worldcoords) + +;;Show all objects in IRTVIEWER +(objects (list *pr2* *center* *arrow* *desk* *needle* *hampen* *tp1* *tp2* *tp3* *tp4* *tp5* *tp6* *tp7*)) + +;;Set initial pose +(send *ri* :speak-jp "初期姿勢に戻ります。" :wait t) +(send *pr2* :reset-pose) +(send *ri* :start-grasp :arms) +(send *ri* :angle-vector (send *pr2* :angle-vector) 2000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "左手を開きます。針を持たせてください。") +(send *ri* :stop-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "閉じます。" :wait t) +(send *ri* :start-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "これから針を持って作業します．離れてください．" :wait t) +(unix:sleep 2) + + +;;Grasping needle (dummy) +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +(send *pr2* :larm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +;;Set Start Position +;;Larm +(setq current-coords (send (send *desk* :get :left-coords) :copy-worldcoords)) +(setq new-coords (send current-coords :translate (float-vector 0 100 50))) +(send *pr2* :larm :inverse-kinematics new-coords :rotation-axis :x) +(send *pr2* :larm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;;Rarm +(setq current-coords (send (send *desk* :get :left-coords) :copy-worldcoords)) +(setq new-coords (send current-coords :translate (float-vector 0 -100 100))) +(send *pr2* :rarm :inverse-kinematics new-coords :rotation-axis :x) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +(do ((i 0 (+ i 1))) ; i を 0 から 1 ずつ増加 + ((> i 3) (format t "done")) ;; それぞれの位置と角度情報をリストに格納 ; i が 3 より大きくなったら終了 + (set-trajectory (- (* i 10) 3)) ; i の値に基づいて座標計算 + (format t "Trajectory tp1:~A tp2:~A tp3:~A tp4:~A tp5:~A tp6:~A tp7:~A. ~%" (send *tp1* :copy-worldcoords) (send *tp2* :copy-worldcoords) (send *tp3* :copy-worldcoords) (send *tp4* :copy-worldcoords) (send *tp5* :copy-worldcoords) (send *tp6* :copy-worldcoords) (send *tp7* :copy-worldcoords)) + (objects (list *pr2* *center* *arrow* *desk* *needle* *hampen* *tp1* *tp2* *tp3* *tp4* *tp5* *tp6* *tp7*)) + (send *irtviewer* :draw-objects) + + (send *ri* :start-grasp :larm :wait t) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp1* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp2* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) ;; 5cm上、5cm手前 + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp3* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp4* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp5* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp6* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + #| + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* + :larm + :inverse-kinematics (send (send (send *tp7* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) +|# + + + + ;;Rarm Swith Hands + (send *ri* :stop-grasp :rarm :wait t) + (send *irtviewer* :draw-objects) + + (send *pr2* :rarm :inverse-kinematics + (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 60.0))) + :move-target (send *needle* :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) 5000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *ri* :start-grasp :rarm :wait t) + (send *irtviewer* :draw-objects) + (send *pr2* :rarm + :inverse-kinematics (send (send (send *tp7* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 -7 0)) + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *ri* :stop-grasp :larm :wait t) + (send *irtviewer* :draw-objects) + + + + (unix:sleep 2)) ; 1秒間スリープ diff --git a/jsk_2024_10_semi/sample/demo-box.l b/jsk_2024_10_semi/sample/demo-box.l new file mode 100644 index 000000000..a5fa73035 --- /dev/null +++ b/jsk_2024_10_semi/sample/demo-box.l @@ -0,0 +1,41 @@ +(require "package://pr2eus/pr2-interface.l") +(pr2-init) + +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box label) + (print (list box label)) + (print (send-all (list box label) :header :stamp)) + )) + +;; ;; test +;; (setq *box-label* (instance box-label-synchronizer :init +;; (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) +;; (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + +(ros::load-ros-manifest "jsk_recognition_msgs") +(setq *target-box* (make-cube 100 100 100)) +(objects (list *pr2* *target-box*)) +(defun box-cb (msg) + (ros::ros-info "received ~A boxes" (length (send msg :boxes))) + (dolist (box (send msg :boxes)) + (when (= (send box :label) 41) ;; 41 -> ball + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + (format t "coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + (when (and (< (elt (send *target-coords* :worldpos) 2) 1000) + (> (elt (send *target-coords* :worldpos) 2) 600)) + (send *target-box* :move-to *target-coords* :world) + (print "update target position") + )))) + +(ros::subscribe "/synchronized_detic_label_boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +;;(ros::subscribe "/kinect_head/depth_registered/boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +(do-until-key + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + ) diff --git a/jsk_2024_10_semi/sample/demo-set-trajectory.l b/jsk_2024_10_semi/sample/demo-set-trajectory.l new file mode 100644 index 000000000..63c94e9e4 --- /dev/null +++ b/jsk_2024_10_semi/sample/demo-set-trajectory.l @@ -0,0 +1,47 @@ +;; (defun test () +;; (let ((r nil)) +;; (dotimes (i 10) +;; (push +;; (make-cube 10 10 10 :pos (float-vector (* (- i 5) 20) 0 0)) +;; r)) +;; (dotimes (i (- (length r) 1)) +;; (send (elt r i) :assoc (elt r (+ 1 i)))) +;; r)) +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (pr2-init)) + +(defclass test1 + :super cascaded-coords + :slots (points)) +(defmethod test1 + (:init (&rest args) + (send-super* :init args) + (dotimes (i 10) + (push + (make-cube 10 10 10 :pos (float-vector (* (- i 5) 20) 0 0)) + points)) + (dotimes (i (- (length points) 1)) + (send (elt points i) :assoc (elt points (+ 1 i)))) + (send self :assoc (car points)) + self) + (:points () points) + ) + + + +;;(setq r (test)) +;;(send (car r) :rotate pi/2 :z) +;;(send (car r) :locate #f(800 0 900) :world) +(setq r (instance test1 :init)) +(send r :rotate pi/2 :z) +(send r :locate #f(800 0 900) :world) +(objects (append (list *pr2*) (send r :points))) +(send *irtviewer* :draw-objects) +;;(objects (list *pr2* (elt (send r :points) 0) (elt (send r :points) 1) (elt (send r :points) 2) +;(dolist (e (send r :points) +; (send *pr2* :rarm :inverse-kinematics e +; :rotation-axis nil +; :debug-view t) +; (send *irtviewer* :draw-objects)) diff --git a/jsk_2024_10_semi/sample/demo_solve-ik.l b/jsk_2024_10_semi/sample/demo_solve-ik.l new file mode 100755 index 000000000..95750df78 --- /dev/null +++ b/jsk_2024_10_semi/sample/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つの箱に両腕を連結することはできない? diff --git a/jsk_2024_10_semi/surgery_motion_1.l b/jsk_2024_10_semi/surgery_motion_1.l new file mode 100755 index 000000000..5abac9189 --- /dev/null +++ b/jsk_2024_10_semi/surgery_motion_1.l @@ -0,0 +1,291 @@ +#!/usr/bin/env roseus +;chmod u+x ./surgery_motion_1.l --> ./surgery_motion_1.l ;; change from ki00119 Michi-Tsubaki + +;;Okada-seisei seminar homework achievement ;;last editted oct 29 ;; Michi-Tsubaki + +;; +;;Thiscode is for trying to make unique motion for sewing something. Still Trying + + +;;Load packages for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") ;;use :speak-jp ;;https://github.com/wkentaro/jsk_pr2eus/blob/fad0691e0131275b76568a68096c6c9f6eead13e/pr2eus/speak.l#L4 +(load "models/arrow-object.l") + + +;;Making PR2 object +(if (not (boundp '*pr2*)) (pr2-init)) + + +;;Setting cube as a desk for surgery. +(setq *desk* (make-cube 500 500 800)) +(send *desk* :translate (float-vector 700 0 400)) +(send *desk* :set-color :brown) +;[memo] (send *desk* :translate (float-vector 500 0 80)) ;;CAUTION!!(by Michi-Tsubaki)<-parameter that make robot motion NOT converge + +;;Setting cylinder as needle. +(setq *needle* (make-cylinder 2 40)) +(send *needle* :translate (float-vector 700 0 850)) +(send *needle* :set-color :yellow) +(send *needle* :rotate #d90 :x) + +;;Set hampen +(setq *hampen* (make-cube 60 60 30)) +(send *hampen* :translate (float-vector 700 0 830)) +;(send *hampen* :translate (v+ (float-vector 0 0 310) (send *desk* :pos)) +(send *hampen* :set-color :white) + +;;Set coordination. +(send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *desk*)) +;;Set coordination. +(send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) ;;質問：parent座標をpr2に設定したら収束しなくなった ;;still confusing + +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + + +;;Show Worldrecord +(setq *arrow* (arrow)) +(send *arrow* :copy-worldcoords) + +;;Show all objects in viewer. +(objects (list *pr2* *desk* *needle* *hampen*)) ;;Dont forget to add all items + + + +;Setting Initial Pose +(send *ri* :speak-jp "初期姿勢に戻ります" :wait t) +(send *ri* :start-grasp :arms :wait t) +(send *pr2* :reset-pose) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :speak-jp "右手を開きます。スポンジをもたせてください。") +(send *ri* :stop-grasp :rarm :wait t) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :start-grasp :rarm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :speak-jp "左手を開きます。針を持たせてください。") +(send *ri* :stop-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :start-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :speak-jp "これから針を持って作業します．離れてください．") +(unix:sleep 2) + +;Putting arms above each side of the desk +;;Setting the right arm and the left arm to the initial position. +;;USING INVERSE KINEMATICS +(send *pr2* :larm :inverse-kinematics + (send (send *desk* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *desk* :get :left-coords) :copy-worldcoords) +; :rotation-axis :z) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +;;Graspe needle +;;1. Reach +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +(send *pr2* :larm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + +;;2. Graspe +;(send *ri* :start-grasp :larm) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + + +;;Stitch with the left hand +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 -10.0 -10.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* :larm :wrist-p :joint-angle -100) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 50.0 20.0 0.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 -12.0 20.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + + +(send *pr2* :larm :wrist-p :joint-angle -80) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector -35.0 -10.0 5.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +(send *pr2* :larm :elbow-r :joint-angle 95) +(send *pr2* :larm :shoulder-r :joint-angle 95) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 2.0 2.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + + +;;Switching hands +(send *pr2* :rarm :shoulder-p :joint-angle -20) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +(send *pr2* :rarm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector -10.0 0 0) + :move-target (send *needle* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis :x)) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +;(send *ri* :stop-grasp :rarm) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + +;;Grasping needle with right hand +(progn (send (send *needle* :copy-worldcoords) :draw-on :size 50) ;;debug + (setq *next-needle-coords* (send (send *needle* :copy-worldcoords) :transform (make-coords :pos #f(0 0 80) :rpy (float-vector 0 0 0)))) + (send *next-needle-coords* :draw-on :flush t :size 100) ;; debug + (send *pr2* :rarm :inverse-kinematics (send *next-needle-coords* :copy-worldcoords) :rotation-axis t :look-at-target t :debug-view t) + ) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *needle* :get :left-coords) :copy-worldcoords) +; :rotation-axis :y) ;;yだと向きが違うき +(send (send *needle* :parent) :dissoc *needle*) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(progn (send (send *needle* :copy-worldcoords) :draw-on :size 50) ;;debug + (setq *next-needle-coords* (send (send *needle* :copy-worldcoords) :transform (make-coords :pos #f(0 0 40) :rpy (float-vector 0 0 0)))) + (send *next-needle-coords* :draw-on :flush t :size 100) ;; debug + (send *pr2* :rarm :inverse-kinematics (send *next-needle-coords* :copy-worldcoords) :rotation-axis t :look-at-target t :debug-view t) + ) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *needle* :get :left-coords) :copy-worldcoords) +; :rotation-axis :y) ;;yだと向きが違うき +(send (send *needle* :parent) :dissoc *needle*) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + + +(send *ri* :stop-grasp :larm) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + + +;(send *pr2* :larm :inverse-kinematics +; (send (send *desk* :get :left-coords) :copy-worldcoords) +; :rotation-axis t) +;(send (send *needle* :parent) :dissoc *needle*) +;(send *pr2* :rarm :end-coords :assoc *needle*) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + +(send *pr2* :rarm :inverse-kinematics + (send (send *desk* :get :left-coords) :copy-worldcoords) + :rotation-axis t) +(send (send *needle* :parent) :dissoc *needle*) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + + + +;(send *pr2* :larm :inverse-kinematics +; (send (send *needle* :get :left-coords) :copy-worldcoords) +; :rotation-axis :z) +;(send *pr2* :larm :end-coords :assoc *needle*) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + +;;how to dwaw coordination arrow +;(send (send (send *needle* :get :left-coords) :copy-worldcoords) :draw-on :flush t) + diff --git a/jsk_2024_10_semi/surgery_motion_2.l b/jsk_2024_10_semi/surgery_motion_2.l new file mode 100755 index 000000000..46c26567b --- /dev/null +++ b/jsk_2024_10_semi/surgery_motion_2.l @@ -0,0 +1,314 @@ +#!/usr/bin/env roseus +;chmod u+x ./surgery_motion_1.l --> ./surgery_motion_1.l ;; change from ki00119 Michi-Tsubaki + +;;Okada-seisei seminar homework achievement ;;last editted oct 29 ;; Michi-Tsubaki + +;; +;;Thiscode is for trying to make unique motion for sewing something. Still Trying + + +;;Load packages for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") ;;use :speak-jp +;;https://github.com/wkentaro/jsk_pr2eus/blob/fad0691e0131275b76568a68096c6c9f6eead13e/pr2eus/speak.l#L4 +(load "models/arrow-object.l") + + +;;Making PR2 object +(if (not (boundp '*pr2*)) (pr2-init)) + + +;;Setting cube as a desk for surgery. +(setq *desk* (make-cube 500 500 750)) +(send *desk* :translate (float-vector 700 0 375)) +(send *desk* :set-color :brown) +;[memo] (send *desk* :translate (float-vector 500 0 80)) ;;CAUTION!!(by Michi-Tsubaki)<-parameter that make robot motion NOT converge + + +;;Setting cylinder as needle. +(setq *needle* (make-cylinder 2 40)) +(send *needle* :translate (float-vector 700 0 800)) +(send *needle* :set-color :yellow) +(send *needle* :rotate #d90 :x) + + +;;Set hampen +(setq *hampen* (make-cube 60 60 24)) +(send *hampen* :translate (float-vector 750 0 762)) +;(send *hampen* :translate (v+ (float-vector 0 0 310) (send *desk* :pos)) +(send *hampen* :set-color :white) + +;;Set coordination. +(send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *desk*)) +;;Set coordination. +(send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) ;;質問：parent座標をpr2に設定したら収束しなくなった ;;still confusing + +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + + +;;Show Worldrecord +(setq *arrow* (arrow)) +(send *arrow* :copy-worldcoords) + +;;Show all objects in viewer. +(objects (list *pr2* *desk* *needle* *hampen*)) ;;Dont forget to add all items + + + +;Setting Initial Pose +(send *ri* :speak-jp "初期姿勢に戻ります" :wait t) +(send *ri* :start-grasp :arms :wait t) +(send *pr2* :reset-pose) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :speak-jp "右手を開きます。スポンジをもたせてください。") +(send *ri* :stop-grasp :rarm :wait t) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :start-grasp :rarm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :speak-jp "左手を開きます。針を持たせてください。") +(send *ri* :stop-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :start-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +(unix:sleep 1) +(send *ri* :speak-jp "これから針を持って作業します．離れてください．") +(unix:sleep 2) + +;Putting arms above each side of the desk +;;Setting the right arm and the left arm to the initial position. +;;USING INVERSE KINEMATICS +(send *pr2* :larm :inverse-kinematics + (send (send *desk* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *desk* :get :left-coords) :copy-worldcoords) +; :rotation-axis :z) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + +;;Graspe needle +;;1. Reach +(send *pr2* :larm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) +(send *pr2* :larm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(unix:sleep 1) + +;(progn (send (send *needle* :copy-worldcoords) :draw-on :size 50) ;;debug +; (setq *next-needle-coords* (send (send *needle* :copy-worldcoords) +; :transform (make-coords :pos #f(-20 0 0) :rpy (float-vector 0 0 0)))) +; (send *next-needle-coords* :draw-on :flush t :size 100) ;; debug +; (send *pr2* :larm :inverse-kinematics +; (send *next-needle-coords* :copy-worldcoords) +; :rotation-axis :z +; :look-at-target nil +; :debug-view t) +; ) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + + +;;2. Graspe +;(send *ri* :start-grasp :larm) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) +;(unix:sleep 1) + + + +;;Stitch with the left hand +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 50.0 -10.0 -50.0)) ;;xが前後方向になっている + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +;(send *pr2* :larm :wrist-p :joint-angle -100) + +#| +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 50.0 25.0 0.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) +|# + + + +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 -30.0 5.0)) + :move-target (send *needle* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis :z + :debug-view t) +(send *ri* :angle-vector (send *pr2* :angle-vector) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +(defun + +#| +(send *pr2* :larm :wrist-p :joint-angle -80) + +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector -35.0 0.0 5.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +;(send *pr2* :larm :elbow-r :joint-angle 100) +;(send *pr2* :larm :shoulder-r :joint-angle 95) +(send *pr2* + :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 2.0 2.0)) + :move-target (send *needle* :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) 3000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +#| + +;;Switching hands +(send *pr2* :rarm :shoulder-p :joint-angle -20) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +(send *pr2* :rarm :inverse-kinematics + (send (send *needle* :get :left-coords) :copy-worldcoords) + :translate (float-vector -10.0 0 0) + :move-target (send *needle* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis :x)) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + +;(send *ri* :stop-grasp :rarm) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + +;;Grasping needle with right hand +(progn (send (send *needle* :copy-worldcoords) :draw-on :size 50) ;;debug + (setq *next-needle-coords* (send (send *needle* :copy-worldcoords) :transform (make-coords :pos #f(0 0 80) :rpy (float-vector 0 0 0)))) + (send *next-needle-coords* :draw-on :flush t :size 100) ;; debug + (send *pr2* :rarm :inverse-kinematics (send *next-needle-coords* :copy-worldcoords) :rotation-axis t :look-at-target t :debug-view t) + ) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *needle* :get :left-coords) :copy-worldcoords) +; :rotation-axis :y) ;;yだと向きが違うき +(send (send *needle* :parent) :dissoc *needle*) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(progn (send (send *needle* :copy-worldcoords) :draw-on :size 50) ;;debug + (setq *next-needle-coords* (send (send *needle* :copy-worldcoords) :transform (make-coords :pos #f(0 0 40) :rpy (float-vector 0 0 0)))) + (send *next-needle-coords* :draw-on :flush t :size 100) ;; debug + (send *pr2* :rarm :inverse-kinematics (send *next-needle-coords* :copy-worldcoords) :rotation-axis t :look-at-target t :debug-view t) + ) +;(send *pr2* :rarm :inverse-kinematics +; (send (send *needle* :get :left-coords) :copy-worldcoords) +; :rotation-axis :y) ;;yだと向きが違うき +(send (send *needle* :parent) :dissoc *needle*) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + + +(send *ri* :stop-grasp :larm) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + + + +;(send *pr2* :larm :inverse-kinematics +; (send (send *desk* :get :left-coords) :copy-worldcoords) +; :rotation-axis t) +;(send (send *needle* :parent) :dissoc *needle*) +;(send *pr2* :rarm :end-coords :assoc *needle*) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + +(send *pr2* :rarm :inverse-kinematics + (send (send *desk* :get :left-coords) :copy-worldcoords) + :rotation-axis t) +(send (send *needle* :parent) :dissoc *needle*) +(send *pr2* :rarm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + + + +;(send *pr2* :larm :inverse-kinematics +; (send (send *needle* :get :left-coords) :copy-worldcoords) +; :rotation-axis :z) +;(send *pr2* :larm :end-coords :assoc *needle*) +;(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +;(send *ri* :wait-interpolation) +;(send *irtviewer* :draw-objects) + + +;;how to dwaw coordination arrow +;(send (send (send *needle* :get :left-coords) :copy-worldcoords) :draw-on :flush t) diff --git a/jsk_2024_10_semi/surgery_trajectory_1.l b/jsk_2024_10_semi/surgery_trajectory_1.l new file mode 100755 index 000000000..753605758 --- /dev/null +++ b/jsk_2024_10_semi/surgery_trajectory_1.l @@ -0,0 +1,106 @@ +;;Okada-seisei seminar +;;Surgery-method +;;last editted Nov 12 ;; Michi-Tsubaki + +;; +;;This program is for trying to make unique motion for sewing sponge. Still Trying + + +;;Load packages for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") ;;pkg for speaking + + +;;Define Trajectory Point (7) +(defvar *tp1* nil) +(defvar *tp2* nil) +(defvar *tp3* nil) +(defvar *tp4* nil) +(defvar *tp5* nil) +(defvar *tp6* nil) +(defvar *tp7* nil) + +;;Define set-trajectory function +(defun set-trajectory (x) + (let ((prev-pos (float-vector x -20 40))) ; 最初の座標を原点に設定 + ;; tp1 + (setq *tp1* (make-cube 2 2 2)) + (send *tp1* :translate (v- (float-vector 0 5 0) (send *center* :pos))) + (send *tp1* :rotate #d90 :x) + (send *tp1* :set-color :red) + (setq prev-pos (send *tp1* :pos)) + ;; tp2 + (setq *tp2* (make-cube 2 2 2)) + (send *tp2* :translate (v- (float-vector 0 5 0) prev-pos)) + (send *tp2* :rotate #d90 :x) + (send *tp2* :set-color :red) + (setq prev-pos (send *tp2* :pos)) + ;; tp3 + (setq *tp3* (make-cube 2 2 2)) + (send *tp3* :translate (v- (float-vector 0 5 0) prev-pos)) + (send *tp3* :rotate #d90 :x) + (send *tp3* :set-color :red) + (setq prev-pos (send *tp3* :pos)) + ;; tp4 + (setq *tp4* (make-cube 2 2 2)) + (send *tp4* :translate (v- (float-vector 0 5 0) prev-pos)) + (send *tp4* :rotate #d90 :x) + (send *tp4* :set-color :red) + (setq prev-pos (send *tp4* :pos)) + ;; tp5 + (setq *tp5* (make-cube 2 2 2)) + (send *tp5* :translate (v- (float-vector 0 5 0) prev-pos)) + (send *tp5* :rotate #d90 :x) + (send *tp5* :set-color :red) + (setq prev-pos (send *tp5* :pos)) + ;; tp6 + (setq *tp6* (make-cube 2 2 2)) + (send *tp6* :translate (v- (float-vector 0 5 0) prev-pos)) + (send *tp6* :rotate #d90 :x) + (send *tp6* :set-color :red) + (setq prev-pos (send *tp6* :pos)) + ;; tp7 + (setq *tp7* (make-cube 2 2 2)) + (send *tp7* :translate (v- (float-vector 0 5 0) prev-pos)) + (send *tp7* :rotate #d90 :x) + (send *tp7* :set-color :red) + (setq prev-pos (send *tp7* :pos)))) + +(set-trajectory 0) + +;;Set Center of a Table +(setq *center* (make-cube 10 10 10)) +(send *center* :translate (float-vector 700 0 750)) +(send *center* :set-color :black) + + + + +;;Set coordination. +(send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *pr2*)) +(send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) + +;;Show all objects in IRTVIEWER +(objects (list *pr2* *center* *arrow* *desk* *needle* *hampen* *tp1* *tp2* *tp3* *tp4* *tp5* *tp6* *tp7*)) + + +(do ((i 0 (+ i 1))) ; i を 0 から 1 ずつ増加 + ((> i 3) 'done) ;; それぞれの位置と角度情報をリストに格納 ; i が 3 より大きくなったら終了 + (set-trajectory (- (* i 10) 3)) ; i の値に基づいて座標計算 + (send *irtviewer* :draw-objects) ; 立方体を描画 + (send *needle* :larm :inverse-kinematics + (send (send *tp1* :get :left-coords) :copy-worldcoords) + :rotation-axis :x) + (send *pr2* :larm :end-coords :assoc *needle*) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (unix:sleep 1)) ; 1秒間スリープ diff --git a/jsk_2024_10_semi/trajectory.l b/jsk_2024_10_semi/trajectory.l new file mode 100644 index 000000000..ca330275d --- /dev/null +++ b/jsk_2024_10_semi/trajectory.l @@ -0,0 +1,150 @@ +#!/usr/bin/env roseus + +;;Okada-seisei seminar +;;Surgery-method dev2 +;;last editted Nov 12 ;; Michi-Tsubaki + +;; +;;This program is for trying to make unique motion for sewing sponge. Still Trying + +;;Load packages for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") ;;pkg for speaking +(load "models/arrow-object.l") + +;;Set PR2 +(if (not (boundp '*pr2*)) (pr2-init)) + +;;Set Center of a Table +(setq *center* (make-cube 10 10 10)) +(send *center* :translate (float-vector 700 0 750)) +(send *center* :set-color :black) + +;;Set cube as a desk for surgery. +(setq *desk* (make-cube 500 500 750)) +(send *desk* :translate (float-vector 700 0 750/2)) +(send *desk* :set-color :brown) + +;;Set cylinder as needle. +(setq *needle* (make-cylinder 0.5 70)) +(send *needle* :translate (v+ (float-vector 0 20 100) (send *center* :pos))) +(send *needle* :set-color :yellow) +(send *needle* :rotate #d90 :x) + +;;Set hampen +(setq *hampen* (make-cube 50 50 50)) +(send *hampen* :translate (v+ (float-vector 0 0 10) (send *center* :pos))) +(send *hampen* :set-color :white) +(send *hampen* :rotate #d45 :x) + + + + +(defclass traj + :super cascaded-coords + :slots (points)) +(defmethod traj + (:init (&rest args) + (send-super* :init args) + (dotimes (i 10) + (push + (make-cube 10 10 10 :pos (float-vector (* (- i 5) 20) 0 0)) + points)) + (dotimes (i (- (length points) 1)) + (send (elt points i) :assoc (elt points (+ 1 i)))) + (send self :assoc (car points)) + self) ;; :init + (:points () points) + ) ;; defmethod +;;This program is for trying to make unique motion for sewing sponge. Still Trying + +;;Load pkgs for using PR2 model and PR2 robot in real. +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") ;;pkg for speaking +(load "models/arrow-object.l") + +;;Define Trajectory Point (7) +(defvar *tp1* nil) +(defvar *tp2* nil) +(defvar *tp3* nil) +(defvar *tp4* nil) +(defvar *tp5* nil) +(defvar *tp6* nil) +(defvar *tp7* nil) + +;;Set PR2 +(if (not (boundp '*pr2*)) (pr2-init)) + +;;Set Center of a Table +(setq *center* (make-cube 10 10 10)) +(send *center* :translate (float-vector 700 0 750)) +(send *center* :set-color :black) + +;;Set cube as a desk for surgery. +(setq *desk* (make-cube 500 500 750)) +(send *desk* :translate (float-vector 700 0 750/2)) +(send *desk* :set-color :brown) + +;;Set cylinder as needle. +;;Set needle +(setq *needle* (make-cylinder 0.5 70)) +(send *needle* :translate (v+ (float-vector 0 20 100) (send *center* :pos))) +(send *needle* :set-color :yellow) +(send *needle* :rotate #d90 :x) + + +;;Show Worldrecord +(setq *arrow* (arrow)) +(send *arrow* :copy-worldcoords) + +;;Show all objects in IRTVIEWER +(objects (list *pr2* *center* *arrow* *desk* *needle* *hampen*)) + + +;;Set initial pose +(send *ri* :speak-jp "初期姿勢に戻ります。" :wait t) +(send *pr2* :reset-pose) +(send *ri* :start-grasp :arms) + +(send *ri* :angle-vector (send *pr2* :angle-vector) 2000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "左手を開きます。針を持たせてください。") +(send *ri* :stop-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "閉じます。" :wait t) +(send *ri* :start-grasp :larm :wait t) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "これから針を持って作業します．離れてください．" :wait t) +(unix:sleep 2) + + +(setq r (instance traj :init)) +(send r :rotate pi/2 :z) +(send r :locate #f(800 0 900) :world) +(objects (append (list *pr2* *center* *arrow* *desk* *needle* *hampen*) (send r :points))) + +;;(objects (list *pr2* (elt (send r :points) 0) (elt (send r :points) 1) (elt (send r :points) 2) + +(dolist (e (send r :points) + (send *pr2* :larm :inverse-kinematics e + :rotation-axis t + :debug-view t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (format t "debug~%") + (unix:sleep 1) + ) + + ) +