[WIP] [DO NOT MERGE] Add all demonstations tsukamoto has made

jsk_2020_10_semi/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_2020_10_semi)
+
+find_package(catkin REQUIRED COMPONENTS)
+
+catkin_package()
+
+install(DIRECTORY euslisp
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+

jsk_2020_10_semi/README.md

@@ -0,0 +1,2 @@
+# jsk_2020_10_semi
+

jsk_2020_10_semi/euslisp/arduino.l

@@ -0,0 +1,11 @@
+#!/usr/bin/env roseus
+
+(ros::roseus-add-msgs "std_msgs")
+
+(ros::roseus "arduino_client")
+
+(defun arduino-cb (msg) (print (list 'cb (send msg :data))))
+(ros::subscribe "arduino" std_msgs::Int16 #'arduino-cb)
+
+(ros::spin)
+(exit)

jsk_2020_10_semi/euslisp/delivery.l

@@ -0,0 +1,84 @@
+#!/usr/bin/env roseus
+
+;; load robot and scene model
+(require "package://fetcheus/fetch-utils.l")
+(require "package://fetcheus/fetch-interface.l")
+(require "package://jsk_maps/src/eng2-scene.l")
+
+(unless (boundp '*scene*)
+  (setq *scene* (make-eng2-7f-scene)))
+(unless (boundp '*ri*)
+  (fetch-init)) ;; fetch-init instantiate *fetch* and *ri*
+
+;; show in irtviewer
+(objects (list *fetch* *scene*))
+;; show in "fetch Kinematics Simulator"
+(send *ri* :objects (send *scene* :objects))
+(defun move-dock-front ()
+  (setq *dock-front* (send (send *scene* :spot "/eng2/7f/room73B2-fetch-dock-front") :copy-worldcoords))
+  (send *fetch* :move-to *dock-front* :world))
+
+(send *irtviewer* :look-all
+      (send (geo::make-bounding-box
+             (flatten (send-all (send *fetch* :bodies) :vertices)))
+            :grow 2.0))
+
+(defun move-73-A2-front ()
+  (setq *room73A2-front* (send (send *scene* :spot "/eng2/7f/room73A2-front") :copy-worldcoords))
+  (send *fetch* :move-to *room73A2-front* :world)
+  ;;(send *ri* :move-to *room73A2-front* :world)
+  )
+;;(send *fetch* :locate #f(300 0 0))
+(
+;;(send *ri* :move-to *room73A2-front* :world)
+#|
+(setq *room73A3-front* (send (send *scene* :spot "/eng2/7f/room73A3-front") :copy-worldcoords))
+(send *fetch* :move-to *room73A3-front* :world)
+
+
+
+|#
+
+(defun pose-of-give ()
+  (send *fetch* :rarm :inverse-kinematics (send (send *fetch* :copy-worldcoords) :transform (make-coords :pos #f(600 300 1000))) :debug-view t))
+
+#|
+(defun raise-hand ()
+  (send *fetch* :angle-vector #f(5.0000 88.0000 -42.0000 0 -60 1 -77.0000 7.000 0 2.0000)))
+|#
+#|
+(defun catch ()
+  (send *fetch* :angle-vector #f(5 52 3 77 -59 10 -78 74 0 2)))
+|#
+
+#| 顔を検知してそこに描かれた長方形の面積を求める |#
+(defclass face_area
+  :slots (*rect*)
+  )
+(defmethod face_area
+  (:init ()
+	 (ros::roseus "face_recognized_client")
+	 (ros::subscribe "edgetpu_face_detector/output/rects" jsk_recognition_msgs::RectArray #'send self :cb)
+	 (setq *rect* nil)
+	 (ros::ros-info "init")
+	 self)
+  (:cb (msg)
+       (setq *rect* (send msg :rects))
+       )
+  (:loop ()
+	 (ros::rate 10)
+	 (while (ros::ok)
+	   (if *rect*
+	       (progn
+		 (setq w (send (elt *rect* 0) :width))
+		 (setq h (send (elt *rect* 0) :height))
+		 (setq *area* (* w h))
+		 (ros::ros-info "area : ~A" *area*)
+		 )
+	     )
+	     (ros::spin-once)
+	     (ros::sleep)
+	    )
+	 (exit)
+	 )
+  )

jsk_2020_10_semi/euslisp/error_1202.txt

@@ -0,0 +1,25 @@
+24.E1-irteusgl$ (send *ri* :angle-vector (send *fetch* :angle-vector) 5000 :use-torso nil)
+[ WARN] [1606921302.638850734]: continuous joint (upperarm_roll_joint) moves 360.0 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
+Call Stack (max depth: 20):
+  0: at (apply #'send-message self (class . super) :angle-vector-sequence args)
+  1: at (apply #'send-message self (class . super) :angle-vector-sequence args)
+  2: at (apply #'send-message self (class . super) :angle-vector-sequence args)
+  3: at (send-super* :angle-vector-sequence args)
+  4: at (apply #'send self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args)
+  5: at (apply #'send self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args)
+  6: at (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args)
+  7: at (let* (avs (minjerk (instance minjerk-interpolator :init)) (scale-av (send self :sub-angle-vector av prev-av)) dist div) (setq dist (abs (find-extream (coerce diff-av cons) #'abs #'>=))) (setq div (round (/ dist 120.0))) (send minjerk :reset :position-list (list prev-av (v+ prev-av scale-av)) :time-list (list tm)) (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) (return-from :angle-vector-raw (car (last avs))))
+  8: at (progn (let* (avs (minjerk (instance minjerk-interpolator :init)) (scale-av (send self :sub-angle-vector av prev-av)) dist div) (setq dist (abs (find-extream (coerce diff-av cons) #'abs #'>=))) (setq div (round (/ dist 120.0))) (send minjerk :reset :position-list (list prev-av (v+ prev-av scale-av)) :time-list (list tm)) (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) (return-from :angle-vector-raw (car (last avs)))))
+  9: at (if (send self :check-continuous-joint-move-over-180 diff-av) (progn (let* (avs (minjerk (instance minjerk-interpolator :init)) (scale-av (send self :sub-angle-vector av prev-av)) dist div) (setq dist (abs (find-extream (coerce diff-av cons) #'abs #'>=))) (setq div (round (/ dist 120.0))) (send minjerk :reset :position-list (list prev-av (v+ prev-av scale-av)) :time-list (list tm)) (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) (return-from :angle-vector-raw (car (last avs))))))
+  10: at (when (send self :check-continuous-joint-move-over-180 diff-av) (let* (avs (minjerk (instance minjerk-interpolator :init)) (scale-av (send self :sub-angle-vector av prev-av)) dist div) (setq dist (abs (find-extream (coerce diff-av cons) #'abs #'>=))) (setq div (round (/ dist 120.0))) (send minjerk :reset :position-list (list prev-av (v+ prev-av scale-av)) :time-list (list tm)) (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) (return-from :angle-vector-raw (car (last avs)))))
+  11: at (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) (diff-av (v- av prev-av))) (when (send self :check-continuous-joint-move-over-180 diff-av) (let* (avs (minjerk (instance minjerk-interpolator :init)) (scale-av (send self :sub-angle-vector av prev-av)) dist div) (setq dist (abs (find-extream (coerce diff-av cons) #'abs #'>=))) (setq div (round (/ dist 120.0))) (send minjerk :reset :position-list (list prev-av (v+ prev-av scale-av)) :time-list (list tm)) (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) args) (return-from :angle-vector-raw (car (last avs))))) (send-super* :angle-vector av tm args))
+  12: at (apply #'send self :angle-vector-raw av tm ctype start-time args)
+  13: at (apply #'send self :angle-vector-raw av tm ctype start-time args)
+  14: at (send* self :angle-vector-raw av tm ctype start-time args)
+  15: at (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))
+  16: at (progn (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
+  17: at (if (send self :simulation-modep) (progn (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))))
+  18: at (when (send self :simulation-modep) (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
+  19: at (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (use-torso t) (clear-velocities t)) (if (= (length args) 1) (setq ctype (car args) args (cdr args))) (if (and (>= (length args) 2) (null (member (car args) '(:use-torso :start-time :clear-velocities)))) (setq ctype (car args) start-time (cadr args) args (cddr args))) (if (member :use-torso args) (setq use-torso (cadr (member :use-torso args)))) (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) (when (send self :simulation-modep) (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) (when (not (numberp tm)) (warn ":angle-vector tm is not a number, use :angle-vector av tm args")) (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm :start-offset-time (if start-offset-time start-offset-time start-time) :clear-velocities clear-velocities :use-torso use-torso args))
+  And more...
+/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: no such keyword :use-torso in (apply #'send-message self (class . super) :angle-vector-sequence args)

jsk_2020_10_semi/euslisp/face_area_subscribe.l

@@ -0,0 +1,38 @@
+#!/usr/bin/env roseus
+
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+(ros::roseus-add-msgs "geometry_msgs")
+
+(defclass face_area
+  :slots (*rect*))
+(defmethod face_area
+  (:init ()
+	 (ros::roseus "face_recognized_client")
+	 (ros::subscribe "/edgetpu_face_detector/output/rects" jsk_recognition_msgs::RectArray #'send self :cb)
+	 (setq *rect* nil)
+	 (ros::ros-info "init")
+	 self)
+  (:cb (msg)
+       (setq *rect* (send msg :rects))
+       )
+  (:loop ()
+	 (ros::rate 10)
+	 ;;(setq alpha 0.001)
+	 (ros::rate 10)
+	 (while (ros::ok)
+	   ;;(setq alpha 0.001)
+	   (if *rect*
+	       (progn
+		 (ros::ros-info "hello")
+		 (ros::ros-info "width: ~A" (send (car *rect*) :width))
+		 )
+	     )
+	   (ros::spin-once)
+	   (ros::sleep)
+	   )
+	 (exit)
+	 )
+  )
+
+(setq *ri* (instance face_area :init))
+(send *ri* :loop)

jsk_2020_10_semi/euslisp/face_rect_subscribe.l

@@ -0,0 +1,52 @@
+#!/usr/bin/env roseus
+
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+;;(ros::roseus-add-msgs "geometry_msgs")
+#|
+;; これはpublish
+;;(ros::advertise "/edgetpu_face_detector/output/rects" jsk_recognition_msgs:;;:RectArray 1)
+
+;;(unix::sleep 1)
+;;(ros::duration-sleep 1)
+;;(print (ros::get-num-subscribers "/edgetpu_face_detector/output/rects"))
+|#
+
+;; 大域変数として定義
+
+;;(setq *rect* (instance jsk_recognition_msgs::RectArray :init))
+(setq *rect* nil)
+
+
+(defun cb (msg)
+  (if msg
+      (setq *rect* (send msg :rects))
+    )
+)
+;;main
+(ros::roseus "face_rect_client")
+(ros::subscribe "/edgetpu_face_detector/output/rects" jsk_recognition_msgs::RectArray #'cb)
+
+(ros::rate 10)
+(while (ros::ok)
+  (if *rect*
+      (progn
+	;;(ros::ros-info "hello")
+	;;(ros::ros-info "~A" (send (car *rect*))
+	(ros::ros-info "x: ~A" (send (car *rect*) :x))
+	(ros::ros-info "y: ~A" (send (car *rect*) :y))
+	(ros::ros-info "width: ~A" (send (car *rect*) :width))
+	(ros::ros-info "height: ~A" (send (car *rect*) :height))
+
+	;; これらは見慣れない書き方らしい
+	;;(setq w (send (car *rect*) :width)) 
+	;;(setq h (send (car *rect*) :height))
+
+	;; 良い書き方
+	(setq w (send (elt *rect* 0) :width))
+	(setq h (send (elt *rect* 0) :height))
+	(ros::ros-info "area : ~A" (* w h))
+	)
+    )
+  (ros::spin-once)
+  (ros::sleep)
+  )

jsk_2020_10_semi/euslisp/face_rect_subscribe_class.l

@@ -0,0 +1,34 @@
+#!/usr/bin/env roseus
+
+#| 顔に描かれた四角形の面積を表示させる |#
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+
+(defclass face_area
+  :slots (*rect*)
+  )
+(defmethod face_area
+  (:init ()
+	 (ros::roseus "face_recognized_client")
+	 (ros::subscribe "/edgetpu_face_detector/output/rects" jsk_recognition_msgs::RectArray #'send self :cb)
+	 (setq *rect* nil)
+	 (ros::ros-info "init")
+	 self)
+  (:cb (msg)
+       (setq *rect* (send msg :rects))
+       )
+  (:loop ()
+	 (ros::rate 10)
+	 (while (ros::ok)
+	   (if *rect*
+	       (progn
+		 (setq w (send (elt *rect* 0) :width))
+		 (setq h (send (elt *rect* 0) :height))
+		 (ros::ros-info "area: ~A" (* w h))
+		 )
+	     )
+	     (ros::spin-once)
+	     (ros::sleep)
+	    )
+	 (exit)
+	 )
+  )

jsk_2020_10_semi/euslisp/map.txt

@@ -0,0 +1,12 @@
+7.irteusgl$ (mapcar #'(lambda (x) (send x :name)) (send *scene* :spots))
+("/eng2/7f/room73B2-front-of-kitchenboard" "/eng2/7f/room73B2-sink-front" "/eng2/7f/room73B2-beside-chair" "/eng2/7f/room73B2-far-chair-back" "/eng2/7f/room73B2-tmp-chair-back" "/eng2/7f/room73B2-chair-back" "/eng2/7f/room73B2-table-front" "/eng2/7f/room73B2-table-back" "/eng2/7f/room73B2-table-side" "/eng2/7f/room73B2-front-kitchen-table" "/eng2/7f/room73B2-front-of-tv" "door-spot" "coe-spot" "broom-spot" "table-spot" "cook-spot" "fridge-front-spot" "init-spot" "door-spot" "table-spot" "cook-spot" "init-spot" "/eng2/7f/elevator_inside_panel" "/eng2/7f/elevator_call_panel" "/eng2/7f/room73B2-fetch-dock-front" "/eng2/7f/room73A3-front1" "/eng2/7f/room73B2-trashbox-front" "/eng2/7f/room73B2-counter-side" "/eng2/7f/room73B2-center" "/eng2/7f/room73B2-door-front" "/eng2/7f/room73B2-microwave-front" "/eng2/7f/room73B2-sink-front0" "/eng2/7f/room73B2-sink-front1" "/eng2/7f/room73B2-table-front1" "/eng2/7f/room73B2-table-side1" "/eng2/7f/room73B2-fridge-front" "/eng2/7f/room73A3-printer-front" "/eng2/7f/room73B1-kurotobi" "/eng2/7f/mini-kitchen-A-inside" "/eng2/7f/room73B1-nakanishi" "/eng2/7f/non-combustible-trashbox-front" "/eng2/7f/combustible-trashbox-front" "/eng2/7f/room73A1-front" "/eng2/7f/room73A2-front" "/eng2/7f/room73A3-base" "/eng2/7f/room73A3-front" "/eng2/7f/room73A4-front" "/eng2/7f/before-elevator" "/eng2/7f/elevator-outside" "/eng2/7f/elevator_inside_panel-front" "/eng2/7f/elevator_call_panel-front")
+
+3.irteusgl$ (send *fetch* :joint-list)
+(#<linear-joint #X55e397f31e20 torso_lift_joint> #<rotational-joint #X55e397f2de68 shoulder_pan_joint> #<rotational-joint #X55e397f14fc0 shoulder_lift_joint> #<rotational-joint #X55e397f60658 upperarm_roll_joint> #<rotational-joint #X55e399d8d2c0 elbow_flex_joint> #<rotational-joint #X55e399d8d530 forearm_roll_joint> #<rotational-joint #X55e397f7f078 wrist_flex_joint> #<rotational-joint #X55e397f7ea18 wrist_roll_joint> #<rotational-joint #X55e397ee4898 head_pan_joint> #<rotational-joint #X55e397ef23f0 head_tilt_joint>)
+
+32.E1-irteusgl$ (send *fetch* :rarm)
+(#<bodyset-link #X55e397588458 shoulder_pan_link  5500.0 6228.65 807.616 / 3.142 0.0 0.0> #<bodyset-link #X55e3975871b0 shoulder_lift_link  5383.0 6228.65 867.616 / 3.142 1.4 -1.207e-16> #<bodyset-link #X55e3975d40d8 upperarm_roll_link  5345.777 6228.65 651.803 / 3.142 1.4 -0.2> #<bodyset-link #X55e3975e3cd0 elbow_flex_link  5323.172 6228.65 520.738 / 0.198 0.018 -3.108> #<bodyset-link #X55e3975e3148 forearm_roll_link  5516.299 6267.353 517.145 / 0.198 0.018 -3.108> #<bodyset-link #X55e3975e08c8 wrist_flex_link  5638.351 6291.813 514.874 / -2.501 -1.492 -0.445> #<bodyset-link #X55e3975e0070 wrist_roll_link  5629.637 6285.315 652.946 / -2.501 -1.492 -0.445>)
+
+関節名リスト
+10.irteusgl$ (send *fetch* :methods :rarm)
+(:upperarm_roll_joint :upperarm_roll_link_lk :rarm-shoulder-y :rarm-shoulder-p :rarm-shoulder-r :rarm-elbow-p :rarm-elbow-r :rarm-wrist-p :rarm-wrist-r :_make_instance_upperarm_roll_link_geom0 :rarm-end-coords :rarm-root-link :rarm)

jsk_2020_10_semi/euslisp/sample.l

@@ -0,0 +1,49 @@
+#!/usr/bin/env roseus~/semi_ws/src/jsk_demos/jsk_2020_10_semi/euslisp
+
+;; load robot and scene model
+(require "package://fetcheus/fetch-utils.l")
+(require "package://fetcheus/fetch-interface.l")
+(require "package://jsk_maps/src/eng2-scene.l")
+
+(unless (boundp '*scene*)
+  (setq *scene* (make-eng2-7f-scene)))
+(unless (boundp '*ri*)
+  (fetch-init)) ;; fetch-init instantiate *fetch* and *ri*
+
+;; show in irtviewer
+(objects (list *fetch* *scene*))
+;; show in "fetch Kinematics Simulator"
+(send *ri* :objects (send *scene* :objects))
+
+;; use (mapcar #'(lambda (x) (send x :name)) (send *scene* :spots)) to get list of spots
+(send *fetch* :move-to (send *scene* :spot "/eng2/7f/room73B2-fetch-dock-front") :world)
+
+;; check 20201026-soft3-model, the code to focus on the robot model
+(send *irtviewer* :look-all
+      (send (geo::make-bounding-box
+             (flatten (send-all (send *fetch* :bodies) :vertices)))
+            :grow 2.0))
+
+
+;; start to move
+(setq *shelf-front* (send (send (send *scene* :spot "/eng2/7f/room73B2-table-side") :copy-worldcoords) :rotate pi/2 :z))
+(send *fetch* :move-to *shelf-front* :world) ;; moved in irtviewer
+(send *irtviewer* :draw-objects) ;; update viewer
+(send *ri* :move-to *shelf-front*) ;; moved real robot
+
+;; move arms
+(send *fetch* :rarm :angle-vector #f(-55 80 -45 -110 0 30 15))
+(send *irtviewer* :draw-objects) ;; update viewer
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation t)
+
+
+;; back to original position
+(send *fetch* :reset-pose)
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation t)
+(send *irtviewer* :draw-objects) ;; update viewer
+
+(send *fetch* :move-to (send *scene* :spot "/eng2/7f/room73B2-fetch-dock-front") :world)
+(send *irtviewer* :draw-objects) ;; update viewer
+(send *ri* :move-to  (send *scene* :spot "/eng2/7f/room73B2-fetch-dock-front"))