Skip to content

Commit

Permalink
Add set-grasp-style state in stow
Browse files Browse the repository at this point in the history
  • Loading branch information
@pazeshun
pazeshun committed Jun 20, 2017
1 parent 7db4bd3 commit 9b17359
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 71 deletions.
111 changes: 52 additions & 59 deletions jsk_arc2017_baxter/euslisp/lib/arc-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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-))))
Expand All @@ -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
Expand All @@ -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)))
Expand Down Expand Up @@ -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
Expand All @@ -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))
Expand All @@ -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)))
Expand All @@ -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
Expand All @@ -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)
Expand Down
27 changes: 17 additions & 10 deletions jsk_arc2017_baxter/euslisp/lib/stow-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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)))
Expand Down
8 changes: 7 additions & 1 deletion jsk_arc2017_baxter/euslisp/stow-main.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
3 changes: 2 additions & 1 deletion jsk_arc2017_baxter/node_scripts/state_server.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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':
Expand All @@ -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' \
Expand Down

0 comments on commit 9b17359

Please sign in to comment.