Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP] Add pinch and suction grasp #2238

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

[WIP] Add pinch and suction grasp #2238

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
53c4c99
[wip] add suction-and-pinch grasp style
708yamaguchi Jul 7, 2017
2c6a70c
add check-suction-and-cylindrical.l
708yamaguchi Jul 9, 2017
7a82334
add check-suction-and-opposed.l
708yamaguchi Jul 17, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
101 changes: 101 additions & 0 deletions jsk_arc2017_baxter/euslisp/check-suction-and-cylindrical.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
#!/usr/bin/env roseus

(require "package://jsk_arc2017_baxter/euslisp/lib/arc-interface.l")

(ros::load-ros-manifest "jsk_arc2017_baxter")

(unless (boundp '*baxter*) (jsk_arc2017_baxter::arc-init :ctype :rarm-controller))

(defun test ()
(let (prev-av next-av gripper-x pre-end-coords)
(setq prev-av (send *baxter* :angle-vector))
(send *ri* :move-hand :rarm
(send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 0) 1000)
;; :pick-object-with-movable-region
(setq next-av
;; :rotate-gripper :rarm 0
;; :slide-gripper :rarm 80
;; end-coords #f(600 -700 0)
(send *baxter* :rarm :angle-vector
#f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 80.0 0.0)))
(setq gripper-x (send *baxter* :rarm :gripper-x :joint-angle))
(send *baxter* :angle-vector prev-av)
;; First, move prismatic joint to target position
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper :rarm gripper-x :relative nil)
:fast (send *ri* :get-arm-controller :rarm) 0 :scale 5.0)
(send *ri* :wait-interpolation)
;; Fold fingers to avoid collision
(send *ri* :move-hand :rarm
(send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000)
(send *ri* :move-hand :rarm
(send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 110) 1000)
;; Move whole arm to target pose
(send *baxter* :rarm :angle-vector next-av)
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000)

(send *ri* :wait-interpolation)
;; :try-to-suction-and-pinch-object
(send *ri* :move-hand :rarm (send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 110) 1000)

(send *ri* :start-grasp :rarm)

;;(break)

;; :rotate-gripper :rarm 0
;; :slide-gripper :rarm 80
;; end-coords #f(600 -700 -300)
(send *baxter* :rarm :angle-vector
#f(-4.48961 -32.8098 21.798 65.4342 -20.9014 61.0237 -26.78 80.0 0.0))
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 6000)
;; Wait until finger touch
(send *ri* :wait-interpolation-until :rarm
:grasp :finger-flexion :finger-loaded :prismatic-loaded)
(unix::sleep 3)
(send *baxter* :angle-vector
(send *ri* :state :potentio-vector :wait-until-update t))
(setq pre-end-coords (send *baxter* :rarm :end-coords :copy-worldcoords))
;; cylindrically grasp object
(send *ri* :move-hand :rarm
(send *baxter*
:hand-grasp-pose :rarm :cylindrical :angle 40) 1000)
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper :rarm 0 :relative nil)
3000 (send *ri* :get-arm-controller :rarm) 0)
(send *ri* :wait-interpolation)
(send *ri* :move-hand :rarm
(send *baxter*
:hand-grasp-pose :rarm :cylindrical) 1000 :wait nil)
(send *ri* :wait-interpolation-until :rarm :finger-flexion :hand)
(send pre-end-coords :translate #f(0 0 30))
(send *ri* :angle-vector-raw
(send *baxter* :rarm :inverse-kinematics pre-end-coords :rotation-axis t :use-gripper nil)
3000)
(send *ri* :wait-interpolation)
(send *ri* :stop-grasp :rarm :suction)
(unix::sleep 1)
(send *ri* :move-hand :rarm
(send *baxter*
:hand-grasp-pose :rarm :cylindrical :angle 120) 1000)
(send *baxter* :rarm :move-end-pos #f(0 0 10))
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(send *baxter* :rotate-gripper :rarm -90 :relative nil)
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(send *ri* :start-grasp :rarm)
(unix::sleep 1)
(send *baxter* :rotate-gripper :rarm 0 :relative nil)
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000)
(send *ri* :move-hand :rarm
(send *baxter*
:hand-grasp-pose :rarm :cylindrical :angle 180) 4000)
(unix::sleep 1)
(send *baxter* :rarm :angle-vector
#f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 80.0 0.0))
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000
(send *ri* :get-arm-controller :rarm :gripper nil) 0)
(send *ri* :wait-interpolation)
;; (unix::sleep 3)
;; (send *ri* :stop-grasp :rarm :suction)
;; (send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector #f(90 0)) 1000)))
79 changes: 79 additions & 0 deletions jsk_arc2017_baxter/euslisp/check-suction-and-opposed.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
#!/usr/bin/env roseus

(require "package://jsk_arc2017_baxter/euslisp/lib/arc-interface.l")

(ros::load-ros-manifest "jsk_arc2017_baxter")

(unless (boundp '*baxter*) (jsk_arc2017_baxter::arc-init :ctype :rarm-controller))

(defun test ()
(let (prev-av next-av gripper-x end-coords (ik t) (avs nil) tms (count 0))
(setq prev-av (send *baxter* :angle-vector))
(send *ri* :move-hand :rarm
(send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 0) 1000)
;; :pick-object-with-movable-region
(setq next-av
;; :rotate-gripper :rarm 0
;; :slide-gripper :rarm 80
;; end-coords #f(600 -700 0)
(send *baxter* :rarm :angle-vector
#f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 0.0 0.0)))
(setq gripper-x (send *baxter* :rarm :gripper-x :joint-angle))
(send *baxter* :angle-vector prev-av)
;; First, move prismatic joint to target position
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper :rarm gripper-x :relative nil)
:fast (send *ri* :get-arm-controller :rarm) 0 :scale 5.0)
(send *ri* :wait-interpolation)
;; Fold fingers to avoid collision
(send *ri* :move-hand :rarm
(send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000)
(send *ri* :move-hand :rarm
(send *baxter* :hand-grasp-pose :rarm :cylindrical :angle 110) 1000)
;; Move whole arm to target pose
(send *baxter* :rarm :angle-vector next-av)
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000)

(send *ri* :wait-interpolation)
;; :try-to-suction-and-pinch-object
;;(send *ri* :move-hand :rarm (send *baxter* :hand-grasp-pose :rarm :opposed :angle 50) 1000)
(send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector #f(30 30)) 1000)

(send *ri* :start-grasp :rarm)

;;(break)

;; :rotate-gripper :rarm 0
;; :slide-gripper :rarm 80
;; end-coords #f(600 -700 -300)
(send *baxter* :rarm :angle-vector
#f(-4.48961 -32.8098 21.798 65.4342 -20.9014 61.0237 -26.78 0.0 0.0))
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 6000)
;; Wait until finger touch
(send *ri* :wait-interpolation-until :rarm
:grasp :finger-flexion :finger-loaded :prismatic-loaded)
(unix::sleep 1)
(send *baxter* :angle-vector
(send *ri* :state :potentio-vector :wait-until-update t))
;; opposedly grasp object
(unix::sleep 1)

(setq end-coords (send (send *baxter* :rarm :end-coords) :copy-worldcoords))
(setq ik (send *baxter* :angle-vector))
(while (and ik (< count 30))
(setq count (+ count 1))
(setq avs (cons ik avs))
(send *baxter* :rotate-gripper :rarm count :relative nil)
(setq ik (send *baxter* :rarm :inverse-kinematics end-coords)))
(setq avs (reverse avs))
(setq tms (make-list (- count 1) :initial-element (/ 5000.0 count)))
(send *ri* :angle-vector-sequence-raw avs tms)

(send *baxter* :rarm :angle-vector
#f(-4.5161 -49.3405 25.7123 47.8614 -16.4674 94.2234 -30.6975 0.0 0.0))
(send *ri* :angle-vector-raw (send *baxter* :angle-vector) 5000
(send *ri* :get-arm-controller :rarm :gripper nil) 0)
(send *ri* :wait-interpolation)
;; (unix::sleep 3)
;; (send *ri* :stop-grasp :rarm :suction)
;; (send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector #f(90 0)) 1000)))
138 changes: 105 additions & 33 deletions jsk_arc2017_baxter/euslisp/lib/arc-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -356,7 +356,8 @@
;; Setup arm for picking
(let (prev-av next-av gripper-x)
(setq prev-av (send *baxter* :angle-vector))
(if (eq grasp-style :suction)
(cond
((eq grasp-style :suction)
(setq next-av
(send *baxter* arm :inverse-kinematics
(make-coords :pos (v+ obj-pos #f(0 0 150))
Expand All @@ -368,35 +369,41 @@
#'(lambda ()
(and
(< (send *baxter* arm :gripper-x :joint-angle) 110)
(< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))))
(progn
(send *baxter* :rotate-gripper arm 0 :relative nil)
(send *baxter* :slide-gripper arm 60 :relative nil)
(setq next-av
(send *baxter* arm :inverse-kinematics
(make-coords :pos (v+ obj-pos #f(0 0 200))
:rpy (float-vector pinch-yaw 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
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper arm gripper-x :relative nil)
:fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
;; Fold fingers to avoid collision
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000
:wait nil)
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pose arm :cylindrical :angle 110) 1000
:wait nil)
(send *ri* :wait-interpolation)
;; Move whole arm to target pose
(send *ri* :angle-vector-raw
(send *baxter* :angle-vector next-av)
:fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
(send *ri* :wait-interpolation))
(< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))))
((eq grasp-style :pinch)
(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 pinch-yaw 0 0))
:move-palm-end t
:rotation-axis t)))
((eq grasp-style :suction-and-pinch)
(send *baxter* :rotate-gripper :rarm 0 :relative nil)
(send *baxter* :slide-gripper :rarm 80 :relative nil)
(setq next-av
(send *baxter* arm :inverse-kinematics
(make-coords :pos (v+ obj-pos #f(0 0 150))
:rpy (float-vector pinch-yaw 0 0))
: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
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper arm gripper-x :relative nil)
:fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
(send *ri* :wait-interpolation)
;; Fold fingers to avoid collision
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000)
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pose arm :cylindrical :angle 110) 1000)
;; Move whole arm to target pose
(send *ri* :angle-vector-raw
(send *baxter* :angle-vector next-av)
:fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
(send *ri* :wait-interpolation))
(dotimes (i n-trial)
(dotimes (j n-trial-same-pos)
(unless graspingp
Expand Down Expand Up @@ -442,9 +449,13 @@
(:try-to-pick-object
(arm obj-pos &key (offset #f(0 0 0)) (grasp-style :suction)
(pinch-yaw (if (eq grasp-style :pinch) 0)))
(if (eq grasp-style :suction)
(send self :try-to-suction-object arm obj-pos :offset offset)
(send self :try-to-pinch-object arm obj-pos pinch-yaw :offset offset)))
(cond
((eq grasp-style :suction)
(send self :try-to-suction-object arm obj-pos :offset offset))
((eq grasp-style :pinch)
(send self :try-to-pinch-object arm obj-pos pinch-yaw :offset offset))
((eq grasp-style :suction-and-pinch)
(send self :try-to-suction-and-pinch-object arm obj-pos pinch-yaw :offset offset))))
(:try-to-suction-object
(arm obj-pos &key (offset #f(0 0 0)))
(send self :try-to-suction-object-with-gripper-v6 arm obj-pos :offset offset))
Expand Down Expand Up @@ -600,6 +611,67 @@
(setq graspingp (send *ri* :graspingp arm :pinch))
(ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp)
graspingp))
(:try-to-suction-and-pinch-object
(arm obj-pos pinch-yaw &key (offset #f(0 0 0)))
(let (graspingp pre-end-coords)
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pose arm :cylindrical :angle 110) 1000)
(ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a pinch-yaw: ~a" arm pinch-yaw)
;; start the vacuum gripper before approaching to the object
(ros::ros-info "[:try-to-suction-and-pinch-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 (float-vector pinch-yaw 0 0))
:rotation-axis t
:use-gripper nil)
3000 (send *ri* :get-arm-controller arm) 0)
;; Wait until finger touch
(send *ri* :wait-interpolation-until arm
:grasp :finger-flexion :finger-loaded :prismatic-loaded)
(send *ri* :angle-vector-raw
(send *baxter* arm :inverse-kinematics
(make-coords :pos obj-pos
:rpy (float-vector pinch-yaw 0 0))
:rotation-axis t
:use-gripper nil)
3000 (send *ri* :get-arm-controller arm) 0)
(send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)
(send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
(setq pre-end-coords (send (send *baxter* :rarm :end-coords) :worldcoords))
;; cylindrically grasp object
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pose arm :cylindrical :angle 40) 1000)
(send *ri* :angle-vector-raw
(send *baxter* :slide-gripper arm 10 :relative nil)
3000 (send *ri* :get-arm-controller arm) 0)
(send *ri* :wait-interpolation)
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pose arm :cylindrical) 1000 :wait nil)
(send *ri* :wait-interpolation-until arm :finger-flextion :hand)
(unix::sleep 3)
(send *ri* :angle-vector-raw
(send *baxter* :rarm :inverse-kinematics
pre-end-coords :rotation-axis t :use-gripper nil)
3000)
(send *ri* :wait-interpolation-until arm :prismatic-load :finger-loaded)
(send *ri* :stop-grasp arm :suction)
(unix::sleep 1)
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pose arm :cylindrical) 1000)
(send *ri* :start-grasp arm :suction)
(setq graspingp (send *ri* :graspingp arm))
(ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a graspingp: ~a" arm graspingp)
;; lift object
(ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a lift the object" arm)
(send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 200) :world)
3000 (send *ri* :get-arm-controller arm :gripper nil) 0)
(send *ri* :wait-interpolation)
(unix::sleep 1) ;; wait for arm to follow
(setq graspingp (send *ri* :graspingp arm))
(ros::ros-info "[:try-to-suction-and-pinch-object] arm:~a graspingp: ~a" arm graspingp)
graspingp))
(:ik->bin-center
(arm bin &key (offset #f(0 0 0)) (rpy #f(0 0 0))
(rotation-axis t) (use-gripper nil) (move-palm-end nil))
Expand Down