diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 2a26ddcc8..3ea1bee84 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -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)) @@ -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 @@ -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) @@ -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) @@ -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))