Skip to content

Commit

Permalink
[wip] add suction-and-pinch grasp style
Browse files Browse the repository at this point in the history
  • Loading branch information
@708yamaguchi
708yamaguchi committed Jul 9, 2017
1 parent cde2cbb commit f33fe5f
Showing 1 changed file with 87 additions and 15 deletions.
102 changes: 87 additions & 15 deletions jsk_arc2017_baxter/euslisp/lib/arc-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,7 +285,8 @@
(send *ri* :wait-interpolation))
(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 @@ -297,16 +298,25 @@
#'(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 pinch-yaw 0 0))
:move-palm-end t
:rotation-axis t))))
(< -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
Expand Down Expand Up @@ -342,7 +352,7 @@
(send *ri* :move-hand arm
(send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
(send *ri* :gripper-servo-on arm)
graspingp))
graspingp)
(:get-object-position
(arm movable-region &key (object-index 0))
(let (obj-box obj-coords obj-pos obj-box-z-length)
Expand All @@ -369,9 +379,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)))
(if (eq arm :larm)
Expand Down Expand Up @@ -529,6 +543,64 @@
(setq graspingp (send *ri* :graspingp arm))
(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)))
;; hoge hoge
(let (graspingp pre-end-coords)
(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)
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)
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

0 comments on commit f33fe5f

Please sign in to comment.