From 9b1735960b111f591522d7f634260dfb156a641e Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 20 Jun 2017 22:04:19 +0900 Subject: [PATCH] Add set-grasp-style state in stow --- .../euslisp/lib/arc-interface.l | 111 ++++++++---------- .../euslisp/lib/stow-interface.l | 27 +++-- jsk_arc2017_baxter/euslisp/stow-main.l | 8 +- .../node_scripts/state_server.py | 3 +- 4 files changed, 78 insertions(+), 71 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 7f4544082..27731dac4 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -213,7 +213,8 @@ (setq graspingp (send* self :pick-object-with-movable-region arm movable-region args)) graspingp)) (:pick-object-with-movable-region - (arm movable-region &key (n-trial 1) (n-trial-same-pos 1) (do-stop-grasp nil)) + (arm movable-region &key (n-trial 1) (n-trial-same-pos 1) + (do-stop-grasp nil) (grasp-style :suction)) (let (graspingp avs object-index) ;; TODO: object-index is set randomly (setq object-index (random (length (gethash arm object-boxes-)))) @@ -233,18 +234,28 @@ (send *ri* :wait-interpolation)) (let (prev-av next-av gripper-x) (setq prev-av (send *baxter* :angle-vector)) - (setq next-av - (send *baxter* arm :inverse-kinematics - (make-coords :pos (v+ obj-pos #f(0 0 150)) - :rpy #f(0 0 0)) - :use-gripper t - :rotation-axis :z - :stop 200 - :additional-check - #'(lambda () - (and - (< (send *baxter* arm :gripper-x :joint-angle) 110) - (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))) + (if (eq grasp-style :suction) + (setq next-av + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos #f(0 0 150)) + :rpy #f(0 0 0)) + :use-gripper t + :rotation-axis :z + :stop 200 + :additional-check + #'(lambda () + (and + (< (send *baxter* arm :gripper-x :joint-angle) 110) + (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))) + (progn + (send *baxter* :rotate-gripper :rarm 0 :relative nil) + (send *baxter* :slide-gripper :rarm 60 :relative nil) + (setq next-av + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos #f(0 0 200)) + :rpy (float-vector pi/2 0 0)) + :move-palm-end t + :rotation-axis t)))) (setq gripper-x (send *baxter* arm :gripper-x :joint-angle)) (send *baxter* :angle-vector prev-av) ;; First, move prismatic joint to target position @@ -266,10 +277,9 @@ (dotimes (j n-trial-same-pos) (unless graspingp (setq graspingp - (send self :try-to-suction-object arm obj-pos - :offset (float-vector 0 0 (- (* i -50) 30)))) - (if (and (eq arm :rarm) (null graspingp)) - (setq graspingp + (if (eq grasp-style :suction) + (send self :try-to-suction-object arm obj-pos + :offset (float-vector 0 0 (- (* i -50) 30))) (send self :try-to-pinch-object arm obj-pos :offset (float-vector 0 0 (- (* i -50) 30)))))))) (when do-stop-grasp (unless graspingp (send *ri* :stop-grasp arm))) @@ -301,7 +311,7 @@ obj-pos)) (:try-to-suction-object (arm obj-pos &key (offset #f(0 0 0))) - (let (graspingp prev-av target-av pad-angle) + (let (graspingp) (if (eq arm :larm) (progn (send *ri* :angle-vector-raw @@ -317,17 +327,13 @@ (send *ri* :start-grasp arm) (unix::sleep 1)) (progn - (send *ri* :stop-grasp arm :hand) - (send *baxter* :angle-vector - (send *ri* :state :potentio-vector :wait-until-update t)) - (send *ri* :angle-vector-raw (send *baxter* :slide-gripper arm 60 :relative nil) - :fast (send *ri* :get-arm-controller arm) 0 :scale 1.0) - (send *ri* :wait-interpolation) ;; Fold fingers again (send *ri* :move-hand arm (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000) - (setq prev-av (send *baxter* :angle-vector)) - (setq target-av + ;; start the vacuum gripper before approaching to the object + (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm) + (send *ri* :start-grasp arm) + (send *ri* :angle-vector-raw (send *baxter* arm :inverse-kinematics (make-coords :pos (v+ obj-pos offset) :rpy #f(0 0 0)) @@ -338,17 +344,7 @@ #'(lambda () (and (< (send *baxter* arm :gripper-x :joint-angle) 110) - (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))) - (setq pad-angle (send *baxter* arm :gripper-p :joint-angle)) - (send *baxter* :angle-vector prev-av) - (send *baxter* arm :gripper-p :joint-angle pad-angle) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - :fast (send *ri* :get-arm-controller arm) 0 :scale 1.0) - (send *ri* :wait-interpolation) - ;; start the vacuum gripper before approaching to the object - (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm) - (send *ri* :start-grasp arm) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector target-av) + (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))) 3000 (send *ri* :get-arm-controller arm) 0) ;; Wait until grasp or push object to prevent tooth flying (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))) @@ -367,18 +363,26 @@ (unless graspingp (ros::ros-info "[:try-to-suction-object] arm:~a again approach to the object" arm) (let ((temp-av (send *baxter* :angle-vector))) - ;; only if robot can solve IK - (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector) - 3000 (send *ri* :get-arm-controller arm) 0)) - (if (eq arm :larm) - (send *ri* :wait-interpolation-until-grasp arm) - (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)) - (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) - 3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter (if (eq arm :larm) - (send *ri* :wait-interpolation-until-grasp arm) - (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)))) + (progn + ;; only if robot can solve IK + (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) + 3000 (send *ri* :get-arm-controller arm) 0)) + (send *ri* :wait-interpolation-until-grasp arm) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) + 3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter + (send *ri* :wait-interpolation-until-grasp arm) + ) + (progn + ;; only if robot can solve IK + (if (send *baxter* arm :move-end-pos #f(0 0 -100) :local) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) + 3000 (send *ri* :get-arm-controller arm) 0)) + (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) + 3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter + (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))))) (when (eq arm :rarm) ;; Open fingers in bin (send *ri* :move-hand arm @@ -399,17 +403,6 @@ ;; Initialize finger (send *ri* :move-hand arm (send *baxter* :hand-grasp-pose arm :cylindrical :angle 30) 1000) - ;; Initialize arm - (send *baxter* :rotate-gripper :rarm 0 :relative nil) - (send *baxter* :slide-gripper :rarm 60 :relative nil) - (send *ri* :angle-vector - (send *baxter* arm :inverse-kinematics - (make-coords :pos (v+ obj-pos #f(0 0 200)) - :rpy (float-vector pi/2 0 0)) - :move-palm-end t - :rotation-axis t) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :wait-interpolation) ;; start the vacuum gripper before approaching to the object (ros::ros-info "[:try-to-pinch-object] arm:~a start vacuum gripper" arm) (send *ri* :start-grasp arm) diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l index 9cb00c75b..0e3c47561 100644 --- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l @@ -12,19 +12,21 @@ (defclass jsk_arc2017_baxter::stow-interface :super jsk_arc2017_baxter::arc-interface :slots (graspingp - moveit-p + moveit-p picking-fail-count recognize-fail-count target-bin target-obj - tote-contents)) + tote-contents + grasp-style)) (defmethod jsk_arc2017_baxter::stow-interface (:init (&key (moveit nil)) (send-super :init) (setq picking-fail-count 0) (setq recognize-fail-count 0) - (setq moveit-p moveit)) + (setq moveit-p moveit) + (setq grasp-style nil)) (:recognize-bboxes (arm) (ros::ros-info "[main] recognizing bin boxes") (send self :recognize-bin-boxes :stamp (ros::time-now)) @@ -71,14 +73,18 @@ (incf recognize-fail-count) (ros::ros-info "[main] arm: ~a, recognize fail count: ~a" arm recognize-fail-count) (> recognize-fail-count 1)) + (:set-grasp-style (arm) + (if (and (eq grasp-style :suction) (eq arm :rarm)) + (setq grasp-style :pinch) (setq grasp-style :suction))) (:pick-object (arm) (setq recognize-fail-count 0) (when moveit-p (send self :delete-tote-scene arm)) (send self :pick-object-in-tote arm - :n-trial 2 + :n-trial 1 :n-trial-same-pos 1 - :do-stop-grasp nil) - (if (send *ri* :finger-closep arm) + :do-stop-grasp nil + :grasp-style grasp-style) + (if (eq grasp-style :pinch) (send *ri* :angle-vector (send self :ik->tote-center arm :offset #f(0 0 300) :rotation-axis :z :move-palm-end t) @@ -144,7 +150,7 @@ (ros::ros-info "[main] ~a, place object in bin ~a" arm target-bin) (when moveit-p (send self :add-tote-scene arm)) (send *baxter* :head_pan :joint-angle (if (eq arm :larm) 80 -80)) - (if (send *ri* :finger-closep arm) + (if (eq grasp-style :pinch) (send *ri* :angle-vector (send self :ik->bin-center arm target-bin :offset #f(0 0 300) :rotation-axis nil :move-palm-end t) @@ -172,7 +178,7 @@ (when moveit-p (send self :delete-bin-scene target-bin)) (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 -300) :world - :rotation-axis (not (send *ri* :finger-closep arm))) + :rotation-axis (eq grasp-style :suction)) 2000 (send *ri* :get-arm-controller arm) 0) (send *ri* :wait-interpolation) (send *ri* :stop-grasp arm :suction) ;; release object @@ -182,13 +188,14 @@ (send *ri* :wait-interpolation) (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 300) :world - :rotation-axis (not (send *ri* :finger-closep arm))) + :rotation-axis (eq grasp-style :suction)) 1000 (send *ri* :get-arm-controller arm) 0) (send *ri* :wait-interpolation) (if (eq arm :rarm) (send *ri* :move-hand arm (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)) - (when moveit-p (send self :add-bin-scene target-bin)))))) + (when moveit-p (send self :add-bin-scene target-bin)))) + (setq grasp-style nil))) (:return-from-place-object (arm) (send self :fold-pose-back arm) (send *ri* :wait-interpolation))) diff --git a/jsk_arc2017_baxter/euslisp/stow-main.l b/jsk_arc2017_baxter/euslisp/stow-main.l index d5af912e2..7d94bc763 100755 --- a/jsk_arc2017_baxter/euslisp/stow-main.l +++ b/jsk_arc2017_baxter/euslisp/stow-main.l @@ -17,11 +17,12 @@ (:wait-for-user-input !-> :finish) (:wait-for-opposite-arm -> :recognize-object) (:wait-for-opposite-arm !-> :wait-for-opposite-arm) - (:recognize-object -> :pick-object) + (:recognize-object -> :set-grasp-style) (:recognize-object !-> :return-from-recognize-object) (:return-from-recognize-object -> :check-recognize-fail-count) (:check-recognize-fail-count -> :wait-for-user-input) (:check-recognize-fail-count !-> :wait-for-opposite-arm-start-picking) + (:set-grasp-style -> :pick-object) (:pick-object -> :verify-object) (:pick-object !-> :return-from-pick-object) (:return-from-pick-object -> :check-picking-fail-count) @@ -68,6 +69,11 @@ '(lambda (userdata) (send *ti* :update-state *arm* :check-recognize-fail-count) (send *ti* :check-recognize-fail-count *arm*))) + (:set-grasp-style + '(lambda (userdata) + (send *ti* :update-state *arm* :set-grasp-style) + (send *ti* :set-grasp-style *arm*) + t)) (:pick-object '(lambda (userdata) (send *ti* :update-state *arm* :pick-object) diff --git a/jsk_arc2017_baxter/node_scripts/state_server.py b/jsk_arc2017_baxter/node_scripts/state_server.py index 2267512a5..377fed7af 100755 --- a/jsk_arc2017_baxter/node_scripts/state_server.py +++ b/jsk_arc2017_baxter/node_scripts/state_server.py @@ -104,7 +104,7 @@ def _get_can_start(self, state, opposite_state): else: can_start = True else: - # pick task: wait condition + # stow task: wait condition if state == 'wait-for-opposite-arm-start-picking': if opposite_state == 'recognize-object' \ or opposite_state == 'wait-for-user-input': @@ -113,6 +113,7 @@ def _get_can_start(self, state, opposite_state): can_start = False elif state == 'wait-for-opposite-arm': if opposite_state == 'recognize-object' \ + or opposite_state == 'set-grasp-style' \ or opposite_state == 'pick-object' \ or opposite_state == 'verify-object' \ or opposite_state == 'set-target-bin' \